mbed_robotcar / Mbed OS Dontreadme

赤子を殺すより楽な作業よ

Dependencies:   RemoteIR TextLCD

main.cpp@7:4a65f5a25c13, 2020-07-29 (annotated)

Committer:
tomotsugu
Date:
Wed Jul 29 08:12:46 2020 +0000
Revision:
7:4a65f5a25c13
Parent:
6:4817fe7b7654 
robot_car

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yangtzuli 0:0d0037aabe41 1 /* mbed Microcontroller Library
yangtzuli 0:0d0037aabe41 2 * Copyright (c) 2019 ARM Limited
yangtzuli 0:0d0037aabe41 3 * SPDX-License-Identifier: Apache-2.0
yangtzuli 0:0d0037aabe41 4 */
yangtzuli 0:0d0037aabe41 5
yangtzuli 0:0d0037aabe41 6 #include "mbed.h"
yangtzuli 0:0d0037aabe41 7 #include "ReceiverIR.h"
yangtzuli 0:0d0037aabe41 8 #include "rtos.h"
yangtzuli 0:0d0037aabe41 9 #include <stdint.h>
yangtzuli 0:0d0037aabe41 10 #include "platform/mbed_thread.h"
yangtzuli 2:38825726cb1b 11 #include "TextLCD.h"
yangtzuli 0:0d0037aabe41 12
yangtzuli 0:0d0037aabe41 13 Serial pc(USBTX, USBRX);
yangtzuli 0:0d0037aabe41 14
yangtzuli 3:2ae6218973be 15 /* マクロ定義、列挙型定義 */
yangtzuli 3:2ae6218973be 16 #define MIN_V 2.0
yangtzuli 3:2ae6218973be 17 #define MAX_V 2.67
tomotsugu 6:4817fe7b7654 18 #define LOW 0
tomotsugu 6:4817fe7b7654 19 #define HIGH 1
tomotsugu 6:4817fe7b7654 20 #define NORMAL 0
tomotsugu 6:4817fe7b7654 21 #define FAST 1
tomotsugu 6:4817fe7b7654 22 #define VERYFAST 2
yangtzuli 2:38825726cb1b 23
yangtzuli 3:2ae6218973be 24 enum MODE{
yangtzuli 3:2ae6218973be 25 READY = -1,
yangtzuli 3:2ae6218973be 26 ADVANCE = 1,
yangtzuli 2:38825726cb1b 27 RIGHT,
yangtzuli 2:38825726cb1b 28 LEFT,
yangtzuli 2:38825726cb1b 29 BACK,
yangtzuli 2:38825726cb1b 30 STOP,
yangtzuli 2:38825726cb1b 31 LINE_TRACE,
yangtzuli 2:38825726cb1b 32 AVOIDANCE,
yangtzuli 3:2ae6218973be 33 SPEED,
yangtzuli 2:38825726cb1b 34 };
yangtzuli 2:38825726cb1b 35
yangtzuli 3:2ae6218973be 36 /* ピン配置 */
yangtzuli 3:2ae6218973be 37 ReceiverIR ir(p5); // リモコン操作
yangtzuli 3:2ae6218973be 38 DigitalOut trig(p6); // 超音波センサtrigger
yangtzuli 3:2ae6218973be 39 DigitalOut echo(p7); // 超音波センサecho
yangtzuli 3:2ae6218973be 40 DigitalIn ss1(p8); // ライントレースセンサ(左)
yangtzuli 3:2ae6218973be 41 DigitalIn ss2(p9); // ライントレースセンサ
yangtzuli 3:2ae6218973be 42 DigitalIn ss3(p10); // ライントレースセンサ
yangtzuli 3:2ae6218973be 43 DigitalIn ss4(p11); // ライントレースセンサ
yangtzuli 3:2ae6218973be 44 DigitalIn ss5(p12); // ライントレースセンサ(右)
yangtzuli 3:2ae6218973be 45 Serial esp(p13, p14); // Wi-Fiモジュール(tx, rx)
yangtzuli 3:2ae6218973be 46 AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V)
yangtzuli 3:2ae6218973be 47 PwmOut motorR2(p21); // 右モーター後退
yangtzuli 3:2ae6218973be 48 PwmOut motorR1(p22); // 右モーター前進
yangtzuli 3:2ae6218973be 49 PwmOut motorL2(p23); // 左モーター後退
yangtzuli 3:2ae6218973be 50 PwmOut motorL1(p24); // 左モーター前進
yangtzuli 3:2ae6218973be 51 PwmOut servo(p25); // サーボ
yangtzuli 3:2ae6218973be 52 I2C i2c_lcd(p28,p27); // LCD(tx, rx)
yangtzuli 2:38825726cb1b 53
yangtzuli 3:2ae6218973be 54 /* 変数宣言 */
yangtzuli 3:2ae6218973be 55 int mode; // 操作モード
yangtzuli 3:2ae6218973be 56 int run; // 走行状態
yangtzuli 3:2ae6218973be 57 int beforeMode; // 前のモード
tomotsugu 5:5414f85108aa 58 int flag_sp = 0; // スピード変化フラグ
yangtzuli 3:2ae6218973be 59 Timer viewTimer; // スピ―ド変更時に3秒計測
yangtzuli 3:2ae6218973be 60 float motorSpeed[6] = {0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
yangtzuli 3:2ae6218973be 61 // モーター速度設定(後半はライントレース用)
tomotsugu 5:5414f85108aa 62
tomotsugu 5:5414f85108aa 63 Mutex mutex; // ミューテックス
tomotsugu 5:5414f85108aa 64
yangtzuli 3:2ae6218973be 65 // ポートp15を赤外線受信モジュールの接続先に指定
yangtzuli 3:2ae6218973be 66 RemoteIR::Format format;
yangtzuli 3:2ae6218973be 67 uint8_t buf[32];
yangtzuli 3:2ae6218973be 68 uint32_t bitcount;
yangtzuli 3:2ae6218973be 69 uint32_t code;
yangtzuli 3:2ae6218973be 70
tomotsugu 5:5414f85108aa 71 /* bChange, lcdBacklight用変数設定 */
yangtzuli 3:2ae6218973be 72 TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
tomotsugu 5:5414f85108aa 73 int b = 0; // バッテリー残量
tomotsugu 5:5414f85108aa 74 int flag_b = 0; // バックライト点滅フラグ
tomotsugu 5:5414f85108aa 75 int flag_t = 0; // バックライトタイマーフラグ
yangtzuli 3:2ae6218973be 76
tomotsugu 5:5414f85108aa 77 /* ライントレース */
yangtzuli 3:2ae6218973be 78 int sensArray[32] = {0,0,0,0,0,0,0,0, // ライントレースセンサパターン
yangtzuli 3:2ae6218973be 79 0,0,0,0,0,0,0,0,
yangtzuli 3:2ae6218973be 80 0,0,0,0,0,0,0,0,
yangtzuli 3:2ae6218973be 81 0,0,0,0,0,0,0,0};
yangtzuli 0:0d0037aabe41 82
yangtzuli 2:38825726cb1b 83 /* 障害物検知用の変数設定 */
yangtzuli 3:2ae6218973be 84 Timer timer; // 距離計測用タイマ
yangtzuli 3:2ae6218973be 85 int DT; // 距離
yangtzuli 2:38825726cb1b 86 int SC; // 正面
yangtzuli 2:38825726cb1b 87 int SL; // 左
yangtzuli 2:38825726cb1b 88 int SR; // 右
yangtzuli 2:38825726cb1b 89 int SLD; // 左前
yangtzuli 2:38825726cb1b 90 int SRD; // 右前
tomotsugu 5:5414f85108aa 91 int flag_a = 0; // 障害物有無のフラグ
yangtzuli 2:38825726cb1b 92 const int limit = 20; // 障害物の距離のリミット(単位：cm)
yangtzuli 3:2ae6218973be 93 int far; // 最も遠い距離
yangtzuli 2:38825726cb1b 94 int houkou; // 進行方向(1:前 2:左 3:右)
yangtzuli 2:38825726cb1b 95 int i; // ループ変数
yangtzuli 2:38825726cb1b 96
yangtzuli 3:2ae6218973be 97
yangtzuli 3:2ae6218973be 98
yangtzuli 3:2ae6218973be 99 /* プロトタイプ宣言 */
yangtzuli 3:2ae6218973be 100 void decodeIR(void const *argument);
yangtzuli 3:2ae6218973be 101 void motor(void const *argument);
yangtzuli 3:2ae6218973be 102 void changeSpeed();
yangtzuli 2:38825726cb1b 103 void avoidance(void const *argument);
yangtzuli 3:2ae6218973be 104 void trace(void const *argument);
yangtzuli 2:38825726cb1b 105 void watchsurrounding();
yangtzuli 2:38825726cb1b 106 int watch();
tomotsugu 5:5414f85108aa 107 void bChange();
yangtzuli 3:2ae6218973be 108 void display();
yangtzuli 3:2ae6218973be 109 void lcdBacklight(void const *argument);
tomotsugu 5:5414f85108aa 110
tomotsugu 5:5414f85108aa 111 /* スレッド宣言 */
tomotsugu 6:4817fe7b7654 112 Thread thread1(decodeIR, NULL, osPriorityRealtime); //+3
tomotsugu 6:4817fe7b7654 113 Thread thread2(motor, NULL, osPriorityHigh); //+2
tomotsugu 6:4817fe7b7654 114 Thread thread3(avoidance, NULL, osPriorityHigh); //+2
tomotsugu 6:4817fe7b7654 115 Thread thread4(trace, NULL, osPriorityHigh); //+2
tomotsugu 5:5414f85108aa 116
tomotsugu 6:4817fe7b7654 117 /* バックライト点滅タイマー割り込み */
yangtzuli 3:2ae6218973be 118 RtosTimer bTimer(lcdBacklight, osTimerPeriodic);
yangtzuli 2:38825726cb1b 119
tomotsugu 5:5414f85108aa 120 /* リモコン受信スレッド */
yangtzuli 4:3f80c0180e2f 121 void decodeIR(void const *argument){
yangtzuli 4:3f80c0180e2f 122 while(1){
tomotsugu 6:4817fe7b7654 123 if (ir.getState() == ReceiverIR::Received){ // 受信待ち
tomotsugu 6:4817fe7b7654 124 bitcount = ir.getData(&format, buf, sizeof(buf) * 8); // コード受信
tomotsugu 6:4817fe7b7654 125 if(bitcount > 1){ // 受信成功
yangtzuli 1:5bb497a38344 126 code=0;
yangtzuli 1:5bb497a38344 127 for(int j=0;j<4;j++){
yangtzuli 1:5bb497a38344 128 code+=(buf[j]<<(8*(3-j)));
yangtzuli 1:5bb497a38344 129 }
yangtzuli 3:2ae6218973be 130 if(mode!=SPEED){
yangtzuli 3:2ae6218973be 131 beforeMode=mode;
yangtzuli 3:2ae6218973be 132 }
yangtzuli 0:0d0037aabe41 133 switch(code){
tomotsugu 6:4817fe7b7654 134 case 0x40bf27d8: //クイック
yangtzuli 3:2ae6218973be 135 mode = SPEED;
yangtzuli 4:3f80c0180e2f 136 changeSpeed();
yangtzuli 4:3f80c0180e2f 137 display();
yangtzuli 4:3f80c0180e2f 138 mode = beforeMode;
yangtzuli 1:5bb497a38344 139 break;
tomotsugu 6:4817fe7b7654 140 case 0x40be34cb: //レグザリンク
yangtzuli 3:2ae6218973be 141 mode=LINE_TRACE;
yangtzuli 3:2ae6218973be 142 display();
yangtzuli 1:5bb497a38344 143 break;
tomotsugu 6:4817fe7b7654 144 case 0x40bf6e91: //番組表
yangtzuli 3:2ae6218973be 145 mode=AVOIDANCE;
yangtzuli 3:2ae6218973be 146 display();
yangtzuli 1:5bb497a38344 147 break;
tomotsugu 6:4817fe7b7654 148 case 0x40bf3ec1: //↑
yangtzuli 3:2ae6218973be 149 mode = ADVANCE;
yangtzuli 3:2ae6218973be 150 run = ADVANCE;
yangtzuli 3:2ae6218973be 151 display();
yangtzuli 1:5bb497a38344 152 break;
tomotsugu 6:4817fe7b7654 153 case 0x40bf3fc0: //↓
yangtzuli 3:2ae6218973be 154 mode = BACK;
yangtzuli 3:2ae6218973be 155 run = BACK;
yangtzuli 3:2ae6218973be 156 display();
yangtzuli 0:0d0037aabe41 157 break;
tomotsugu 6:4817fe7b7654 158 case 0x40bf5fa0: //←
yangtzuli 3:2ae6218973be 159 mode = LEFT;
yangtzuli 3:2ae6218973be 160 run = LEFT;
yangtzuli 3:2ae6218973be 161 display();
yangtzuli 1:5bb497a38344 162 break;
tomotsugu 6:4817fe7b7654 163 case 0x40bf5ba4: //→
yangtzuli 3:2ae6218973be 164 mode = RIGHT;
yangtzuli 3:2ae6218973be 165 run = RIGHT;
yangtzuli 3:2ae6218973be 166 display();
yangtzuli 1:5bb497a38344 167 break;
tomotsugu 6:4817fe7b7654 168 case 0x40bf3dc2: //決定
yangtzuli 3:2ae6218973be 169 mode = STOP;
yangtzuli 3:2ae6218973be 170 run = STOP;
yangtzuli 3:2ae6218973be 171 display();
yangtzuli 1:5bb497a38344 172 break;
yangtzuli 0:0d0037aabe41 173 default:
yangtzuli 0:0d0037aabe41 174 ;
yangtzuli 0:0d0037aabe41 175 }
yangtzuli 0:0d0037aabe41 176 }
yangtzuli 0:0d0037aabe41 177 }
yangtzuli 4:3f80c0180e2f 178 if(viewTimer.read_ms()>=3000){
yangtzuli 4:3f80c0180e2f 179 viewTimer.stop();
yangtzuli 4:3f80c0180e2f 180 viewTimer.reset();
yangtzuli 4:3f80c0180e2f 181 display();
yangtzuli 4:3f80c0180e2f 182 }
tomotsugu 7:4a65f5a25c13 183 ThisThread::sleep_for(90); // 90ms待つ
yangtzuli 4:3f80c0180e2f 184 }
yangtzuli 2:38825726cb1b 185 }
tomotsugu 5:5414f85108aa 186
tomotsugu 5:5414f85108aa 187 /* モータースレッド */
yangtzuli 2:38825726cb1b 188 void motor(void const *argument){
tomotsugu 5:5414f85108aa 189 while(1){
yangtzuli 3:2ae6218973be 190 switch(run){
yangtzuli 3:2ae6218973be 191 case ADVANCE:
yangtzuli 3:2ae6218973be 192 motorR1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 193 motorR2 = LOW;
yangtzuli 3:2ae6218973be 194 motorL1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 195 motorL2 = LOW;
yangtzuli 3:2ae6218973be 196 break;
yangtzuli 3:2ae6218973be 197 case RIGHT:
yangtzuli 3:2ae6218973be 198 motorR1 = LOW;
yangtzuli 3:2ae6218973be 199 motorR2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 200 motorL1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 201 motorL2 = LOW;
yangtzuli 3:2ae6218973be 202 break;
yangtzuli 3:2ae6218973be 203 case LEFT:
yangtzuli 3:2ae6218973be 204 motorR1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 205 motorR2 = LOW;
yangtzuli 3:2ae6218973be 206 motorL1 = LOW;
yangtzuli 3:2ae6218973be 207 motorL2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 208 break;
yangtzuli 3:2ae6218973be 209 case BACK:
yangtzuli 3:2ae6218973be 210 motorR1 = LOW;
yangtzuli 3:2ae6218973be 211 motorR2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 212 motorL1 = LOW;
yangtzuli 3:2ae6218973be 213 motorL2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 214 break;
yangtzuli 3:2ae6218973be 215 case STOP:
yangtzuli 3:2ae6218973be 216 motorR1 = LOW;
yangtzuli 3:2ae6218973be 217 motorR2 = LOW;
yangtzuli 3:2ae6218973be 218 motorL1 = LOW;
yangtzuli 3:2ae6218973be 219 motorL2 = LOW;
yangtzuli 3:2ae6218973be 220 break;
yangtzuli 3:2ae6218973be 221 }
tomotsugu 7:4a65f5a25c13 222 if(flag_sp > VERYFAST){
tomotsugu 7:4a65f5a25c13 223 flag_sp -= 3 * (flag_sp / 3);
tomotsugu 7:4a65f5a25c13 224 }
tomotsugu 7:4a65f5a25c13 225 ThisThread::sleep_for(30); // 30ms待つ
yangtzuli 2:38825726cb1b 226 }
yangtzuli 2:38825726cb1b 227 }
tomotsugu 5:5414f85108aa 228
tomotsugu 5:5414f85108aa 229 /* スピード変更関数 */
yangtzuli 3:2ae6218973be 230 void changeSpeed(){
tomotsugu 7:4a65f5a25c13 231 if(flag_sp%3 == 2){ // 最速の場合
tomotsugu 7:4a65f5a25c13 232 flag_sp -= 2; // 2段階下げる
tomotsugu 5:5414f85108aa 233 }
tomotsugu 6:4817fe7b7654 234 else{ // 最速以外の場合
tomotsugu 6:4817fe7b7654 235 flag_sp = flag_sp + 1; // 1段階上げる
yangtzuli 3:2ae6218973be 236 }
yangtzuli 3:2ae6218973be 237 }
tomotsugu 5:5414f85108aa 238
tomotsugu 5:5414f85108aa 239 /* ライントレーススレッド */
yangtzuli 3:2ae6218973be 240 void trace(void const *argument){
tomotsugu 5:5414f85108aa 241 while(1){
yangtzuli 3:2ae6218973be 242 if(mode==LINE_TRACE){
yangtzuli 3:2ae6218973be 243 // センサー値読み取り
yangtzuli 3:2ae6218973be 244 int sensor1 = ss1;
yangtzuli 3:2ae6218973be 245 int sensor2 = ss2;
yangtzuli 3:2ae6218973be 246 int sensor3 = ss3;
yangtzuli 3:2ae6218973be 247 int sensor4 = ss4;
yangtzuli 3:2ae6218973be 248 int sensor5 = ss5;
yangtzuli 3:2ae6218973be 249
yangtzuli 3:2ae6218973be 250 int sensD = 0;
yangtzuli 3:2ae6218973be 251 int sensorNum;
yangtzuli 3:2ae6218973be 252
yangtzuli 3:2ae6218973be 253 if(sensor1 > 0) sensD |= 0x10;
yangtzuli 3:2ae6218973be 254 if(sensor2 > 0) sensD |= 0x08;
yangtzuli 3:2ae6218973be 255 if(sensor3 > 0) sensD |= 0x04;
yangtzuli 3:2ae6218973be 256 if(sensor4 > 0) sensD |= 0x02;
yangtzuli 3:2ae6218973be 257 if(sensor5 > 0) sensD |= 0x01;
yangtzuli 3:2ae6218973be 258
yangtzuli 3:2ae6218973be 259 sensorNum = sensArray[sensD];
yangtzuli 3:2ae6218973be 260
yangtzuli 3:2ae6218973be 261 switch(sensorNum){
yangtzuli 3:2ae6218973be 262 case 1:
yangtzuli 3:2ae6218973be 263 run = ADVANCE;
yangtzuli 3:2ae6218973be 264 break;
yangtzuli 3:2ae6218973be 265 case 2:
yangtzuli 3:2ae6218973be 266 run = RIGHT;
yangtzuli 3:2ae6218973be 267 break;
yangtzuli 3:2ae6218973be 268 case 3:
yangtzuli 3:2ae6218973be 269 run = LEFT;
yangtzuli 3:2ae6218973be 270 break;
yangtzuli 3:2ae6218973be 271 case 4:
yangtzuli 3:2ae6218973be 272 flag_sp += 3;
yangtzuli 3:2ae6218973be 273 run = RIGHT;
yangtzuli 3:2ae6218973be 274 break;
yangtzuli 3:2ae6218973be 275 case 5:
yangtzuli 3:2ae6218973be 276 flag_sp += 3;
yangtzuli 3:2ae6218973be 277 run = LEFT;
yangtzuli 3:2ae6218973be 278 break;
yangtzuli 3:2ae6218973be 279 }
yangtzuli 4:3f80c0180e2f 280 ThisThread::sleep_for(30);
yangtzuli 3:2ae6218973be 281 }else{
yangtzuli 4:3f80c0180e2f 282 ThisThread::sleep_for(1);
yangtzuli 3:2ae6218973be 283 }
yangtzuli 3:2ae6218973be 284 }
yangtzuli 3:2ae6218973be 285 }
yangtzuli 3:2ae6218973be 286
tomotsugu 5:5414f85108aa 287 /* 障害物回避走行スレッド */
yangtzuli 2:38825726cb1b 288 void avoidance(void const *argument){
yangtzuli 3:2ae6218973be 289 while(1){
yangtzuli 3:2ae6218973be 290 if(mode==AVOIDANCE){
tomotsugu 6:4817fe7b7654 291 if(flag_a == 0){ // 障害物がない場合
yangtzuli 3:2ae6218973be 292 run = ADVANCE; // 前進
yangtzuli 3:2ae6218973be 293 }
tomotsugu 5:5414f85108aa 294 else{ // 障害物がある場合
yangtzuli 3:2ae6218973be 295 i = 0;
yangtzuli 3:2ae6218973be 296 if(SC < 15){ // 正面15cm以内に障害物が現れた場合
yangtzuli 3:2ae6218973be 297 run = BACK; // 後退
yangtzuli 3:2ae6218973be 298 while(watch() < limit){ // 正面20cm以内に障害物がある間
tomotsugu 6:4817fe7b7654 299 if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
tomotsugu 6:4817fe7b7654 300 break; // avoidanceから抜ける
tomotsugu 6:4817fe7b7654 301 }
yangtzuli 3:2ae6218973be 302 }
yangtzuli 3:2ae6218973be 303 run = STOP; // 停止
yangtzuli 2:38825726cb1b 304 }
yangtzuli 3:2ae6218973be 305 if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
yangtzuli 3:2ae6218973be 306 run = LEFT; // 左折
tomotsugu 6:4817fe7b7654 307 while(i < 10){ // 進行方向確認
tomotsugu 6:4817fe7b7654 308 if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
tomotsugu 6:4817fe7b7654 309 break; // avoidanceから抜ける
tomotsugu 6:4817fe7b7654 310 }
yangtzuli 3:2ae6218973be 311 if(watch() > limit){
yangtzuli 3:2ae6218973be 312 i++;
yangtzuli 3:2ae6218973be 313 }
yangtzuli 3:2ae6218973be 314 else{
yangtzuli 3:2ae6218973be 315 i = 0;
yangtzuli 3:2ae6218973be 316 }
yangtzuli 3:2ae6218973be 317 }
yangtzuli 3:2ae6218973be 318 run = STOP; // 停止
yangtzuli 3:2ae6218973be 319 }
yangtzuli 3:2ae6218973be 320 else { // 全方向以外
yangtzuli 3:2ae6218973be 321 far = SC; // 正面を最も遠い距離に設定
yangtzuli 3:2ae6218973be 322 houkou = 1; // 進行方向を前に設定
yangtzuli 3:2ae6218973be 323 if(far < SLD || far < SL){ // 左または左前がより遠い場合
yangtzuli 3:2ae6218973be 324 if(SL < SLD){ // 左前が左より遠い場合
yangtzuli 3:2ae6218973be 325 far = SLD; // 左前を最も遠い距離に設定
yangtzuli 3:2ae6218973be 326 }
yangtzuli 3:2ae6218973be 327 else{ // 左が左前より遠い場合
yangtzuli 3:2ae6218973be 328 far = SL; // 左を最も遠い距離に設定
yangtzuli 3:2ae6218973be 329 }
yangtzuli 3:2ae6218973be 330 houkou = 2; // 進行方向を左に設定
yangtzuli 2:38825726cb1b 331 }
yangtzuli 3:2ae6218973be 332 if(far < SRD || far < SR){ // 右または右前がより遠い場合
yangtzuli 3:2ae6218973be 333 if(SR < SRD){ // 右前が右より遠い場合
yangtzuli 3:2ae6218973be 334 far = SRD; // 右前を最も遠い距離に設定
yangtzuli 3:2ae6218973be 335 }
yangtzuli 3:2ae6218973be 336 else{ // 右が右前よりも遠い場合
yangtzuli 3:2ae6218973be 337 far = SR; // 右を最も遠い距離に設定
yangtzuli 3:2ae6218973be 338 }
yangtzuli 3:2ae6218973be 339 houkou = 3; // 進行方向を右に設定
yangtzuli 3:2ae6218973be 340 }
yangtzuli 3:2ae6218973be 341 switch(houkou){ // 進行方向の場合分け
yangtzuli 3:2ae6218973be 342 case 1: // 前の場合
yangtzuli 3:2ae6218973be 343 run = ADVANCE; // 前進
tomotsugu 6:4817fe7b7654 344 ThisThread::sleep_for(1000); // 1秒待つ
yangtzuli 3:2ae6218973be 345 break;
yangtzuli 3:2ae6218973be 346 case 2: // 左の場合
yangtzuli 3:2ae6218973be 347 run = LEFT; // 左折
tomotsugu 6:4817fe7b7654 348 while(i < 10){ // 進行方向確認
tomotsugu 6:4817fe7b7654 349 if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
tomotsugu 6:4817fe7b7654 350 break; // avoidanceから抜ける
tomotsugu 6:4817fe7b7654 351 }
yangtzuli 3:2ae6218973be 352 if(watch() > (far - 2)){
yangtzuli 3:2ae6218973be 353 i++;
yangtzuli 3:2ae6218973be 354 }
yangtzuli 3:2ae6218973be 355 else{
yangtzuli 3:2ae6218973be 356 i = 0;
yangtzuli 3:2ae6218973be 357 }
yangtzuli 3:2ae6218973be 358 }
yangtzuli 3:2ae6218973be 359 run = STOP; // 停止
yangtzuli 3:2ae6218973be 360 break;
yangtzuli 3:2ae6218973be 361 case 3: // 右の場合
yangtzuli 3:2ae6218973be 362 run = RIGHT; // 右折
tomotsugu 6:4817fe7b7654 363 while(i < 10){ // 進行方向確認
tomotsugu 6:4817fe7b7654 364 if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
tomotsugu 6:4817fe7b7654 365 braek; // avoidanceから抜ける
tomotsugu 6:4817fe7b7654 366 }
yangtzuli 3:2ae6218973be 367 if(watch() > (far - 2)){
yangtzuli 3:2ae6218973be 368 i++;
yangtzuli 3:2ae6218973be 369 }
yangtzuli 3:2ae6218973be 370 else{
yangtzuli 3:2ae6218973be 371 i = 0;
yangtzuli 3:2ae6218973be 372 }
yangtzuli 3:2ae6218973be 373 }
yangtzuli 3:2ae6218973be 374 run = STOP; // 停止
yangtzuli 3:2ae6218973be 375 break;
yangtzuli 2:38825726cb1b 376 }
yangtzuli 2:38825726cb1b 377 }
tomotsugu 5:5414f85108aa 378 }
tomotsugu 5:5414f85108aa 379 flag_a = 0; // 障害物有無フラグを0にセット
yangtzuli 3:2ae6218973be 380 watchsurrounding();
yangtzuli 3:2ae6218973be 381 }else{
tomotsugu 6:4817fe7b7654 382 ThisThread::sleep_for(1); // 1ms待つ
yangtzuli 3:2ae6218973be 383 }
yangtzuli 2:38825726cb1b 384 }
yangtzuli 2:38825726cb1b 385 }
tomotsugu 5:5414f85108aa 386
yangtzuli 2:38825726cb1b 387 int watch(){
yangtzuli 2:38825726cb1b 388 trig = 0;
yangtzuli 2:38825726cb1b 389 ThisThread::sleep_for(5); // 5ms待つ
yangtzuli 2:38825726cb1b 390 trig = 1;
yangtzuli 2:38825726cb1b 391 ThisThread::sleep_for(15); // 15ms待つ
yangtzuli 2:38825726cb1b 392 trig = 0;
tomotsugu 6:4817fe7b7654 393 while(echo.read() == 0){
tomotsugu 6:4817fe7b7654 394 }
yangtzuli 2:38825726cb1b 395 timer.start(); // 距離計測タイマースタート
yangtzuli 2:38825726cb1b 396 while(echo.read() == 1){
yangtzuli 2:38825726cb1b 397 }
yangtzuli 2:38825726cb1b 398 timer.stop(); // 距離計測タイマーストップ
yangtzuli 2:38825726cb1b 399 DT = timer.read_us()*0.01657; // 距離計算
tomotsugu 6:4817fe7b7654 400 if(DT > 100){ // 検知範囲外なら400cmに設定
tomotsugu 6:4817fe7b7654 401 DT = 100;
yangtzuli 2:38825726cb1b 402 }
yangtzuli 2:38825726cb1b 403 timer.reset(); // 距離計測タイマーリセット
yangtzuli 2:38825726cb1b 404 return DT;
yangtzuli 0:0d0037aabe41 405 }
yangtzuli 0:0d0037aabe41 406
yangtzuli 2:38825726cb1b 407 void watchsurrounding(){
yangtzuli 2:38825726cb1b 408 SC = watch();
yangtzuli 2:38825726cb1b 409 if(SC < limit){ // 正面20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 410 run = STOP; // 停止
yangtzuli 2:38825726cb1b 411 }
yangtzuli 2:38825726cb1b 412 servo.pulsewidth_us(1925); // サーボを左に40度回転
tomotsugu 6:4817fe7b7654 413 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 414 SLD = watch();
yangtzuli 2:38825726cb1b 415 if(SLD < limit){ // 左前20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 416 run = STOP; // 停止
yangtzuli 2:38825726cb1b 417 }
yangtzuli 2:38825726cb1b 418 servo.pulsewidth_us(2400); // サーボを左に90度回転
tomotsugu 6:4817fe7b7654 419 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 420 SL = watch();
yangtzuli 2:38825726cb1b 421 if(SL < limit){ // 左20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 422 run = STOP; // 停止
yangtzuli 2:38825726cb1b 423 }
yangtzuli 2:38825726cb1b 424 servo.pulsewidth_us(1450);
yangtzuli 2:38825726cb1b 425 ThisThread::sleep_for(100);
yangtzuli 2:38825726cb1b 426 servo.pulsewidth_us(925); // サーボを右に40度回転
tomotsugu 6:4817fe7b7654 427 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 428 SRD = watch();
yangtzuli 2:38825726cb1b 429 if(SRD < limit){ // 右前20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 430 run = STOP; // 停止
yangtzuli 2:38825726cb1b 431 }
yangtzuli 2:38825726cb1b 432 servo.pulsewidth_us(500); // サーボを右に90度回転
tomotsugu 6:4817fe7b7654 433 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 434 SR = watch();
yangtzuli 2:38825726cb1b 435 if(SR < limit){ // 右20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 436 run = STOP; // 停止
yangtzuli 2:38825726cb1b 437 }
yangtzuli 2:38825726cb1b 438 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
tomotsugu 6:4817fe7b7654 439 ThisThread::sleep_for(100); // 100ms待つ
yangtzuli 2:38825726cb1b 440 if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
tomotsugu 6:4817fe7b7654 441 flag_a = 1; // フラグに1をセット
yangtzuli 2:38825726cb1b 442 }
yangtzuli 3:2ae6218973be 443 }
yangtzuli 3:2ae6218973be 444
tomotsugu 5:5414f85108aa 445 /* ディスプレイ表示関数 */
yangtzuli 3:2ae6218973be 446 void display(){
yangtzuli 3:2ae6218973be 447 mutex.lock();
yangtzuli 3:2ae6218973be 448 lcd.setAddress(0,1);
yangtzuli 3:2ae6218973be 449 switch(mode){
yangtzuli 3:2ae6218973be 450 case ADVANCE:
yangtzuli 3:2ae6218973be 451 lcd.printf("Mode:Advance ");
yangtzuli 3:2ae6218973be 452 break;
yangtzuli 3:2ae6218973be 453 case RIGHT:
tomotsugu 5:5414f85108aa 454 lcd.printf("Mode:TurnRight ");
yangtzuli 3:2ae6218973be 455 break;
yangtzuli 3:2ae6218973be 456 case LEFT:
tomotsugu 5:5414f85108aa 457 lcd.printf("Mode:TurnLeft ");
yangtzuli 3:2ae6218973be 458 break;
yangtzuli 3:2ae6218973be 459 case BACK:
yangtzuli 3:2ae6218973be 460 lcd.printf("Mode:Back ");
yangtzuli 3:2ae6218973be 461 break;
yangtzuli 3:2ae6218973be 462 case STOP:
yangtzuli 3:2ae6218973be 463 lcd.printf("Mode:Stop ");
yangtzuli 3:2ae6218973be 464 break;
yangtzuli 3:2ae6218973be 465 case READY:
yangtzuli 3:2ae6218973be 466 lcd.printf("Mode:Ready ");
yangtzuli 3:2ae6218973be 467 break;
yangtzuli 3:2ae6218973be 468 case LINE_TRACE:
yangtzuli 3:2ae6218973be 469 lcd.printf("Mode:LineTrace ");
yangtzuli 3:2ae6218973be 470 break;
yangtzuli 3:2ae6218973be 471 case AVOIDANCE:
yangtzuli 3:2ae6218973be 472 lcd.setAddress(0,1);
yangtzuli 3:2ae6218973be 473 lcd.printf("Mode:Avoidance ");
yangtzuli 3:2ae6218973be 474 break;
yangtzuli 3:2ae6218973be 475 case SPEED:
yangtzuli 3:2ae6218973be 476 switch(flag_sp){
yangtzuli 3:2ae6218973be 477 case(NORMAL):
yangtzuli 3:2ae6218973be 478 lcd.printf("Speed:Normal ");
yangtzuli 3:2ae6218973be 479 break;
yangtzuli 3:2ae6218973be 480 case(FAST):
yangtzuli 3:2ae6218973be 481 lcd.printf("Speed:Fast ");
yangtzuli 3:2ae6218973be 482 break;
yangtzuli 3:2ae6218973be 483 case(VERYFAST):
yangtzuli 3:2ae6218973be 484 lcd.printf("Speed:VeryFast ");
yangtzuli 3:2ae6218973be 485 break;
yangtzuli 3:2ae6218973be 486 }
yangtzuli 3:2ae6218973be 487 viewTimer.reset();
yangtzuli 3:2ae6218973be 488 viewTimer.start();
yangtzuli 3:2ae6218973be 489 break;
yangtzuli 3:2ae6218973be 490 }
yangtzuli 3:2ae6218973be 491 mutex.unlock();
yangtzuli 3:2ae6218973be 492 }
yangtzuli 3:2ae6218973be 493
yangtzuli 3:2ae6218973be 494 void lcdBacklight(void const *argument){
yangtzuli 3:2ae6218973be 495 if(flag_b == 1){
yangtzuli 3:2ae6218973be 496 lcd.setBacklight(TextLCD::LightOn);
yangtzuli 3:2ae6218973be 497 }else{
yangtzuli 3:2ae6218973be 498 lcd.setBacklight(TextLCD::LightOff);
yangtzuli 3:2ae6218973be 499 }
yangtzuli 3:2ae6218973be 500 flag_b = !flag_b;
yangtzuli 3:2ae6218973be 501 }
yangtzuli 2:38825726cb1b 502
tomotsugu 5:5414f85108aa 503 void bChange(){
yangtzuli 3:2ae6218973be 504 lcd.setBacklight(TextLCD::LightOn);
yangtzuli 3:2ae6218973be 505 while(1){
yangtzuli 4:3f80c0180e2f 506 b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10;
yangtzuli 4:3f80c0180e2f 507 if(b < 0){//すべての機能停止(今はなし)
yangtzuli 4:3f80c0180e2f 508 /*lcd.setBacklight(TextLCD::LightOff);
yangtzuli 3:2ae6218973be 509 bTimer.stop();
yangtzuli 4:3f80c0180e2f 510 exit(1);*/
yangtzuli 4:3f80c0180e2f 511 b = 0;
tomotsugu 5:5414f85108aa 512 }
tomotsugu 5:5414f85108aa 513 else if(b > 100){
yangtzuli 3:2ae6218973be 514 b = 100;
yangtzuli 3:2ae6218973be 515 }
yangtzuli 3:2ae6218973be 516 mutex.lock();
yangtzuli 3:2ae6218973be 517 lcd.setAddress(0,0);
yangtzuli 2:38825726cb1b 518 lcd.printf("Battery:%3d%%",b);
yangtzuli 3:2ae6218973be 519 mutex.unlock();
yangtzuli 3:2ae6218973be 520 if(b <= 30){
yangtzuli 3:2ae6218973be 521 if(flag_t == 0){
yangtzuli 3:2ae6218973be 522 bTimer.start(500);
yangtzuli 3:2ae6218973be 523 flag_t = 1;
yangtzuli 3:2ae6218973be 524 }
tomotsugu 5:5414f85108aa 525 }
tomotsugu 5:5414f85108aa 526 else{
yangtzuli 3:2ae6218973be 527 if(flag_t == 1){
yangtzuli 3:2ae6218973be 528 bTimer.stop();
yangtzuli 3:2ae6218973be 529 lcd.setBacklight(TextLCD::LightOn);
yangtzuli 3:2ae6218973be 530 flag_t = 0;
yangtzuli 3:2ae6218973be 531 }
yangtzuli 3:2ae6218973be 532 }
yangtzuli 2:38825726cb1b 533 }
yangtzuli 4:3f80c0180e2f 534 ThisThread::sleep_for(500);
yangtzuli 2:38825726cb1b 535 }
tomotsugu 5:5414f85108aa 536
yangtzuli 0:0d0037aabe41 537 int main() {
yangtzuli 3:2ae6218973be 538 mode = READY;
yangtzuli 3:2ae6218973be 539 beforeMode = READY;
yangtzuli 3:2ae6218973be 540 run = STOP;
yangtzuli 3:2ae6218973be 541 flag_sp = NORMAL;
yangtzuli 4:3f80c0180e2f 542 display();
yangtzuli 0:0d0037aabe41 543 while(1){
yangtzuli 4:3f80c0180e2f 544 bChange();
yangtzuli 4:3f80c0180e2f 545 ThisThread::sleep_for(1);
yangtzuli 0:0d0037aabe41 546 }
yangtzuli 0:0d0037aabe41 547 }