linetrace saikyo!
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* mbed Microcontroller Library 00002 * Copyright (c) 2019 ARM Limited 00003 * SPDX-License-Identifier: Apache-2.0 00004 */ 00005 00006 #include "mbed.h" 00007 #include "platform/mbed_thread.h" 00008 #include <stdio.h> 00009 #include <stdlib.h> 00010 00011 /* マクロ定義、列挙型定義 */ 00012 #define LOW 0 00013 00014 // 使うロボットカーに合わせる 00015 #define MBED01 1 00016 #define MBED09 0.99 00017 #define MBED MBED09 //使うロボットカーの番号にする 00018 00019 // 0: 前進 1: 旋回 2: 速度差 3: 障害物回避後戻る 00020 float motorSpeedR1[9] = {0.5 , 0.5 , 0.5 , 0.5 }; 00021 float motorSpeedR2[9] = {0.5 , 0.5 , 0.5 , 0.5 }; 00022 float motorSpeedL1[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED}; 00023 float motorSpeedL2[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED}; 00024 00025 /* trace用変数 */ 00026 int sensArray[32] = {0,4,2,4,1,4,2,4, // ライントレースセンサパターン 00027 3,4,1,4,3,4,1,4, // 0:前回動作継続 00028 5,1,5,1,5,1,5,1, // 1:高速前進 00029 5,1,5,1,5,1,5,1}; // 2:左が速い前進 00030 // 3:右が速い前進 00031 // 4:右旋回 00032 // 5:左旋回 00033 00034 /* 操作モード定義 */ 00035 enum MODE{ 00036 READY = -1, // -1:待ち 00037 ADVANCE = 1, // 1:前進 00038 RIGHT, // 2:右折 00039 LEFT, // 3:左折 00040 BACK, // 4:後退 00041 STOP, // 5:停止 00042 LINE_TRACE, // 6:ライントレース 00043 AVOIDANCE, // 7:障害物回避 00044 SPEED, // 8:スピード制御 00045 LT_R, // 9:低速右折(ライントレース時) 00046 LT_L, // 10:低速左折(ライントレース時) 00047 AVOID_R, // 11:ライントレース+障害物回避右 00048 AVOID_L // 12:ライントレース+障害物回避左 00049 }; 00050 00051 /* ピン配置 */ 00052 DigitalOut trig(p6); // 超音波センサtrigger 00053 DigitalIn echo(p7); // 超音波センサecho 00054 PwmOut servo(p25); // サーボ 00055 00056 // 2輪 00057 DigitalIn ss1(p8); // ライントレースセンサ(左) 00058 DigitalIn ss2(p9); // ライントレースセンサ 00059 DigitalIn ss3(p10); // ライントレースセンサ 00060 DigitalIn ss4(p11); // ライントレースセンサ 00061 DigitalIn ss5(p12); // ライントレースセンサ(右) 00062 // 2輪 00063 PwmOut motorR2(p22); // 右モーター後退 00064 PwmOut motorR1(p21); // 右モーター前進 00065 PwmOut motorL2(p23); // 左モーター後退 00066 PwmOut motorL1(p24); // 左モーター前進 00067 00068 DigitalOut led1(LED1); // ボードのLED1(右) 00069 DigitalOut led4(LED4); // ボードのLED4(左) 00070 00071 int run = 0; // 走行モード 00072 int speed = 0; // 走行スピード 00073 int dist = 1000; // 障害物までの距離 00074 int avoid_flag = 0; // 障害物検知したかどうかのフラグ 00075 00076 // 障害物回避用変数 00077 Timer timer; // 距離計測用タイマ 00078 Timer t; // 回避時間計測用タイマ 00079 int DT = 10000; // 障害物までの距離 00080 int flag_a = 0; // 障害物有無のフラグ 00081 const int limit = 35; // 障害物の距離の限界値(単位:cm) 00082 int t1 = 0; // 検知にかかった時間 00083 00084 LocalFileSystem local("local"); // ファイルを扱うときに使う 00085 RawSerial pc(USBTX, USBRX); // PCとシリアル通信するときに使う 00086 00087 void motor(); // モーター制御関数 00088 int watch(); // 障害物検知関数 00089 void avoidance(); // 障害物回避関数 00090 00091 void motor(){ 00092 switch(run){ 00093 // 前進 00094 case ADVANCE: 00095 motorR1 = motorSpeedR1[speed]; // 右前進モーターON 00096 motorR2 = LOW; // 右後退モーターOFF 00097 motorL1 = motorSpeedL1[speed]; // 左前進モーターON 00098 motorL2 = LOW; // 左後退モーターOFF 00099 break; 00100 // 右旋回 00101 case RIGHT: 00102 motorR1 = LOW; // 右前進モーターOFF 00103 motorR2 = motorSpeedR2[speed]; // 右後退モーターON 00104 motorL1 = motorSpeedL1[speed]; // 左前進モーターON 00105 motorL2 = LOW; // 左後退モーターOFF 00106 break; 00107 // 左旋回 00108 case LEFT: 00109 motorR1 = motorSpeedR1[speed]; // 右前進モーターON 00110 motorR2 = LOW; // 右後退モーターOFF 00111 motorL1 = LOW; // 左前進モーターOFF 00112 motorL2 = motorSpeedL2[speed]; // 左後退モーターON 00113 break; 00114 // 後退 00115 case BACK: 00116 motorR1 = LOW; // 右前進モーターOFF 00117 motorR2 = motorSpeedR2[speed]; // 右後退モーターON 00118 motorL1 = LOW; // 左前進モーターOFF 00119 motorL2 = motorSpeedR2[speed]; // 左後退モーターON 00120 break; 00121 // 停止 00122 case STOP: 00123 motorR1 = LOW; // 右前進モーターOFF 00124 motorR2 = LOW; // 右後退モーターOFF 00125 motorL1 = LOW; // 左前進モーターOFF 00126 motorL2 = LOW; // 左後退モーターOFF 00127 break; 00128 // 前進(左が速い) 00129 case LT_R: 00130 motorR1 = motorSpeedR2[speed]; // 右前進モーターON 00131 motorR2 = LOW; // 右後退モーターOFF 00132 motorL1 = motorSpeedL1[speed]; // 左前進モーターON 00133 motorL2 = LOW; // 左後退モーターOFF 00134 break; 00135 // 前進(右が速い) 00136 case LT_L: 00137 motorR1 = motorSpeedR1[speed]; // 右前進モーターON 00138 motorR2 = LOW; // 右後退モーターOFF 00139 motorL1 = motorSpeedL2[speed]; // 左前進モーターON 00140 motorL2 = LOW; // 左後退モーターOFF 00141 break; 00142 // ライントレース+障害物回避右 00143 case AVOID_R: 00144 motorR1 = 0.5; // 右前進モーターON 00145 motorR2 = LOW; // 右後退モーターOFF 00146 motorL1 = 1; // 左前進モーターON 00147 motorL2 = LOW; // 左後退モーターOFF 00148 break; 00149 // ライントレース+障害物回避左 00150 case AVOID_L: 00151 motorR1 = 1; // 右前進モーターON 00152 motorR2 = LOW; // 右後退モーターOFF 00153 motorL1 = 0.5; // 左前進モーターON 00154 motorL2 = LOW; // 左後退モーターOFF 00155 break; 00156 } 00157 } 00158 00159 void trace(){ 00160 int sensor1 = ss1; 00161 int sensor2 = ss2; 00162 int sensor3 = ss3; 00163 int sensor4 = ss4; 00164 int sensor5 = ss5; 00165 int sensD = 0; 00166 00167 /* センサー値の決定 */ 00168 if(sensor1 > 0) sensD |= 0x10; 00169 if(sensor2 > 0) sensD |= 0x08; 00170 if(sensor3 > 0) sensD |= 0x04; 00171 if(sensor4 > 0) sensD |= 0x02; 00172 if(sensor5 > 0) sensD |= 0x01; 00173 00174 /* センサー値によって場合分け */ 00175 switch(sensArray[sensD]){ 00176 case 1: 00177 run = ADVANCE; // 高速で前進 00178 speed = 0; 00179 break; 00180 case 2: 00181 run = LT_R; // 前進(左が速い) 00182 speed = 2; 00183 break; 00184 case 3: 00185 run = LT_L; // 前進(右が速い) 00186 speed = 2; 00187 break; 00188 case 4: 00189 run = RIGHT; // 右旋回 00190 speed = 1; 00191 break; 00192 case 5: 00193 run = LEFT; // 左旋回 00194 speed = 1; 00195 break; 00196 default: 00197 break; // 前回動作を継続 00198 } 00199 motor(); 00200 } 00201 00202 int watch(){ 00203 do{ 00204 do{ 00205 trig = 0; 00206 ThisThread::sleep_for(1); // 5ms待つ 00207 trig = 1; 00208 ThisThread::sleep_for(1); // 15ms待つ 00209 trig = 0; 00210 timer.start(); 00211 t1=timer.read_ms(); 00212 while(echo.read() == 0 && t1<3){ 00213 t1=timer.read_ms(); 00214 } 00215 timer.stop(); 00216 timer.reset(); 00217 }while(t1 >= 3); 00218 timer.start(); // 距離計測タイマースタート 00219 while(echo.read() == 1){ 00220 } 00221 timer.stop(); // 距離計測タイマーストップ 00222 DT = (int)(timer.read_us()*0.01657); // 距離計算 00223 if(timer.read_ms() > 1000){ 00224 DT = 1000; 00225 break; 00226 } 00227 }while(DT > 1000); 00228 if(DT > 400){ 00229 DT = 400; 00230 } 00231 timer.reset(); 00232 return DT; 00233 } 00234 00235 void avoidance(int c){ 00236 dist = watch(); 00237 if(dist <= limit){ 00238 if(c == 0){ 00239 // 左に回避 00240 run = AVOID_L; 00241 motor(); 00242 ThisThread::sleep_for(300); 00243 speed = 3; 00244 run = LT_R; 00245 motor(); 00246 ThisThread::sleep_for(500); 00247 }else{ 00248 // 右に回避 00249 run = AVOID_R; 00250 motor(); 00251 ThisThread::sleep_for(300); 00252 speed = 3; 00253 run = LT_L; 00254 motor(); 00255 ThisThread::sleep_for(500); 00256 } 00257 avoid_flag = 1; 00258 } 00259 } 00260 int main(){ 00261 // 初期設定 00262 motorR1.period_us(40); // PWM周期設定 00263 motorR2.period_us(40); // PWM周期設定 00264 motorL1.period_us(40); // PWM周期設定 00265 motorL2.period_us(40); // PWM周期設定 00266 //servo.period_us(40); // PWM周期設定 00267 avoid_flag = 0; // 障害物回避していない 00268 00269 // ファイルに回避方向を保存して制御 00270 FILE *fp; 00271 const char* fname = "/local/avo.txt"; // ファイル名 00272 int c; // 読み込んだデータの格納 00273 if(( fp = fopen(fname, "r") ) == NULL){ 00274 // pc.printf("can't open.\r\n"); 00275 fclose(fp); 00276 exit(1); 00277 } 00278 c = fgetc(fp); // ファイルからデータ読み込み 00279 c -= '0'; // 文字を数字に変換 00280 fclose(fp); 00281 // pc.printf("c = %d\r\n", c); 00282 // pc.printf("c - '0' = %d\r\n", c - '0'); 00283 if(( fp = fopen(fname, "w") ) == NULL ){ 00284 fclose(fp); 00285 exit(1); 00286 } 00287 if(c == 0){ // 読み込んだデータが 0 なら 1 を書き込み 00288 fputc('1', fp); 00289 led4 = 1; 00290 // pc.printf("1\r\n"); 00291 }else{ // 読み込んだデータが 1 なら 0 を書き込み 00292 fputc('0', fp); 00293 led1 = 1; 00294 // pc.printf("0\r\n"); 00295 } 00296 fclose(fp); 00297 00298 while (true) { 00299 trace(); 00300 if(avoid_flag == 0){ // まだ障害物を検知していなければ 00301 avoidance(c); 00302 } 00303 } 00304 }
Generated on Sun Jul 24 2022 01:31:25 by
1.7.2