mbed_robotcar
linetrace saikyo!
Revision 2:cc4e4527ea43, committed 2020-09-24
- Comitter:
- takuminomura
- Date:
- Thu Sep 24 06:58:39 2020 +0000
- Parent:
- 1:fd7dd12feee0
- Commit message:
- miyasui;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Sep 17 08:04:18 2020 +0000
+++ b/main.cpp Thu Sep 24 06:58:39 2020 +0000
@@ -9,48 +9,27 @@
#include <stdlib.h>
/* マクロ定義、列挙型定義 */
-#define NORMAL 0 // 普通
-#define FAST 1 // 速い
-#define VERYFAST 2 // とても速い
#define LOW 0
// 使うロボットカーに合わせる
#define MBED01 1
-#define MBED02 0.719 //Mbed02号機の左右のモーター速度比(R:L = 1: 0.719)
-#define MBED03 1
-#define MBED04 0.781 //Mbed04号機の左右のモーター速度比(R:L = 1: 0.781)
-#define MBED05 0.95 //Mbed05号機の左右のモーター速度比(R:L = 1: 0.953)
-#define MBED06 1
-#define MBED07 1 //Mbed07号機の左右のモーター速度比(R:L = 1: 1.000)
-#define MBED08 1
-#define MBED09 0.9
+#define MBED09 0.99
#define MBED MBED09 //使うロボットカーの番号にする
-// MSR = Motor Speed Right
-// MSL = Motor Speed Left
-//// 4輪
-//#define MSR0 1.0 // 基準速度(右モータ)
-//#define MSR1 1.0 // 低速旋回(右モータ)
-//#define MSR2 1.0 // 中速旋回(右モータ)
-//#define MSR3 1.0
-//#define MSL0 MSR0*MBED // 基準速度(左モータ)
-//#define MSL1 MSR1*MBED // 低速旋回(左モータ)
-//#define MSL2 MSR2*MBED // 中速旋回(左モータ)
-//#define MSL3 MSR3*MBED
+
+// 0: 前進 1: 旋回 2: 速度差 3: 障害物回避後戻る
+float motorSpeedR1[9] = {0.5 , 0.5 , 0.5 , 0.5 };
+float motorSpeedR2[9] = {0.5 , 0.5 , 0.5 , 0.5 };
+float motorSpeedL1[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
+float motorSpeedL2[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
-// 2輪
-#define MSR0 0.7 // 基準速度(右モータ)
-#define MSR1 0.63 // 低速旋回(右モータ)
-#define MSR2 0.63 // 中速旋回(右モータ)
-#define MSR3 0.76 // 前進左右差(右モータ)
-#define MSL0 MSR0*MBED // 基準速度(左モータ)
-#define MSL1 MSR1*MBED // 低速旋回(左モータ)
-#define MSL2 MSR2*MBED // 中速旋回(左モータ)
-#define MSL3 MSR3*MBED // 前進左右差(左モータ)
-//linetrace under 30s parameters
-//#define MSR0 0.7 // 基準速度(右モータ)
-//#define MSR1 0.63 // 低速旋回(右モータ)
-//#define MSR2 0.63 // 中速旋回(右モータ)
-//#define MSR3 0.76 // 前進左右差(右モータ)
+/* trace用変数 */
+int sensArray[32] = {0,4,2,4,1,4,2,4, // ライントレースセンサパターン
+ 3,4,1,4,3,4,1,4, // 0:前回動作継続
+ 5,1,5,1,5,1,5,1, // 1:高速前進
+ 5,1,5,1,5,1,5,1}; // 2:左が速い前進
+ // 3:右が速い前進
+ // 4:右旋回
+ // 5:左旋回
/* 操作モード定義 */
enum MODE{
@@ -65,28 +44,15 @@
SPEED, // 8：スピード制御
LT_R, // 9:低速右折(ライントレース時)
LT_L, // 10:低速左折(ライントレース時)
- AVOID_R,
- AVOID_L
+ AVOID_R, // 11:ライントレース＋障害物回避右
+ AVOID_L // 12:ライントレース＋障害物回避左
};
/* ピン配置 */
-//ReceiverIR ir(p5); // リモコン操作
DigitalOut trig(p6); // 超音波センサtrigger
DigitalIn echo(p7); // 超音波センサecho
PwmOut servo(p25); // サーボ
-// 4輪
-//DigitalIn ss1(p12); // ライントレースセンサ(左)
-//DigitalIn ss2(p11); // ライントレースセンサ
-//DigitalIn ss3(p10); // ライントレースセンサ
-//DigitalIn ss4(p9); // ライントレースセンサ
-//DigitalIn ss5(p8); // ライントレースセンサ(右)
-//// 4輪
-//PwmOut motorR2(p21); // 右モーター後退
-//PwmOut motorR1(p22); // 右モーター前進
-//PwmOut motorL2(p23); // 左モーター後退
-//PwmOut motorL1(p24); // 左モーター前進
-
// 2輪
DigitalIn ss1(p8); // ライントレースセンサ(左)
DigitalIn ss2(p9); // ライントレースセンサ
@@ -99,118 +65,92 @@
PwmOut motorL2(p23); // 左モーター後退
PwmOut motorL1(p24); // 左モーター前進
-DigitalOut led1(LED1);
-DigitalOut led4(LED4);
-
-//AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V)
-
-
-
-
-//I2C i2c_lcd(p28,p27); // LCD(tx, rx)
-
-float motorSpeedR1[9] = {MSR0, MSR1 , MSR2, MSR3 , MSR3};
-float motorSpeedR2[9] = {MSR0, MSR1*0.9, MSR2, MSR3*0.6, MSR3*0.7};
-float motorSpeedL1[9] = {MSL0, MSL1 , MSL2, MSL3 , MSL3};
-float motorSpeedL2[9] = {MSL0, MSL1*0.9, MSL2, MSL3*0.6, MSL3*0.85};
+DigitalOut led1(LED1); // ボードのLED1(右)
+DigitalOut led4(LED4); // ボードのLED4(左)
-int run = 0;
-int speed = 0;
-int dist = 1000;
-int avoid_flag = 0;
-
-/* trace用変数 */
-int sensArray[32] = {0,4,2,4,1,4,2,4, // ライントレースセンサパターン
- 3,4,1,4,3,4,1,4, // 0:前回動作継続
- 5,1,5,1,5,1,5,1, // 1:高速前進
- 5,1,5,1,5,1,5,1}; // 2:低速右折
- // 3:低速左折
- // 4:中速右折
- // 5:中速左折
- // 6:高速右折
- // 7:高速左折
+int run = 0; // 走行モード
+int speed = 0; // 走行スピード
+int dist = 1000; // 障害物までの距離
+int avoid_flag = 0; // 障害物検知したかどうかのフラグ
// 障害物回避用変数
-Timer timer; // 距離計測用タイマ
-Timer t; // 回避時間計測用タイマ
-int DT = 10000; // 距離
-int SC; // 正面
-int SLD; // 左前
-int SRD; // 右前
-int flag_a = 0; // 障害物有無のフラグ
-const int limit = 35; // 障害物の距離のリミット(単位：cm)
-int far; // 最も遠い距離
-int houkou; // 進行方向(1:前 2:左 3:右)
-int t1 = 0;
+Timer timer; // 距離計測用タイマ
+Timer t; // 回避時間計測用タイマ
+int DT = 10000; // 障害物までの距離
+int flag_a = 0; // 障害物有無のフラグ
+const int limit = 35; // 障害物の距離の限界値(単位：cm)
+int t1 = 0; // 検知にかかった時間
+LocalFileSystem local("local"); // ファイルを扱うときに使う
+RawSerial pc(USBTX, USBRX); // PCとシリアル通信するときに使う
-LocalFileSystem local("local");
-RawSerial pc(USBTX, USBRX);
-
-void motor();
-int watch();
+void motor(); // モーター制御関数
+int watch(); // 障害物検知関数
+void avoidance(); // 障害物回避関数
void motor(){
switch(run){
- /* 前進 */
+ // 前進
case ADVANCE:
motorR1 = motorSpeedR1[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL1[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
- /* 右折 */
+ // 右旋回
case RIGHT:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = motorSpeedR2[speed]; // 右後退モーターON
motorL1 = motorSpeedL1[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
- /* 左折 */
+ // 左旋回
case LEFT:
motorR1 = motorSpeedR1[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = LOW; // 左前進モーターOFF
motorL2 = motorSpeedL2[speed]; // 左後退モーターON
break;
- /* 後退 */
+ // 後退
case BACK:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = motorSpeedR2[speed]; // 右後退モーターON
motorL1 = LOW; // 左前進モーターOFF
motorL2 = motorSpeedR2[speed]; // 左後退モーターON
break;
- /* 停止 */
+ // 停止
case STOP:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = LOW; // 右後退モーターOFF
motorL1 = LOW; // 左前進モーターOFF
motorL2 = LOW; // 左後退モーターOFF
break;
- /* 前進(左が速い) */
+ // 前進(左が速い)
case LT_R:
motorR1 = motorSpeedR2[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL1[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
- /* 前進(右が速い) */
+ // 前進(右が速い)
case LT_L:
motorR1 = motorSpeedR1[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL2[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
+ // ライントレース＋障害物回避右
case AVOID_R:
motorR1 = 0.5; // 右前進モーターON
- motorR2 = LOW; // 右後退モーターOFF
+ motorR2 = LOW; // 右後退モーターOFF
motorL1 = 1; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
+ // ライントレース＋障害物回避左
case AVOID_L:
- motorR1 = 1; // 右前進モーターON
- motorR2 = LOW; // 右後退モーターOFF
- motorL1 = 0.5; // 左前進モーターON
+ motorR1 = 1; // 右前進モーターON
+ motorR2 = LOW; // 右後退モーターOFF
+ motorL1 = 0.5; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
}
@@ -238,31 +178,21 @@
speed = 0;
break;
case 2:
-// run = RIGHT; // 低速で右折
- run = LT_R; // 前進(左が速い)
- speed = 3;
+ run = LT_R; // 前進(左が速い)
+ speed = 2;
break;
case 3:
-// run = LEFT; // 低速で左折
run = LT_L; // 前進(右が速い)
- speed = 3;
+ speed = 2;
break;
case 4:
- run = RIGHT; // 低速で右折
+ run = RIGHT; // 右旋回
speed = 1;
break;
case 5:
- run = LEFT; // 低速で左折
+ run = LEFT; // 左旋回
speed = 1;
break;
- case 6:
- run = RIGHT; // 中速で右折
- speed = 2;
- break;
- case 7:
- run = LEFT; // 中速で左折
- speed = 2;
- break;
default:
break; // 前回動作を継続
}
@@ -270,7 +200,7 @@
}
int watch(){
- do{
+ do{
do{
trig = 0;
ThisThread::sleep_for(1); // 5ms待つ
@@ -294,21 +224,47 @@
DT = 1000;
break;
}
- }while(DT > 1000);
- if(DT > 400){
- DT = 400;
- }
- timer.reset();
- return DT;
+ }while(DT > 1000);
+ if(DT > 400){
+ DT = 400;
+ }
+ timer.reset();
+ return DT;
}
+void avoidance(int c){
+ dist = watch();
+ if(dist <= limit){
+ if(c == 0){
+ // 左に回避
+ run = AVOID_L;
+ motor();
+ ThisThread::sleep_for(300);
+ speed = 3;
+ run = LT_R;
+ motor();
+ ThisThread::sleep_for(500);
+ }else{
+ // 右に回避
+ run = AVOID_R;
+ motor();
+ ThisThread::sleep_for(300);
+ speed = 3;
+ run = LT_L;
+ motor();
+ ThisThread::sleep_for(500);
+ }
+ avoid_flag = 1;
+ }
+}
int main(){
- motorR1.period_us(40);
- motorR2.period_us(40);
- motorL1.period_us(40);
- motorL2.period_us(40);
- servo.period_us(40);
- avoid_flag = 0;
+ // 初期設定
+ motorR1.period_us(40); // PWM周期設定
+ motorR2.period_us(40); // PWM周期設定
+ motorL1.period_us(40); // PWM周期設定
+ motorL2.period_us(40); // PWM周期設定
+//servo.period_us(40); // PWM周期設定
+ avoid_flag = 0; // 障害物回避していない
// ファイルに回避方向を保存して制御
FILE *fp;
@@ -319,8 +275,8 @@
fclose(fp);
exit(1);
}
- c = fgetc(fp);
- c -= '0';
+ c = fgetc(fp); // ファイルからデータ読み込み
+ c -= '0'; // 文字を数字に変換
fclose(fp);
// pc.printf("c = %d\r\n", c);
// pc.printf("c - '0' = %d\r\n", c - '0');
@@ -328,11 +284,11 @@
fclose(fp);
exit(1);
}
- if(c == 0){
+ if(c == 0){ // 読み込んだデータが 0 なら 1 を書き込み
fputc('1', fp);
led4 = 1;
// pc.printf("1\r\n");
- }else{
+ }else{ // 読み込んだデータが 1 なら 0 を書き込み
fputc('0', fp);
led1 = 1;
// pc.printf("0\r\n");
@@ -341,30 +297,8 @@
while (true) {
trace();
- if(avoid_flag == 0){
- dist = watch();
- if(dist <= limit){
- if(c == 0){
- // 左に回避
- run = AVOID_L;
- motor();
- ThisThread::sleep_for(220);
- speed = 4;
- run = LT_R;
- motor();
- ThisThread::sleep_for(1500);
- }else{
- // 右に回避
- run = AVOID_R;
- motor();
- ThisThread::sleep_for(250);
- speed = 4;
- run = LT_L;
- motor();
- ThisThread::sleep_for(1000);
- }
- avoid_flag = 1;
- }
+ if(avoid_flag == 0){ // まだ障害物を検知していなければ
+ avoidance(c);
}
}
}