PICプログラミングでセンサーとモータードライバを使いこなす
公開日: 2021年02月21日最終更新日: 2022年01月28日
電子工作で大体何でも作れるようになった話を制御するソフトウェアについての記事です。
ハードウェアが固まってきたことでようやく制御ソフトウェアの開発ができます。
制御ソフトウェアの開発
PIC12F1822のプログラムメモリは2KB、SRAMは128バイトです。動作周波数は最大32MHzですが、今回は8MHzで動かすことにします。この限られたリソースの中にプログラムを詰め込まなければならないと考えると、ワクワクしてきますね。
今回作成したプログラムでは、加速度センサと距離センサから読み取った計測値をもとにマシンを制御する処理を1秒間に50回実行しています。 PICのプログラムでは無限ループの中に処理を記述していきます。ループの処理はタイマー割り込みによって制御され20msに一度実行されます。1周の処理にかかる時間は20ms弱で、センサからの値を取得する時間が大きな割合を占めます。
距離センサの範囲に入るまでは、加速度センサの値から進んだ距離と速度を計算してマシンをコントロールしています。最初は加速して2m/sくらいまで速度を上げて距離を稼ぎ、測距センサの有効距離である壁の50cm手前までに40cm/s以下に減速させておけば、距離センサの計測距離に入ってからのブレーキが間に合うという考えです。
問題は加速度センサの誤差がどれくらい出るかです。速度と距離は加速度センサの値を積分して求めるため、センサの誤差が増幅されて蓄積していきます。 そこで、加速度センサだけを使って指定の距離で停止させるテストをしてみたところ、加速度センサの値から計算した距離と実際の距離のズレはプラスマイナス10%程度でした。もっと誤差が大きくなるんじゃないかと思ってましたがこれならなんとか使えそうです。50cm＋誤差を含めた距離で減速位置を調整します。
一定速度で距離センサの計測範囲内に入れるようになったら、距離センサで速度と距離を計測して停止位置の調整をします。 距離センサのデータシートを見ると計測値が安定するまでに40ms必要なようです。常時計測しながら走っても遅延が出そうですが、処理のループが1周20msですしあまり気にしないことにします。 距離はアナログ電圧で取得しており、ADCの分解能では0.1mmくらいが限界です。また、測距センサの赤外線を反射する対象の素材や環境によって計測値がぶれます。0.5mmくらいは誤差がありそうです。 ここでは、距離センサの計測範囲内に入ってから速度を10cm/s以下まで落とし、壁の直前で完全停止するようにしました。しかし、全体の処理が50Hzなのでそのまま計測値を使うと速度の誤差が25cm/sになるため、速度を10cm/sに調整しようとしてもマイナス2.5cm/s～22.5cm/sになってしまいます。なので、誤差の影響を減らすためにcurrent = current 0.2 + last 0.8として過去の値から少しずつ変化させるようにしてみました。
そして、実験で速度やブレーキに適した位置を調べておきます。速度についてはPWMと比例するので、事前に確認しておけばだいたいいい感じになります。今回はフローリングで確認しましたが路面によっても状況が変わります。レース当日はタイルカーペットでした……。
回路図
HWの記事でも触れましたが、マシンの回路図は以下のようになっています。
加速度センサーと測距センサーの測定値をPICに取り込み、PWMでモータードライバを制御する構成になっています。
電源はPIC用とモーター用の2系統のリチウムイオンバッテリーを持っています。PIC用の電源には定電圧回路が組み込まれており、安定して3.3Vを供給します。
LCDはデバッグ用で、組み込み時には使用しません。I2Cは簡単に切り離せるのが良いですね。
マシンのプログラム
突貫で作ったコードで見せられたものじゃないのですが、恥を忍んで載せておきます。 本記事を書く上で室内で動作確認していたため、壁までの距離を3.5mに合わせてあります。
#include "mcc_generated_files/mcc.h"
#include <stdint.h>
/*
Main application
*/
//#define LCD_ADDR 0x3E
#define MMA8452_ADRS 0x1D
//#define MMA8452_STATUS 0x00
#define MMA8452_OUT_Y_MSB 0x03
#define MMA8452_XYZ_DATA_CFG 0x0E
#define MMA8452_CTRL_REG1 0x2A
#define MMA8452_CTRL_REG1_ACTV_BIT 0x01
//#define MMA8452_WHO_AM_I 0x0D
#define MMA8452_CTRL_REG1_CONFIG 0x00
#define MMA8452_G_SCALE 2
#define SLAVE_I2C_GENERIC_RETRY_MAX 10
uint8_t g_buf[2] = {0, 0};
bool g_flag_exec = false;
void writeI2C(uint8_t addr, uint8_t* writeData, uint8_t len) {
I2C_MESSAGE_STATUS status;
I2C_MasterWrite(writeData, len, addr, &status);
while (status == I2C_MESSAGE_PENDING){
__delay_us(100);
}
}
void readI2C(uint8_t addr, uint8_t reg, uint8_t* readData, uint8_t len) {
I2C_MESSAGE_STATUS status;
I2C_TRANSACTION_REQUEST_BLOCK readTRB[2];
uint16_t timeOut;
// this initial value is important
status = I2C_MESSAGE_PENDING;
// we need to create the TRBs for a random read sequence to the EEPROM
// Build TRB for sending address
I2C_MasterWriteTRBBuild( &readTRB[0],
®,
1,
addr);
// Build TRB for receiving data
I2C_MasterReadTRBBuild( &readTRB[1],
readData,
len,
addr);
timeOut = 0;
while(status != I2C_MESSAGE_FAIL)
{
// now send the transactions
I2C_MasterTRBInsert(2, readTRB, &status);
// wait for the message to be sent or status has changed.
while(status == I2C_MESSAGE_PENDING){
__delay_us(100);
}
if (status == I2C_MESSAGE_COMPLETE)
break;
// if status is I2C_MESSAGE_ADDRESS_NO_ACK,
// or I2C_DATA_NO_ACK,
// The device may be busy and needs more time for the last
// write so we can retry writing the data, this is why we
// use a while loop here
// check for max retry and skip this byte
if (timeOut == SLAVE_I2C_GENERIC_RETRY_MAX)
break;
else
timeOut++;
}
}
void initMMA8452Q() {
g_buf[1] = 0x00;//g_buf[0] & ~(MMA8452_CTRL_REG1_ACTV_BIT);
g_buf[0] = MMA8452_CTRL_REG1;
writeI2C(MMA8452_ADRS, g_buf, 2);
g_buf[0] = MMA8452_XYZ_DATA_CFG;
g_buf[1] = (MMA8452_G_SCALE >> 2);
writeI2C(MMA8452_ADRS, g_buf, 2);
readI2C(MMA8452_ADRS, MMA8452_CTRL_REG1, &g_buf, 1);
g_buf[1] = MMA8452_CTRL_REG1_CONFIG | MMA8452_CTRL_REG1_ACTV_BIT;
g_buf[0] = MMA8452_CTRL_REG1;
writeI2C(MMA8452_ADRS, g_buf, 2);
}
int getRange() {
uint16_t result = 0;
result = ADC_GetConversion(channel_AN0);
return result;
}
int getAccel() {
int16_t result = 0;
readI2C(MMA8452_ADRS, MMA8452_OUT_Y_MSB, &g_buf, 2);
result = (g_buf[0] << 4) + (g_buf[1] >> 4);
if(result >= 0x0800) {
result -= 4096;
}
return result;
}
void timer1Callback(void) {
g_flag_exec = true;
}
/*
Main application
*/
void main(void)
{
// initialize the device
SYSTEM_Initialize();
TMR1_SetInterruptHandler(timer1Callback);
// When using interrupts, you need to set the Global and Peripheral Interrupt Enable bits
// Use the following macros to:
// Enable the Global Interrupts
INTERRUPT_GlobalInterruptEnable();
// Enable the Peripheral Interrupts
INTERRUPT_PeripheralInterruptEnable();
// Disable the Global Interrupts
//INTERRUPT_GlobalInterruptDisable();
// Disable the Peripheral Interrupts
//INTERRUPT_PeripheralInterruptDisable();
uint16_t range = 0;
uint16_t last_range = 0;
int32_t accel_offset = 0;
int32_t accel = 0;
uint16_t duty = 0;
uint8_t brake_phase = 0;
int32_t velocity = 0;
int32_t last_velocity = 0;
bool g_flag_finish = false;
uint16_t phase = 0;
RA4 = 0;
initMMA8452Q();
__delay_ms(800);
for(int i=0; i < 16; i++) {
accel_offset += getAccel();
__delay_us(1250);
}
accel_offset >>= 4;
int32_t trip = 0;
while (1)
{
duty = 0;
if(g_flag_exec && !g_flag_finish) {
if(phase == 0) {
range = getRange();
accel = 0;
// 加速度は加速方向がマイナス、減速方向がプラス
for(int i = 0; i < 4; i++){
accel += (getAccel() - accel_offset); // milli G, 1G = 9800mm/s^2
__delay_us(1250);
}
accel >>= 2;
velocity += (accel * -191); // (-accel * 9.8m/s^2 / 1024) * 20ms unit is um/s
if(velocity < 0) {
velocity = 0;
}
trip += ((velocity + last_velocity) / 100); // (current + last) / 2 um/s * 20ms / 1000 sec unit is um (10m = 10,000,000 um))
if(trip <= 2000000) {
duty = 200;
} else if(2500000 < trip) {
duty = 40;
}
if(250 < range && range < 600) {
phase = 1;
}
} else {
range = 0;
for(int i = 0; i < 8; i++){
range += getRange();
__delay_us(2000);
}
range = range >> 3;
range = (range * 2 + last_range * 8) / 10;
velocity = (range - last_range) * 50; // mm/s 20ms周期のループなので50mm/s単位でしかわからない。
if(range < 250) { // 50cm
RA4 = 0;
duty = 30;
if (brake_phase == 1) {
brake_phase = 2;
RA4 = 0;
duty = 50;
}
}else {
RA4 = 0;
duty = 20;
if(brake_phase < 2 && velocity > 50) {
RA4 = 1;
duty = 200;
brake_phase = 1;
}
if(range > 450) { // 30cm
RA4 = 1;
duty = 100;
g_flag_finish = true;
}
}
}
EPWM_LoadDutyValue(duty);
last_velocity = velocity;
last_range = range;
g_flag_exec = false;
} else if(g_flag_finish) {
__delay_ms(100);
RA4 = 1;
duty = 0;
EPWM_LoadDutyValue(duty);
}
}
}これをビルドすると、PICの記憶領域はもういっぱいいっぱいです。
Memory Summary:
Program space used 7E4h ( 2020) of 800h words ( 98.6%)
Data space used 77h ( 119) of 80h bytes ( 93.0%)
EEPROM space used 0h ( 0) of 100h bytes ( 0.0%)
Data stack space used 0h ( 0) of 7h bytes ( 0.0%)
Configuration bits used 2h ( 2) of 2h words (100.0%)
ID Location space used 0h ( 0) of 4h bytes ( 0.0%)感じたこと
今回、ハードウェアの製作に時間が取られたためソフトウェアに時間を割くことができませんでした。美しいとはお世辞にも言い難い、いきあたりばったりの制御になっています。本当はフィードバック制御についてちゃんと勉強して、マシンの数理モデルを作ってみたかったです。
また、加速度センサの誤差が許容範囲だったとはいえ、加速度センサから求められる速度や距離は誤差が大きいため、タイヤの回転数を取得するロータリーエンコーダなどを併用すればよかったなと思いました。
そして、カルマンフィルタを使って相互に補完すれば精度がもっと上がるはずです。