ロボットのメカ部、ハードウェア部の製作が完了しましたので、次にソフトウェア開発に進みます。
ソフトウェア開発の概要
お掃除ロボットには、Arduino UNOとESP-Wroom-02の二つのマイコンを搭載していますので、ソフトウェアも、それぞれに必要になります。
ただし、ESP-Wroom-02側のプログラムは、お掃除ロボットを作ってみた（その１）･･･ロボットの構造で既に紹介済みですので、ここでは、Arduino UNOのソフト開発について説明します。
Arduino UNOのソフトウェア開発環境（IDE）には、Arduino IDE Ver.1.6.12を使用しています。バージョンが古いのは、ESP-Wroom-02のソフト開発を同じ環境で行うためで、当時の最新バージョン（Arduino1.8.2）を使用すると問題が発生しました。
この旧バージョンのIDEは、下記のサイトからダウンロード出来ます。
Arduino 1.6.x, 1.5.x BETA
Arduino UNO側ソフト開発には、駆動系、センサー系、制御系、マンマシン系の各プログラムに分かれており、それぞれ下記に示します。開発手順としては、各機能単位で単体プログラムを作成、テストを行った後、全体を結合し、総合的にテストすることになります。
プログラムの説明
駆動系ソフト（Arduino UNO）
１．関数名：void OutputToDriver(int PWMCntl_R, int PWMCntl_L, byte DrvCntl)
ロボットの各車輪のモータードライバーへ出力を行います。パラメターは、下記です。
- int PWMCntl_R：右車輪モーターへ出力するPWM値（0-255の値でPWM制御を行う）
- int PWMCntl_L：左車輪モーターへ出力するPWM値（0-255の値でPWM制御を行う）
- byte DrvCntl：左右車輪の回転方向（前進：0x05、後進：0x0A、右旋回：0x06.左旋回：0x09）
２．関数名：void OutputToBrushMotorDriver(bool onoff)
回転ブラシの運転/停止を行います。
パラメーターは、運転：１、停止：０です。回転速度は、PWM値で与えますが、PWM_Brushでプログラム内定義された固定値としています。
３．関数名：void OutputToFanRelay(bool OnOffCntl)
サイクロン吸引機ターボファンの運転/停止を行います。
パラメーターは、運転：１、停止：０です。
センサー系ソフト（Arduino UNO）
１．関数名：int Check_LS()
リミットスイッチおよび反射型フォトインタラプタの状態をスキャンします。
リミットスイッチはメカニカルコンタクトなのでチャタリング対策をしています。結果は、intの下位3ビットにセットして返します。（リミットスイッチが働いた時、または床を検出出来なかった時にセットされます）
２．関数名：void distance_from_wall()
左右に取付けた超音波測距センサーで測定した壁からの距離をそれぞれグローバル変数distance_L、distance_Rにmm単位でセットします。
測定は移動平均を使い、４個のデータの平均を取ります。
制御系ソフト（Arduino UNO）
１．関数名：void operation(int ope_mod)
制御には、手動操作と自動運転があります。
パラメーター”ope_mod”は、各運転時の内容を表わしますが、第4ビットが”0″の時「手動操作」、”1″の時「自動運転」に分けています。
２．関数名：void manual_operation(int ope_mod)
パラメーターとして、手動操作の内容（運転モード）を渡します。
３．関数名：void auto_operation(int ope_mod)
パラメーターとして、自動運転の内容（運転モード）を渡します。
ただし、今回は、手動のみの公開とし、自動は次回とします。
マン・マシン系ソフト（Arduino UNO）
１．関数名：int input_command()
パソコンまたは、ESP-Wroom-02からロボットへの運転モードを入力します。
Arduino UNOにUSBケーブルで接続した場合はパソコンからの入力を、また、バッテリー駆動でESP-Wroom-02とシリアル接続した場合はWEBからの入力となります。
２．関数名：void LCD_Disp()
運転状況を16桁×2行LCDディスプレイに表示します。
プログラムリスト
下記にArduino UNOのプログラムリストを示します。
/*
V0.00: robot制御プログラムを作成。ステータスをLCDに表示
(詳細、掃除ロボットプログラム設計.doc)
*/
include <Wire.h>
include <LiquidCrystal_I2C.h>
include <MsTimer2.h>
define LCD1602_ADDRESS 0x27
define Forward 0x05 // B0101
define Backward 0x0a // B1010
define RightRotate 0x06 // B0110
define LeftRotate 0x09 // B1001
define RightWheel_IN1 8
define RightWheel_IN2 7
define LeftWheel_IN1 13
define LeftWheel_IN2 12
define RightWheel_ENA 9
define LeftWheel_ENA 10
define BrushMotor 6
define VacumFan 5
define RightFront_LS A0
define LeftFront_LS A1
define Floor_Detect A2
/*
define PWMDuty0 0
define PWMDuty20 51
define PWMDuty40 102
define PWMDuty60 153
define PWMDuty80 204
define PWMDuty100 255
*/
define Speed_Stop 0
define Speed_Low 102
// #define Speed_Mid 127
define Speed_High 153
// #define Speed_Full 204
define PWM_Brush 51
define TrigPin_R 11
define EchoPin_R 4
define TrigPin_L 3
define EchoPin_L 2
define PIDINTVL 2
static int LimitSW_STA = 0;
static int PWM_R =0;
static int PWM_L =0;
static byte cntl_side = 'L';
static int auto_sta = 0; // 内部変数autostaで各モードの自動シーケンスを制御
static boolean PID_EN = false;
static int count_sec =5;
static int every_sec =3;
static int travel_sec =3;
static int PID_intvl = PIDINTVL;
static int MV = 0;
static int distance_R = 0;
static int distance_L = 0;
static char lcd_up[17];
static char lcd_lo[17];
// for 1602
LiquidCrystal_I2C lcd(LCD1602_ADDRESS,16,2); //I2Cアドレス入力 １回目購入分は、0x3f, ２回目は、0ｘ27で異なる点注意。
void LCD_Disp();
void tim_int_100ms();
void DebugPrint(char *pnt, String str){
if( false // プリントをする場合、コメントアウト
// || ( pnt== "")
// || ( pnt== "")
// || ( pnt== "MANU" )
// || ( pnt== "LimitSW" )
// || ( pnt== "UltraSonic" )
|| (pnt== "Debug" )
)
Serial.println( "[" + String(pnt) + "] " + str );
}
void setup()
{
Serial.begin(9600);
delay(10);
pinMode(RightWheel_IN1, OUTPUT);
pinMode(RightWheel_IN2, OUTPUT);
pinMode(LeftWheel_IN1, OUTPUT);
pinMode(LeftWheel_IN2, OUTPUT);
// pinMode(RightWheel_ENA 9, OUTPUT);
// pinMode(LeftWheel_ENA 10, OUTPUT);
pinMode(RightFront_LS, INPUT_PULLUP);
pinMode(LeftFront_LS, INPUT_PULLUP);
pinMode(Floor_Detect, INPUT_PULLUP);
pinMode(TrigPin_R, OUTPUT);
pinMode(EchoPin_R, INPUT);
pinMode(TrigPin_L, OUTPUT);
pinMode(EchoPin_L, INPUT);
pinMode(VacumFan, OUTPUT);
pinMode(BrushMotor, OUTPUT);
// attachInterrupt(digitalPinToInterrupt(RightFront_LS), Check_RFLS, CHANGE); 割込みは2,3ピンのみ使用可、今回は使はない。
lcd.init();
lcd.backlight();
strcpy(lcd_up, "Start …");
strcpy(lcd_lo, "Waiting Start ..");
LCD_Disp();
delay(100);
//100ms毎の割込み
MsTimer2::set(100, tim_int_100ms);
MsTimer2::start();
}
int input_command(){
// テストでは、キーボードより入力。最終的には、ESP-Wroom-02からOperationModeへ代入する。
static String inputString = ""; // a String to hold incoming dat
static boolean stringComplete = false; // whether the string is complete
int inputCom;
while (Serial.available()) {
// get the new byte:
char inChar = (char)Serial.read();
// add it to the inputString: inputString += inChar; // if the incoming character is a newline, set a flag so the main loop can if (inChar == '\n') { stringComplete = true; }
}
if(stringComplete){
inputCom = inputString.toInt(); // Get Operation Mod
inputString = "";
stringComplete = false;
return inputCom;
}
}
int Check_LS(){
int LSW_STA = digitalRead(RightFront_LS) | digitalRead(LeftFront_LS)<<1;
while(LSW_STA != ( digitalRead(RightFront_LS) | digitalRead(LeftFront_LS)<<1) ){
delay( 10); // チャタリング対策
LSW_STA = digitalRead(RightFront_LS) | digitalRead(LeftFront_LS)<<1 ;
}
LSW_STA |= digitalRead(Floor_Detect)<<2;
return LSW_STA ^ 0x3;
}
void LCD_Disp(){ // for 1602
// lcd_upの内容を上段に、lcd_loの内容を下段に表示する。
lcd.clear();
lcd.print(lcd_up);
lcd.setCursor(0, 1);
lcd.print(lcd_lo);
Serial.println("[LCD UP]:" + String(lcd_up) + " [LO]:" + String(lcd_lo));
}
void OutputToDriver(int PWMCntl_R, int PWMCntl_L, byte DrvCntl){
PWM_R = PWMCntl_R;
PWM_L = PWMCntl_L;
digitalWrite(RightWheel_IN1, 0x01 & DrvCntl);
digitalWrite(RightWheel_IN2, 0x01 & DrvCntl>>1);
digitalWrite(LeftWheel_IN1, 0x01 & DrvCntl>>2);
digitalWrite(LeftWheel_IN2, 0x01 & DrvCntl>>3);
analogWrite(RightWheel_ENA, PWMCntl_R);
analogWrite(LeftWheel_ENA, PWMCntl_L);
}
void OutputToBrushMotorDriver(bool onoff){
if( onoff == HIGH){
analogWrite(BrushMotor, PWM_Brush);
}else{
analogWrite(BrushMotor, 0);
}
}
void OutputToFanRelay(bool OnOffCntl){
digitalWrite(VacumFan, OnOffCntl);
}
void operation(int ope_mod){
DebugPrint("Debug", "operation()…ope_mod = " + String(ope_mod));
switch ( ope_mod & 0x20) {
case 0: // manual manual_operation(ope_mod & 0x1f); sprintf(&lcd_up[0],"M]: %d",ope_mod); break; case 0x20: // auto auto_sta = 0; auto_operation(ope_mod & 0xdf); break; default: break;
}
LCD_Disp();
}
void auto_operation(int ope_mod){
static int prev_dist=0;
int distance;
sprintf(&lcd_up[0],"Auto: %d-%d",ope_mod, auto_sta);
switch(ope_mod & 0xdf){
case 0: // MODE=32 自動モードでの停止
OutputToDriver(Speed_Stop, Speed_Stop, 0);
OutputToBrushMotorDriver(LOW);
OutputToFanRelay(LOW);
break;
case 1: // MODE=33 掃除１･･･壁沿いに走行し、進行方向に障害物を検出するとUターンして戻る。走行は、PID制御を適用。 switch(auto_sta){ case 0: // Start // ブラシ起動 OutputToBrushMotorDriver(HIGH); // バキューム起動 OutputToFanRelay(HIGH); // 壁からの距離を計測するため、旋回を開始 OutputToDriver(Speed_Low, Speed_Low, RightRotate); prev_dist = measure_distance('L'); sprintf(&lcd_lo[0],"[STA]: FW Lo"); auto_sta++; travel_sec = 1; break; case 1: distance = measure_distance('L'); if( (prev_dist !=0) && (distance != 0) ){ if(prev_dist <= distance){ PID_EN = true; auto_sta++; } } prev_dist = distance; travel_sec = 1; break; case 2: if(LimitSW_STA){ // リミットスイッチによる障害物検出時 PID_EN = false; OutputToDriver(Speed_Stop, Speed_Stop, 0); auto_sta++; } travel_sec = 5; break; case 3: // 少しバックする if( travel_sec <= 0 ){ OutputToDriver(Speed_Low, Speed_Low, Backward); auto_sta++; travel_sec = 10; } break; case 4: // 走行動作を帰る場合、0.5秒停止を入れる if( travel_sec <= 0 ){ OutputToDriver(Speed_Stop, Speed_Stop, 0); auto_sta++; travel_sec = 5; } break; case 5: // バック後、スイッチターン（1秒） if( travel_sec <= 0 ){ OutputToBrushMotorDriver(HIGH); OutputToDriver(Speed_Stop, Speed_High, Backward); auto_sta++; travel_sec = 10; } break; case 6: // 走行動作を帰る場合、0.5秒停止を入れる if( travel_sec <= 0 ){ OutputToDriver(Speed_Stop, Speed_Stop, 0); auto_sta++; travel_sec = 5; } break; case 7: // スイッチターン後、直進走行 if( travel_sec <= 0 ){ OutputToBrushMotorDriver(HIGH); OutputToDriver(Speed_Low, Speed_Low, Forward); PID_EN = true; auto_sta = 1; travel_sec = 30; } break; default: break; } break; case 2: // MODE=34 往復走行。一定時間直進、または障害物検出後、Uターンして走行を続ける。 // 1:直進、2:バック、3:Uターン、 switch(auto_sta){ case 0: // Start // ブラシ起動 OutputToBrushMotorDriver(HIGH); // バキューム起動 OutputToFanRelay(HIGH); // 直進走行 OutputToDriver(Speed_Low, Speed_Low, Forward); sprintf(&lcd_lo[0],"[STA]: FW Lo"); auto_sta = 1; travel_sec = 30; break; case 1: // 一定時間後、または、リミットスイッチ作動で一時停止 if( travel_sec <= 0 ){ OutputToDriver(Speed_Stop, Speed_Stop, 0); auto_sta++; travel_sec = 5; } if(LimitSW_STA){ // リミットスイッチによる障害物検出時 OutputToDriver(Speed_Stop, Speed_Stop, 0); auto_sta++; travel_sec = 5; } break; case 2: // 少しバックする if( travel_sec <= 0 ){ OutputToDriver(Speed_Low, Speed_Low, Backward); auto_sta++; travel_sec = 10; } break; case 3: // 走行動作を帰る場合、0.5秒停止を入れる if( travel_sec <= 0 ){ OutputToDriver(Speed_Stop, Speed_Stop, 0); auto_sta++; travel_sec = 5; } break; case 4: // バック後、スイッチターン（1秒） if( travel_sec <= 0 ){ OutputToBrushMotorDriver(HIGH); OutputToDriver(Speed_Stop, Speed_High, Backward); auto_sta++; travel_sec = 10; } break; case 5: // 走行動作を帰る場合、0.5秒停止を入れる if( travel_sec <= 0 ){ OutputToDriver(Speed_Stop, Speed_Stop, 0); auto_sta++; travel_sec = 5; } break; case 6: // スイッチターン後、直進走行 if( travel_sec <= 0 ){ OutputToBrushMotorDriver(HIGH); OutputToDriver(Speed_Low, Speed_Low, Forward); auto_sta = 1; travel_sec = 30; } break; default: break; } break; case 3: // MODE=35 ランダム走行。障害物を検出後、一定角度（例えば60度）回転して、走行を続ける。 break; case 4: // MODE=36 部屋の壁づたいに四角を描きながら走行し、元の位置に戻るとひとつ内側の軌道に移動して、内部へ走行を続ける。 break; case 18: // MODE=50 センサー入力表示 Serial.println("Distance[R] = " + String(distance_R) + ", Distance[L] = " + String(distance_L)); Serial.println("LimitSW [R] = " + String( LimitSW_STA&01?"ON":"OFF") + ", LimitSW [L] = " + String( LimitSW_STA&02?"ON":"OFF") + ", PhotoSW [U] = " + String( LimitSW_STA&04?"ON":"OFF")); travel_sec = 10; break; default: break;
}
}
void manual_operation(int ope_mod){
switch(ope_mod){
case 0: // 手動停止
OutputToDriver(Speed_Stop, Speed_Stop, 0);
OutputToBrushMotorDriver(LOW);
OutputToFanRelay(LOW);
break; case 1: //前進１（低速） OutputToDriver(Speed_Low, Speed_Low, Forward); break; case 2: //前進２（高速） OutputToDriver(Speed_High, Speed_High, Forward); break; case 3: //後進１（低速） OutputToDriver(Speed_Low, Speed_Low, Backward); break; case 4: //後進２（高速） OutputToDriver(Speed_High, Speed_High, Backward); break; case 5: //前進右折１（右低速、左高速） OutputToDriver(Speed_Low, Speed_High, Forward); break; case 6: //前進右折２（右停止、左高速） OutputToDriver(Speed_Stop, Speed_High, Forward); break; case 7: //前進右折３（右停止、左低速） OutputToDriver(Speed_Stop, Speed_Low, Forward); break; case 8: //前進左折１（右高速、左低速） OutputToDriver(Speed_High, Speed_Low, Forward); break; case 9: //前進左折２（右高速、左停止） OutputToDriver(Speed_High, Speed_Stop, Forward); break; case 10: //前進左折３（右低速、左停止） OutputToDriver(Speed_Low, Speed_Stop, Forward); break; case 11: //後進右折１（右低速、左高速） OutputToDriver(Speed_Low, Speed_High, Backward); break; case 12: //後進右折２（右停止、左高速） OutputToDriver(Speed_Stop, Speed_High, Backward); break; case 13: //後進右折３（右停止、左低速） OutputToDriver(Speed_Stop, Speed_Low, Backward); break; case 14: //後進左折１（右高速、左低速） OutputToDriver(Speed_High, Speed_Low, Backward); break; case 15: //後進左折２（右高速、左停止） OutputToDriver(Speed_High, Speed_Stop, Backward); break; case 16: //後進左折３（右低速、左停止） OutputToDriver(Speed_Low, Speed_Stop, Backward); break; case 17: //右旋回１：（右高速後転、左高速前転） OutputToDriver(Speed_High, Speed_High, RightRotate); break; case 18: //右旋回２：（右低速後転、左低速前転） OutputToDriver(Speed_Low, Speed_Low, RightRotate); break; case 19: //左旋回１：（右高速前転、左高速後転） OutputToDriver(Speed_High, Speed_High, LeftRotate); break; case 20: //左旋回２：（右低速前転、左低速後転） OutputToDriver(Speed_Low, Speed_Low, LeftRotate); break; case 25: // ブラシ起動 OutputToBrushMotorDriver(HIGH); break; case 26: // バキューム起動 OutputToFanRelay(HIGH); break; default: // break;
}
}
void PID_calc(){
// PID Control parameter
static int Kp = 20;
static int Ti = 20;
static int Td = 20;
static int SV = 20;
static int PV = 20;
static int e0 = 0;
static int Sigm_e = 0;
if(cntl_side='L'){
PV = distance_L;
}else{
PV = distance_R;
}
if(PV==0){
return; // timeout
}
int e = PV - SV;
Sigm_e += e; // need to have some limitation
int p_comp = Kp * e;
int i_comp = Kp * (Sigm_e / Ti);
int d_comp = Kp * Td * (e-e0) / PID_intvl;
e0 = e;
MV = p_comp + i_comp + d_comp;
}
void PID_Output(){
int accel = PWM_R + MV;
int decel = PWM_R - MV;
if(accel>= 255) accel = 255;
else if(accel <= 0) accel =0;
if(decel>= 255) decel = 255;
else if(decel <= 0) decel =0;
if(cntl_side='L'){
analogWrite(RightWheel_ENA, accel);
analogWrite( LeftWheel_ENA, decel);
}else{
analogWrite(RightWheel_ENA, decel);
analogWrite( LeftWheel_ENA, accel);
}
}
void distance_from_wall(){ // UltraSonic Distance Measurement
define N_MvngAve 4
static int dist_R[9];
static int dist_L[9];
static unsigned int pos_i = 0;
pos_i = pos_i % N_MvngAve;
dist_R[pos_i] = measure_distance('R');
dist_L[pos_i] = measure_distance('L');
pos_i++;
int i=0;
int sumR = 0;
int sumL = 0;
while(i<N_MvngAve){
sumR += dist_R[i];
sumL += dist_L[i];
i++;
}
distance_R = sumR / N_MvngAve;
distance_L = sumL / N_MvngAve;
DebugPrint("UltraSonic", "Distance_R = " + String(distance_R) + ", Distance_L = " + String(distance_L));
}
int measure_distance(byte side){ // 距離を㎜単位で返す
long duration;
if(side == 'R'){
digitalWrite(TrigPin_R, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(TrigPin_R, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(TrigPin_R, LOW);
duration = pulseIn(EchoPin_R, HIGH, 10000L); // timeout when return is 0
DebugPrint("UltraSonic", "side(R) = " + String(side, HEX) + ", Distance_R = " + String((duration/2) / 2.91));
}else if(side == 'L'){
digitalWrite(TrigPin_L, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(TrigPin_L, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(TrigPin_L, LOW);
duration = pulseIn(EchoPin_L, HIGH, 10000L); // timeout when return is 0
DebugPrint("UltraSonic", "side(L) = " + String(side, HEX) + ", Distance_L = " + String((duration/2) / 2.91));
}
return (int)(duration/2) / 2.91;
}
void tim_int_100ms() { // １00ミリ秒ごとのタイマー割込み
static int count_100ms = 10;
--travel_sec;
if( PID_EN & (--PID_intvl <= 0 )) { // 数秒周期での処理
PID_calc();
PID_intvl = PIDINTVL;
}
if(--count_100ms <= 0){
count_100ms = 10;
count_sec--;
every_sec--;
}
}
void loop() {
static int OperationMode = 0;
static int Prev_OpeMod=0;
static int Prev_LSW_STA=0;
LimitSW_STA = Check_LS();
if(LimitSW_STA != Prev_LSW_STA){
Prev_LSW_STA = LimitSW_STA;
}
DebugPrint("LimitSW", "LimitSW_STA = " + String(LimitSW_STA, HEX));
if(LimitSW_STA){ // リミットswが働いた時、取敢えず止める。以降は、手動、自動、それぞれで、新たな動作が設定されるまで入力待ち。
analogWrite(RightWheel_ENA, Speed_Stop);
analogWrite( LeftWheel_ENA, Speed_Stop);
}else if(PID_EN){
PID_Output();
}
if( (OperationMode & 0x20 ) && (travel_sec <= 0) ) { // 自動モードでの処理
auto_operation(OperationMode & 0xdf);
}
distance_from_wall();
OperationMode = input_command(); // コマンド入力チェック
if(OperationMode != Prev_OpeMod){
Prev_OpeMod = OperationMode;
operation(OperationMode); // 各運転モードのスタート
}
if( count_sec <= 0 ) { // 数秒周期での処理
Serial.println("Start …");
count_sec = 1;
}
if( every_sec <= 0 ){ // 毎秒周期での処理
// digitalWrite(LeftWheel_IN1, !digitalRead(LeftWheel_IN1));
every_sec = 1;
}
}
