mbed_robotcar / Mbed OS LineTrace_and_Avoidance_2tires

linetrace saikyo!

main.cpp@2:cc4e4527ea43, 2020-09-24 (annotated)

Committer:
takuminomura
Date:
Thu Sep 24 06:58:39 2020 +0000
Revision:
2:cc4e4527ea43
Parent:
1:fd7dd12feee0 
miyasui;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takuminomura 0:55ababc360bf 1 /* mbed Microcontroller Library
takuminomura 0:55ababc360bf 2 * Copyright (c) 2019 ARM Limited
takuminomura 0:55ababc360bf 3 * SPDX-License-Identifier: Apache-2.0
takuminomura 0:55ababc360bf 4 */
takuminomura 0:55ababc360bf 5
takuminomura 0:55ababc360bf 6 #include "mbed.h"
takuminomura 0:55ababc360bf 7 #include "platform/mbed_thread.h"
takuminomura 1:fd7dd12feee0 8 #include <stdio.h>
takuminomura 1:fd7dd12feee0 9 #include <stdlib.h>
takuminomura 0:55ababc360bf 10
takuminomura 0:55ababc360bf 11 /* マクロ定義、列挙型定義 */
takuminomura 0:55ababc360bf 12 #define LOW 0
takuminomura 0:55ababc360bf 13
takuminomura 0:55ababc360bf 14 // 使うロボットカーに合わせる
takuminomura 0:55ababc360bf 15 #define MBED01 1
takuminomura 2:cc4e4527ea43 16 #define MBED09 0.99
takuminomura 0:55ababc360bf 17 #define MBED MBED09 //使うロボットカーの番号にする
takuminomura 2:cc4e4527ea43 18
takuminomura 2:cc4e4527ea43 19 // 0: 前進 1: 旋回 2: 速度差 3: 障害物回避後戻る
takuminomura 2:cc4e4527ea43 20 float motorSpeedR1[9] = {0.5 , 0.5 , 0.5 , 0.5 };
takuminomura 2:cc4e4527ea43 21 float motorSpeedR2[9] = {0.5 , 0.5 , 0.5 , 0.5 };
takuminomura 2:cc4e4527ea43 22 float motorSpeedL1[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
takuminomura 2:cc4e4527ea43 23 float motorSpeedL2[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
takuminomura 0:55ababc360bf 24
takuminomura 2:cc4e4527ea43 25 /* trace用変数 */
takuminomura 2:cc4e4527ea43 26 int sensArray[32] = {0,4,2,4,1,4,2,4, // ライントレースセンサパターン
takuminomura 2:cc4e4527ea43 27 3,4,1,4,3,4,1,4, // 0:前回動作継続
takuminomura 2:cc4e4527ea43 28 5,1,5,1,5,1,5,1, // 1:高速前進
takuminomura 2:cc4e4527ea43 29 5,1,5,1,5,1,5,1}; // 2:左が速い前進
takuminomura 2:cc4e4527ea43 30 // 3:右が速い前進
takuminomura 2:cc4e4527ea43 31 // 4:右旋回
takuminomura 2:cc4e4527ea43 32 // 5:左旋回
takuminomura 0:55ababc360bf 33
takuminomura 0:55ababc360bf 34 /* 操作モード定義 */
takuminomura 0:55ababc360bf 35 enum MODE{
takuminomura 0:55ababc360bf 36 READY = -1, // -1：待ち
takuminomura 0:55ababc360bf 37 ADVANCE = 1, // 1：前進
takuminomura 0:55ababc360bf 38 RIGHT, // 2：右折
takuminomura 0:55ababc360bf 39 LEFT, // 3：左折
takuminomura 0:55ababc360bf 40 BACK, // 4：後退
takuminomura 0:55ababc360bf 41 STOP, // 5：停止
takuminomura 0:55ababc360bf 42 LINE_TRACE, // 6：ライントレース
takuminomura 0:55ababc360bf 43 AVOIDANCE, // 7：障害物回避
takuminomura 0:55ababc360bf 44 SPEED, // 8：スピード制御
takuminomura 0:55ababc360bf 45 LT_R, // 9:低速右折(ライントレース時)
takuminomura 0:55ababc360bf 46 LT_L, // 10:低速左折(ライントレース時)
takuminomura 2:cc4e4527ea43 47 AVOID_R, // 11:ライントレース＋障害物回避右
takuminomura 2:cc4e4527ea43 48 AVOID_L // 12:ライントレース＋障害物回避左
takuminomura 0:55ababc360bf 49 };
takuminomura 0:55ababc360bf 50
takuminomura 0:55ababc360bf 51 /* ピン配置 */
takuminomura 0:55ababc360bf 52 DigitalOut trig(p6); // 超音波センサtrigger
takuminomura 0:55ababc360bf 53 DigitalIn echo(p7); // 超音波センサecho
takuminomura 0:55ababc360bf 54 PwmOut servo(p25); // サーボ
takuminomura 0:55ababc360bf 55
takuminomura 0:55ababc360bf 56 // 2輪
takuminomura 0:55ababc360bf 57 DigitalIn ss1(p8); // ライントレースセンサ(左)
takuminomura 0:55ababc360bf 58 DigitalIn ss2(p9); // ライントレースセンサ
takuminomura 0:55ababc360bf 59 DigitalIn ss3(p10); // ライントレースセンサ
takuminomura 0:55ababc360bf 60 DigitalIn ss4(p11); // ライントレースセンサ
takuminomura 0:55ababc360bf 61 DigitalIn ss5(p12); // ライントレースセンサ(右)
takuminomura 0:55ababc360bf 62 // 2輪
takuminomura 0:55ababc360bf 63 PwmOut motorR2(p22); // 右モーター後退
takuminomura 0:55ababc360bf 64 PwmOut motorR1(p21); // 右モーター前進
takuminomura 0:55ababc360bf 65 PwmOut motorL2(p23); // 左モーター後退
takuminomura 0:55ababc360bf 66 PwmOut motorL1(p24); // 左モーター前進
takuminomura 0:55ababc360bf 67
takuminomura 2:cc4e4527ea43 68 DigitalOut led1(LED1); // ボードのLED1(右)
takuminomura 2:cc4e4527ea43 69 DigitalOut led4(LED4); // ボードのLED4(左)
takuminomura 0:55ababc360bf 70
takuminomura 2:cc4e4527ea43 71 int run = 0; // 走行モード
takuminomura 2:cc4e4527ea43 72 int speed = 0; // 走行スピード
takuminomura 2:cc4e4527ea43 73 int dist = 1000; // 障害物までの距離
takuminomura 2:cc4e4527ea43 74 int avoid_flag = 0; // 障害物検知したかどうかのフラグ
takuminomura 1:fd7dd12feee0 75
takuminomura 1:fd7dd12feee0 76 // 障害物回避用変数
takuminomura 2:cc4e4527ea43 77 Timer timer; // 距離計測用タイマ
takuminomura 2:cc4e4527ea43 78 Timer t; // 回避時間計測用タイマ
takuminomura 2:cc4e4527ea43 79 int DT = 10000; // 障害物までの距離
takuminomura 2:cc4e4527ea43 80 int flag_a = 0; // 障害物有無のフラグ
takuminomura 2:cc4e4527ea43 81 const int limit = 35; // 障害物の距離の限界値(単位：cm)
takuminomura 2:cc4e4527ea43 82 int t1 = 0; // 検知にかかった時間
takuminomura 1:fd7dd12feee0 83
takuminomura 2:cc4e4527ea43 84 LocalFileSystem local("local"); // ファイルを扱うときに使う
takuminomura 2:cc4e4527ea43 85 RawSerial pc(USBTX, USBRX); // PCとシリアル通信するときに使う
takuminomura 1:fd7dd12feee0 86
takuminomura 2:cc4e4527ea43 87 void motor(); // モーター制御関数
takuminomura 2:cc4e4527ea43 88 int watch(); // 障害物検知関数
takuminomura 2:cc4e4527ea43 89 void avoidance(); // 障害物回避関数
takuminomura 0:55ababc360bf 90
takuminomura 0:55ababc360bf 91 void motor(){
takuminomura 0:55ababc360bf 92 switch(run){
takuminomura 2:cc4e4527ea43 93 // 前進
takuminomura 0:55ababc360bf 94 case ADVANCE:
takuminomura 0:55ababc360bf 95 motorR1 = motorSpeedR1[speed]; // 右前進モーターON
takuminomura 0:55ababc360bf 96 motorR2 = LOW; // 右後退モーターOFF
takuminomura 0:55ababc360bf 97 motorL1 = motorSpeedL1[speed]; // 左前進モーターON
takuminomura 0:55ababc360bf 98 motorL2 = LOW; // 左後退モーターOFF
takuminomura 0:55ababc360bf 99 break;
takuminomura 2:cc4e4527ea43 100 // 右旋回
takuminomura 0:55ababc360bf 101 case RIGHT:
takuminomura 0:55ababc360bf 102 motorR1 = LOW; // 右前進モーターOFF
takuminomura 0:55ababc360bf 103 motorR2 = motorSpeedR2[speed]; // 右後退モーターON
takuminomura 0:55ababc360bf 104 motorL1 = motorSpeedL1[speed]; // 左前進モーターON
takuminomura 0:55ababc360bf 105 motorL2 = LOW; // 左後退モーターOFF
takuminomura 0:55ababc360bf 106 break;
takuminomura 2:cc4e4527ea43 107 // 左旋回
takuminomura 0:55ababc360bf 108 case LEFT:
takuminomura 0:55ababc360bf 109 motorR1 = motorSpeedR1[speed]; // 右前進モーターON
takuminomura 0:55ababc360bf 110 motorR2 = LOW; // 右後退モーターOFF
takuminomura 0:55ababc360bf 111 motorL1 = LOW; // 左前進モーターOFF
takuminomura 0:55ababc360bf 112 motorL2 = motorSpeedL2[speed]; // 左後退モーターON
takuminomura 0:55ababc360bf 113 break;
takuminomura 2:cc4e4527ea43 114 // 後退
takuminomura 0:55ababc360bf 115 case BACK:
takuminomura 0:55ababc360bf 116 motorR1 = LOW; // 右前進モーターOFF
takuminomura 0:55ababc360bf 117 motorR2 = motorSpeedR2[speed]; // 右後退モーターON
takuminomura 0:55ababc360bf 118 motorL1 = LOW; // 左前進モーターOFF
takuminomura 0:55ababc360bf 119 motorL2 = motorSpeedR2[speed]; // 左後退モーターON
takuminomura 0:55ababc360bf 120 break;
takuminomura 2:cc4e4527ea43 121 // 停止
takuminomura 0:55ababc360bf 122 case STOP:
takuminomura 0:55ababc360bf 123 motorR1 = LOW; // 右前進モーターOFF
takuminomura 0:55ababc360bf 124 motorR2 = LOW; // 右後退モーターOFF
takuminomura 0:55ababc360bf 125 motorL1 = LOW; // 左前進モーターOFF
takuminomura 0:55ababc360bf 126 motorL2 = LOW; // 左後退モーターOFF
takuminomura 0:55ababc360bf 127 break;
takuminomura 2:cc4e4527ea43 128 // 前進(左が速い)
takuminomura 0:55ababc360bf 129 case LT_R:
takuminomura 0:55ababc360bf 130 motorR1 = motorSpeedR2[speed]; // 右前進モーターON
takuminomura 0:55ababc360bf 131 motorR2 = LOW; // 右後退モーターOFF
takuminomura 0:55ababc360bf 132 motorL1 = motorSpeedL1[speed]; // 左前進モーターON
takuminomura 0:55ababc360bf 133 motorL2 = LOW; // 左後退モーターOFF
takuminomura 0:55ababc360bf 134 break;
takuminomura 2:cc4e4527ea43 135 // 前進(右が速い)
takuminomura 0:55ababc360bf 136 case LT_L:
takuminomura 0:55ababc360bf 137 motorR1 = motorSpeedR1[speed]; // 右前進モーターON
takuminomura 0:55ababc360bf 138 motorR2 = LOW; // 右後退モーターOFF
takuminomura 0:55ababc360bf 139 motorL1 = motorSpeedL2[speed]; // 左前進モーターON
takuminomura 0:55ababc360bf 140 motorL2 = LOW; // 左後退モーターOFF
takuminomura 0:55ababc360bf 141 break;
takuminomura 2:cc4e4527ea43 142 // ライントレース＋障害物回避右
takuminomura 1:fd7dd12feee0 143 case AVOID_R:
takuminomura 1:fd7dd12feee0 144 motorR1 = 0.5; // 右前進モーターON
takuminomura 2:cc4e4527ea43 145 motorR2 = LOW; // 右後退モーターOFF
takuminomura 1:fd7dd12feee0 146 motorL1 = 1; // 左前進モーターON
takuminomura 1:fd7dd12feee0 147 motorL2 = LOW; // 左後退モーターOFF
takuminomura 1:fd7dd12feee0 148 break;
takuminomura 2:cc4e4527ea43 149 // ライントレース＋障害物回避左
takuminomura 1:fd7dd12feee0 150 case AVOID_L:
takuminomura 2:cc4e4527ea43 151 motorR1 = 1; // 右前進モーターON
takuminomura 2:cc4e4527ea43 152 motorR2 = LOW; // 右後退モーターOFF
takuminomura 2:cc4e4527ea43 153 motorL1 = 0.5; // 左前進モーターON
takuminomura 1:fd7dd12feee0 154 motorL2 = LOW; // 左後退モーターOFF
takuminomura 1:fd7dd12feee0 155 break;
takuminomura 0:55ababc360bf 156 }
takuminomura 0:55ababc360bf 157 }
takuminomura 0:55ababc360bf 158
takuminomura 0:55ababc360bf 159 void trace(){
takuminomura 0:55ababc360bf 160 int sensor1 = ss1;
takuminomura 0:55ababc360bf 161 int sensor2 = ss2;
takuminomura 0:55ababc360bf 162 int sensor3 = ss3;
takuminomura 0:55ababc360bf 163 int sensor4 = ss4;
takuminomura 0:55ababc360bf 164 int sensor5 = ss5;
takuminomura 0:55ababc360bf 165 int sensD = 0;
takuminomura 0:55ababc360bf 166
takuminomura 0:55ababc360bf 167 /* センサー値の決定 */
takuminomura 0:55ababc360bf 168 if(sensor1 > 0) sensD |= 0x10;
takuminomura 0:55ababc360bf 169 if(sensor2 > 0) sensD |= 0x08;
takuminomura 0:55ababc360bf 170 if(sensor3 > 0) sensD |= 0x04;
takuminomura 0:55ababc360bf 171 if(sensor4 > 0) sensD |= 0x02;
takuminomura 0:55ababc360bf 172 if(sensor5 > 0) sensD |= 0x01;
takuminomura 0:55ababc360bf 173
takuminomura 0:55ababc360bf 174 /* センサー値によって場合分け */
takuminomura 0:55ababc360bf 175 switch(sensArray[sensD]){
takuminomura 0:55ababc360bf 176 case 1:
takuminomura 0:55ababc360bf 177 run = ADVANCE; // 高速で前進
takuminomura 0:55ababc360bf 178 speed = 0;
takuminomura 0:55ababc360bf 179 break;
takuminomura 0:55ababc360bf 180 case 2:
takuminomura 2:cc4e4527ea43 181 run = LT_R; // 前進(左が速い)
takuminomura 2:cc4e4527ea43 182 speed = 2;
takuminomura 0:55ababc360bf 183 break;
takuminomura 0:55ababc360bf 184 case 3:
takuminomura 0:55ababc360bf 185 run = LT_L; // 前進(右が速い)
takuminomura 2:cc4e4527ea43 186 speed = 2;
takuminomura 0:55ababc360bf 187 break;
takuminomura 0:55ababc360bf 188 case 4:
takuminomura 2:cc4e4527ea43 189 run = RIGHT; // 右旋回
takuminomura 0:55ababc360bf 190 speed = 1;
takuminomura 0:55ababc360bf 191 break;
takuminomura 0:55ababc360bf 192 case 5:
takuminomura 2:cc4e4527ea43 193 run = LEFT; // 左旋回
takuminomura 0:55ababc360bf 194 speed = 1;
takuminomura 0:55ababc360bf 195 break;
takuminomura 0:55ababc360bf 196 default:
takuminomura 0:55ababc360bf 197 break; // 前回動作を継続
takuminomura 0:55ababc360bf 198 }
takuminomura 0:55ababc360bf 199 motor();
takuminomura 0:55ababc360bf 200 }
takuminomura 0:55ababc360bf 201
takuminomura 1:fd7dd12feee0 202 int watch(){
takuminomura 2:cc4e4527ea43 203 do{
takuminomura 1:fd7dd12feee0 204 do{
takuminomura 1:fd7dd12feee0 205 trig = 0;
takuminomura 1:fd7dd12feee0 206 ThisThread::sleep_for(1); // 5ms待つ
takuminomura 1:fd7dd12feee0 207 trig = 1;
takuminomura 1:fd7dd12feee0 208 ThisThread::sleep_for(1); // 15ms待つ
takuminomura 1:fd7dd12feee0 209 trig = 0;
takuminomura 1:fd7dd12feee0 210 timer.start();
takuminomura 1:fd7dd12feee0 211 t1=timer.read_ms();
takuminomura 1:fd7dd12feee0 212 while(echo.read() == 0 && t1<3){
takuminomura 1:fd7dd12feee0 213 t1=timer.read_ms();
takuminomura 1:fd7dd12feee0 214 }
takuminomura 1:fd7dd12feee0 215 timer.stop();
takuminomura 1:fd7dd12feee0 216 timer.reset();
takuminomura 1:fd7dd12feee0 217 }while(t1 >= 3);
takuminomura 1:fd7dd12feee0 218 timer.start(); // 距離計測タイマースタート
takuminomura 1:fd7dd12feee0 219 while(echo.read() == 1){
takuminomura 1:fd7dd12feee0 220 }
takuminomura 1:fd7dd12feee0 221 timer.stop(); // 距離計測タイマーストップ
takuminomura 1:fd7dd12feee0 222 DT = (int)(timer.read_us()*0.01657); // 距離計算
takuminomura 1:fd7dd12feee0 223 if(timer.read_ms() > 1000){
takuminomura 1:fd7dd12feee0 224 DT = 1000;
takuminomura 1:fd7dd12feee0 225 break;
takuminomura 1:fd7dd12feee0 226 }
takuminomura 2:cc4e4527ea43 227 }while(DT > 1000);
takuminomura 2:cc4e4527ea43 228 if(DT > 400){
takuminomura 2:cc4e4527ea43 229 DT = 400;
takuminomura 2:cc4e4527ea43 230 }
takuminomura 2:cc4e4527ea43 231 timer.reset();
takuminomura 2:cc4e4527ea43 232 return DT;
takuminomura 1:fd7dd12feee0 233 }
takuminomura 0:55ababc360bf 234
takuminomura 2:cc4e4527ea43 235 void avoidance(int c){
takuminomura 2:cc4e4527ea43 236 dist = watch();
takuminomura 2:cc4e4527ea43 237 if(dist <= limit){
takuminomura 2:cc4e4527ea43 238 if(c == 0){
takuminomura 2:cc4e4527ea43 239 // 左に回避
takuminomura 2:cc4e4527ea43 240 run = AVOID_L;
takuminomura 2:cc4e4527ea43 241 motor();
takuminomura 2:cc4e4527ea43 242 ThisThread::sleep_for(300);
takuminomura 2:cc4e4527ea43 243 speed = 3;
takuminomura 2:cc4e4527ea43 244 run = LT_R;
takuminomura 2:cc4e4527ea43 245 motor();
takuminomura 2:cc4e4527ea43 246 ThisThread::sleep_for(500);
takuminomura 2:cc4e4527ea43 247 }else{
takuminomura 2:cc4e4527ea43 248 // 右に回避
takuminomura 2:cc4e4527ea43 249 run = AVOID_R;
takuminomura 2:cc4e4527ea43 250 motor();
takuminomura 2:cc4e4527ea43 251 ThisThread::sleep_for(300);
takuminomura 2:cc4e4527ea43 252 speed = 3;
takuminomura 2:cc4e4527ea43 253 run = LT_L;
takuminomura 2:cc4e4527ea43 254 motor();
takuminomura 2:cc4e4527ea43 255 ThisThread::sleep_for(500);
takuminomura 2:cc4e4527ea43 256 }
takuminomura 2:cc4e4527ea43 257 avoid_flag = 1;
takuminomura 2:cc4e4527ea43 258 }
takuminomura 2:cc4e4527ea43 259 }
takuminomura 0:55ababc360bf 260 int main(){
takuminomura 2:cc4e4527ea43 261 // 初期設定
takuminomura 2:cc4e4527ea43 262 motorR1.period_us(40); // PWM周期設定
takuminomura 2:cc4e4527ea43 263 motorR2.period_us(40); // PWM周期設定
takuminomura 2:cc4e4527ea43 264 motorL1.period_us(40); // PWM周期設定
takuminomura 2:cc4e4527ea43 265 motorL2.period_us(40); // PWM周期設定
takuminomura 2:cc4e4527ea43 266 //servo.period_us(40); // PWM周期設定
takuminomura 2:cc4e4527ea43 267 avoid_flag = 0; // 障害物回避していない
takuminomura 1:fd7dd12feee0 268
takuminomura 1:fd7dd12feee0 269 // ファイルに回避方向を保存して制御
takuminomura 1:fd7dd12feee0 270 FILE *fp;
takuminomura 1:fd7dd12feee0 271 const char* fname = "/local/avo.txt"; // ファイル名
takuminomura 1:fd7dd12feee0 272 int c; // 読み込んだデータの格納
takuminomura 1:fd7dd12feee0 273 if(( fp = fopen(fname, "r") ) == NULL){
takuminomura 1:fd7dd12feee0 274 // pc.printf("can't open.\r\n");
takuminomura 1:fd7dd12feee0 275 fclose(fp);
takuminomura 1:fd7dd12feee0 276 exit(1);
takuminomura 1:fd7dd12feee0 277 }
takuminomura 2:cc4e4527ea43 278 c = fgetc(fp); // ファイルからデータ読み込み
takuminomura 2:cc4e4527ea43 279 c -= '0'; // 文字を数字に変換
takuminomura 1:fd7dd12feee0 280 fclose(fp);
takuminomura 1:fd7dd12feee0 281 // pc.printf("c = %d\r\n", c);
takuminomura 1:fd7dd12feee0 282 // pc.printf("c - '0' = %d\r\n", c - '0');
takuminomura 1:fd7dd12feee0 283 if(( fp = fopen(fname, "w") ) == NULL ){
takuminomura 1:fd7dd12feee0 284 fclose(fp);
takuminomura 1:fd7dd12feee0 285 exit(1);
takuminomura 1:fd7dd12feee0 286 }
takuminomura 2:cc4e4527ea43 287 if(c == 0){ // 読み込んだデータが 0 なら 1 を書き込み
takuminomura 1:fd7dd12feee0 288 fputc('1', fp);
takuminomura 1:fd7dd12feee0 289 led4 = 1;
takuminomura 1:fd7dd12feee0 290 // pc.printf("1\r\n");
takuminomura 2:cc4e4527ea43 291 }else{ // 読み込んだデータが 1 なら 0 を書き込み
takuminomura 1:fd7dd12feee0 292 fputc('0', fp);
takuminomura 1:fd7dd12feee0 293 led1 = 1;
takuminomura 1:fd7dd12feee0 294 // pc.printf("0\r\n");
takuminomura 1:fd7dd12feee0 295 }
takuminomura 1:fd7dd12feee0 296 fclose(fp);
takuminomura 1:fd7dd12feee0 297
takuminomura 0:55ababc360bf 298 while (true) {
takuminomura 0:55ababc360bf 299 trace();
takuminomura 2:cc4e4527ea43 300 if(avoid_flag == 0){ // まだ障害物を検知していなければ
takuminomura 2:cc4e4527ea43 301 avoidance(c);
takuminomura 1:fd7dd12feee0 302 }
takuminomura 0:55ababc360bf 303 }
takuminomura 0:55ababc360bf 304 }