mbed_robotcar / Mbed OS robotcarWifi3-demo-0827

ボタンを押すと、 バッテリ更新を停止し、 他のボタンもロックさせる

Dependencies:   RemoteIR TextLCD

main.cpp@7:19ebd0851f49, 2020-07-30 (annotated)

Committer:
yangtzuli
Date:
Thu Jul 30 04:40:26 2020 +0000
Revision:
7:19ebd0851f49
Parent:
6:800a745c7f2e
Child:
8:a47dbf4fa455
test finish

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
yangtzuli 7:19ebd0851f49 18 #define LOW 0
yangtzuli 7:19ebd0851f49 19 #define HIGH 1
yangtzuli 3:2ae6218973be 20 #define NORMAL 0 // フラグ値
yangtzuli 3:2ae6218973be 21 #define FAST 1 // フラグ値
yangtzuli 3:2ae6218973be 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 5:3fffb364744b 39 DigitalIn 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; // 前のモード
yangtzuli 3:2ae6218973be 58 int flag_sp = 0; // スピード変化フラグ
yangtzuli 3:2ae6218973be 59 Timer viewTimer; // スピ―ド変更時に3秒計測
yangtzuli 6:800a745c7f2e 60 float motorSpeed[9] = {0.7, 0.8, 0.9, 0.75, 0.85, 0.95, 0.8, 0.9, 1.0};
yangtzuli 3:2ae6218973be 61 // モーター速度設定(後半はライントレース用)
yangtzuli 3:2ae6218973be 62 Mutex mutex;
yangtzuli 3:2ae6218973be 63 // ポートp15を赤外線受信モジュールの接続先に指定
yangtzuli 3:2ae6218973be 64 RemoteIR::Format format;
yangtzuli 3:2ae6218973be 65 uint8_t buf[32];
yangtzuli 3:2ae6218973be 66 uint32_t bitcount;
yangtzuli 3:2ae6218973be 67 uint32_t code;
yangtzuli 3:2ae6218973be 68
yangtzuli 3:2ae6218973be 69 //bChangeのいろいろ
yangtzuli 3:2ae6218973be 70 TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
yangtzuli 3:2ae6218973be 71 int b = 0;
yangtzuli 3:2ae6218973be 72 double test;
yangtzuli 3:2ae6218973be 73 int flag_b = 0;
yangtzuli 3:2ae6218973be 74 int flag_t = 0;
yangtzuli 3:2ae6218973be 75
yangtzuli 3:2ae6218973be 76 // ライントレース
yangtzuli 6:800a745c7f2e 77 int sensArray[32] = {1,6,2,4,1,1,2,1, // ライントレースセンサパターン
yangtzuli 6:800a745c7f2e 78 3,1,1,1,3,1,1,1,
yangtzuli 6:800a745c7f2e 79 7,1,1,1,1,1,1,1,
yangtzuli 6:800a745c7f2e 80 5,1,1,1,1,1,1,1};
yangtzuli 0:0d0037aabe41 81
yangtzuli 2:38825726cb1b 82 /* 障害物検知用の変数設定 */
yangtzuli 3:2ae6218973be 83 Timer timer; // 距離計測用タイマ
yangtzuli 3:2ae6218973be 84 int DT; // 距離
yangtzuli 2:38825726cb1b 85 int SC; // 正面
yangtzuli 2:38825726cb1b 86 int SL; // 左
yangtzuli 2:38825726cb1b 87 int SR; // 右
yangtzuli 2:38825726cb1b 88 int SLD; // 左前
yangtzuli 2:38825726cb1b 89 int SRD; // 右前
yangtzuli 5:3fffb364744b 90 int flag_a = 0; // 障害物有無のフラグ
yangtzuli 2:38825726cb1b 91 const int limit = 20; // 障害物の距離のリミット(単位：cm)
yangtzuli 3:2ae6218973be 92 int far; // 最も遠い距離
yangtzuli 2:38825726cb1b 93 int houkou; // 進行方向(1:前 2:左 3:右)
yangtzuli 2:38825726cb1b 94 int i; // ループ変数
yangtzuli 2:38825726cb1b 95
yangtzuli 3:2ae6218973be 96
yangtzuli 3:2ae6218973be 97
yangtzuli 3:2ae6218973be 98 /* プロトタイプ宣言 */
yangtzuli 3:2ae6218973be 99 void decodeIR(void const *argument);
yangtzuli 3:2ae6218973be 100 void motor(void const *argument);
yangtzuli 3:2ae6218973be 101 void changeSpeed();
yangtzuli 2:38825726cb1b 102 void avoidance(void const *argument);
yangtzuli 3:2ae6218973be 103 void trace(void const *argument);
yangtzuli 2:38825726cb1b 104 void watchsurrounding();
yangtzuli 2:38825726cb1b 105 int watch();
yangtzuli 5:3fffb364744b 106 void bChange();
yangtzuli 3:2ae6218973be 107 void display();
yangtzuli 3:2ae6218973be 108 void lcdBacklight(void const *argument);
yangtzuli 4:3f80c0180e2f 109 Thread thread1(decodeIR, NULL, osPriorityRealtime);//+3
yangtzuli 4:3f80c0180e2f 110 Thread thread2(motor, NULL, osPriorityHigh);//+2
yangtzuli 4:3f80c0180e2f 111 Thread thread3(avoidance, NULL, osPriorityHigh);//+2
yangtzuli 4:3f80c0180e2f 112 Thread thread4(trace, NULL, osPriorityHigh);//+2
yangtzuli 3:2ae6218973be 113 RtosTimer bTimer(lcdBacklight, osTimerPeriodic);
yangtzuli 2:38825726cb1b 114
yangtzuli 5:3fffb364744b 115 DigitalOut led1(LED1);
yangtzuli 3:2ae6218973be 116
yangtzuli 5:3fffb364744b 117
yangtzuli 5:3fffb364744b 118 void decodeIR(void const *argument){
yangtzuli 5:3fffb364744b 119 while(1){
yangtzuli 0:0d0037aabe41 120 // 受信待ち
yangtzuli 4:3f80c0180e2f 121 pc.printf("decodeIR\r\n");
yangtzuli 3:2ae6218973be 122 if (ir.getState() == ReceiverIR::Received) {
yangtzuli 0:0d0037aabe41 123 // コード受信
yangtzuli 3:2ae6218973be 124 bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
yangtzuli 0:0d0037aabe41 125 if(bitcount > 1){
yangtzuli 0:0d0037aabe41 126 // 受信成功
yangtzuli 1:5bb497a38344 127 code=0;
yangtzuli 1:5bb497a38344 128 for(int j=0;j<4;j++){
yangtzuli 1:5bb497a38344 129 code+=(buf[j]<<(8*(3-j)));
yangtzuli 1:5bb497a38344 130 }
yangtzuli 3:2ae6218973be 131 if(mode!=SPEED){
yangtzuli 3:2ae6218973be 132 beforeMode=mode;
yangtzuli 3:2ae6218973be 133 }
yangtzuli 3:2ae6218973be 134
yangtzuli 3:2ae6218973be 135 //pc.printf("%0x\r\n",code);
yangtzuli 0:0d0037aabe41 136 switch(code){
yangtzuli 1:5bb497a38344 137 case 0x40bf27d8://クイック
yangtzuli 3:2ae6218973be 138 pc.printf("mode = SPEED\r\n");
yangtzuli 3:2ae6218973be 139 mode = SPEED;
yangtzuli 4:3f80c0180e2f 140 changeSpeed();
yangtzuli 4:3f80c0180e2f 141 display();
yangtzuli 4:3f80c0180e2f 142 mode = beforeMode;
yangtzuli 1:5bb497a38344 143 break;
yangtzuli 1:5bb497a38344 144 case 0x40be34cb://レグザリンク
yangtzuli 6:800a745c7f2e 145 pc.printf("mode = LINE_TRACE\r\n");
yangtzuli 3:2ae6218973be 146 mode=LINE_TRACE;
yangtzuli 3:2ae6218973be 147 display();
yangtzuli 1:5bb497a38344 148 break;
yangtzuli 1:5bb497a38344 149 case 0x40bf6e91://番組表
yangtzuli 3:2ae6218973be 150 pc.printf("mode = AVOIDANCE\r\n");
yangtzuli 3:2ae6218973be 151 mode=AVOIDANCE;
yangtzuli 3:2ae6218973be 152 display();
yangtzuli 1:5bb497a38344 153 break;
yangtzuli 1:5bb497a38344 154 case 0x40bf3ec1://↑
yangtzuli 3:2ae6218973be 155 pc.printf("mode = ADVANCE\r\n");
yangtzuli 3:2ae6218973be 156 mode = ADVANCE;
yangtzuli 3:2ae6218973be 157 run = ADVANCE;
yangtzuli 3:2ae6218973be 158 display();
yangtzuli 1:5bb497a38344 159 break;
yangtzuli 1:5bb497a38344 160 case 0x40bf3fc0://↓
yangtzuli 3:2ae6218973be 161 pc.printf("mode = BACK\r\n");
yangtzuli 3:2ae6218973be 162 mode = BACK;
yangtzuli 3:2ae6218973be 163 run = BACK;
yangtzuli 3:2ae6218973be 164 display();
yangtzuli 0:0d0037aabe41 165 break;
yangtzuli 1:5bb497a38344 166 case 0x40bf5fa0://←
yangtzuli 3:2ae6218973be 167 pc.printf("mode = LEFT\r\n");
yangtzuli 3:2ae6218973be 168 mode = LEFT;
yangtzuli 3:2ae6218973be 169 run = LEFT;
yangtzuli 3:2ae6218973be 170 display();
yangtzuli 1:5bb497a38344 171 break;
yangtzuli 1:5bb497a38344 172 case 0x40bf5ba4://→
yangtzuli 3:2ae6218973be 173 pc.printf("mode = RIGHT\r\n");
yangtzuli 3:2ae6218973be 174 mode = RIGHT;
yangtzuli 3:2ae6218973be 175 run = RIGHT;
yangtzuli 3:2ae6218973be 176 display();
yangtzuli 1:5bb497a38344 177 break;
yangtzuli 1:5bb497a38344 178 case 0x40bf3dc2://決定
yangtzuli 3:2ae6218973be 179 pc.printf("mode = STOP\r\n");
yangtzuli 3:2ae6218973be 180 mode = STOP;
yangtzuli 3:2ae6218973be 181 run = STOP;
yangtzuli 3:2ae6218973be 182 display();
yangtzuli 1:5bb497a38344 183 break;
yangtzuli 0:0d0037aabe41 184 default:
yangtzuli 0:0d0037aabe41 185 ;
yangtzuli 0:0d0037aabe41 186 }
yangtzuli 0:0d0037aabe41 187 }
yangtzuli 0:0d0037aabe41 188 }
yangtzuli 4:3f80c0180e2f 189 if(viewTimer.read_ms()>=3000){
yangtzuli 4:3f80c0180e2f 190 viewTimer.stop();
yangtzuli 4:3f80c0180e2f 191 viewTimer.reset();
yangtzuli 4:3f80c0180e2f 192 display();
yangtzuli 4:3f80c0180e2f 193 }
yangtzuli 4:3f80c0180e2f 194 ThisThread::sleep_for(90);
yangtzuli 4:3f80c0180e2f 195 }
yangtzuli 2:38825726cb1b 196 }
yangtzuli 2:38825726cb1b 197 void motor(void const *argument){
yangtzuli 3:2ae6218973be 198 while(true){
yangtzuli 3:2ae6218973be 199 pc.printf("motor\r\n");
yangtzuli 3:2ae6218973be 200 switch(run){
yangtzuli 3:2ae6218973be 201 case ADVANCE:
yangtzuli 3:2ae6218973be 202 motorR1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 203 motorR2 = LOW;
yangtzuli 3:2ae6218973be 204 motorL1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 205 motorL2 = LOW;
yangtzuli 3:2ae6218973be 206 break;
yangtzuli 3:2ae6218973be 207 case RIGHT:
yangtzuli 3:2ae6218973be 208 motorR1 = LOW;
yangtzuli 3:2ae6218973be 209 motorR2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 210 motorL1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 211 motorL2 = LOW;
yangtzuli 3:2ae6218973be 212 break;
yangtzuli 3:2ae6218973be 213 case LEFT:
yangtzuli 3:2ae6218973be 214 motorR1 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 215 motorR2 = LOW;
yangtzuli 3:2ae6218973be 216 motorL1 = LOW;
yangtzuli 3:2ae6218973be 217 motorL2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 218 break;
yangtzuli 3:2ae6218973be 219 case BACK:
yangtzuli 3:2ae6218973be 220 motorR1 = LOW;
yangtzuli 3:2ae6218973be 221 motorR2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 222 motorL1 = LOW;
yangtzuli 3:2ae6218973be 223 motorL2 = motorSpeed[flag_sp];
yangtzuli 3:2ae6218973be 224 break;
yangtzuli 3:2ae6218973be 225 case STOP:
yangtzuli 3:2ae6218973be 226 motorR1 = LOW;
yangtzuli 3:2ae6218973be 227 motorR2 = LOW;
yangtzuli 3:2ae6218973be 228 motorL1 = LOW;
yangtzuli 3:2ae6218973be 229 motorL2 = LOW;
yangtzuli 3:2ae6218973be 230 break;
yangtzuli 3:2ae6218973be 231 }
yangtzuli 6:800a745c7f2e 232 if(flag_sp > VERYFAST){
yangtzuli 6:800a745c7f2e 233 flag_sp -= 3 * (flag_sp / 3);
yangtzuli 6:800a745c7f2e 234 }
yangtzuli 3:2ae6218973be 235 pc.printf(" motor\r\n");
yangtzuli 4:3f80c0180e2f 236 ThisThread::sleep_for(30);
yangtzuli 2:38825726cb1b 237 }
yangtzuli 2:38825726cb1b 238 }
yangtzuli 3:2ae6218973be 239 void changeSpeed(){
yangtzuli 6:800a745c7f2e 240 if(flag_sp%3 == 2){
yangtzuli 6:800a745c7f2e 241 flag_sp -= 2;
yangtzuli 3:2ae6218973be 242
yangtzuli 3:2ae6218973be 243 }else{
yangtzuli 3:2ae6218973be 244 flag_sp = flag_sp + 1;
yangtzuli 3:2ae6218973be 245 }
yangtzuli 3:2ae6218973be 246 }
yangtzuli 3:2ae6218973be 247 void trace(void const *argument){
yangtzuli 3:2ae6218973be 248 while(true){
yangtzuli 3:2ae6218973be 249 if(mode==LINE_TRACE){
yangtzuli 6:800a745c7f2e 250 //pc.printf("line trace\r\n");
yangtzuli 3:2ae6218973be 251 // センサー値読み取り
yangtzuli 3:2ae6218973be 252 int sensor1 = ss1;
yangtzuli 3:2ae6218973be 253 int sensor2 = ss2;
yangtzuli 3:2ae6218973be 254 int sensor3 = ss3;
yangtzuli 3:2ae6218973be 255 int sensor4 = ss4;
yangtzuli 3:2ae6218973be 256 int sensor5 = ss5;
yangtzuli 6:800a745c7f2e 257 pc.printf("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5);
yangtzuli 3:2ae6218973be 258 int sensD = 0;
yangtzuli 3:2ae6218973be 259 int sensorNum;
yangtzuli 3:2ae6218973be 260
yangtzuli 3:2ae6218973be 261 if(sensor1 > 0) sensD |= 0x10;
yangtzuli 3:2ae6218973be 262 if(sensor2 > 0) sensD |= 0x08;
yangtzuli 3:2ae6218973be 263 if(sensor3 > 0) sensD |= 0x04;
yangtzuli 3:2ae6218973be 264 if(sensor4 > 0) sensD |= 0x02;
yangtzuli 3:2ae6218973be 265 if(sensor5 > 0) sensD |= 0x01;
yangtzuli 3:2ae6218973be 266
yangtzuli 3:2ae6218973be 267 sensorNum = sensArray[sensD];
yangtzuli 3:2ae6218973be 268
yangtzuli 3:2ae6218973be 269 switch(sensorNum){
yangtzuli 3:2ae6218973be 270 case 1:
yangtzuli 7:19ebd0851f49 271 if(mode==LINE_TRACE){
yangtzuli 7:19ebd0851f49 272 run = ADVANCE;
yangtzuli 7:19ebd0851f49 273 }
yangtzuli 6:800a745c7f2e 274 pc.printf("ADVANCE");
yangtzuli 3:2ae6218973be 275 break;
yangtzuli 3:2ae6218973be 276 case 2:
yangtzuli 7:19ebd0851f49 277 if(mode==LINE_TRACE){
yangtzuli 7:19ebd0851f49 278 run = RIGHT;
yangtzuli 7:19ebd0851f49 279 }
yangtzuli 6:800a745c7f2e 280 pc.printf("RIGHT");
yangtzuli 3:2ae6218973be 281 break;
yangtzuli 3:2ae6218973be 282 case 3:
yangtzuli 7:19ebd0851f49 283 if(mode==LINE_TRACE){
yangtzuli 7:19ebd0851f49 284 run = LEFT;
yangtzuli 7:19ebd0851f49 285 }
yangtzuli 6:800a745c7f2e 286 pc.printf("LEFT");
yangtzuli 3:2ae6218973be 287 break;
yangtzuli 3:2ae6218973be 288 case 4:
yangtzuli 7:19ebd0851f49 289 if(mode==LINE_TRACE){
yangtzuli 7:19ebd0851f49 290 flag_sp %= 3 + 3;
yangtzuli 7:19ebd0851f49 291 run = RIGHT;
yangtzuli 7:19ebd0851f49 292 }
yangtzuli 6:800a745c7f2e 293 pc.printf("RIGHT");
yangtzuli 3:2ae6218973be 294 break;
yangtzuli 3:2ae6218973be 295 case 5:
yangtzuli 7:19ebd0851f49 296 if(mode==LINE_TRACE){
yangtzuli 7:19ebd0851f49 297 flag_sp %= 3 + 3;
yangtzuli 7:19ebd0851f49 298 run = LEFT;
yangtzuli 7:19ebd0851f49 299 }
yangtzuli 6:800a745c7f2e 300 pc.printf("LEFT");
yangtzuli 6:800a745c7f2e 301 break;
yangtzuli 6:800a745c7f2e 302 case 6:
yangtzuli 7:19ebd0851f49 303 if(mode==LINE_TRACE){
yangtzuli 7:19ebd0851f49 304 flag_sp %= 3 + 6;
yangtzuli 7:19ebd0851f49 305 run = RIGHT;
yangtzuli 7:19ebd0851f49 306 }
yangtzuli 6:800a745c7f2e 307 pc.printf("RIGHT");
yangtzuli 6:800a745c7f2e 308 break;
yangtzuli 6:800a745c7f2e 309 case 7:
yangtzuli 7:19ebd0851f49 310 if(mode==LINE_TRACE){
yangtzuli 7:19ebd0851f49 311 flag_sp %= 3 + 6;
yangtzuli 7:19ebd0851f49 312 run = LEFT;
yangtzuli 7:19ebd0851f49 313 }
yangtzuli 6:800a745c7f2e 314 pc.printf("LEFT");
yangtzuli 3:2ae6218973be 315 break;
yangtzuli 3:2ae6218973be 316 }
yangtzuli 6:800a745c7f2e 317 //pc.printf(" line trace\r\n");
yangtzuli 4:3f80c0180e2f 318 ThisThread::sleep_for(30);
yangtzuli 3:2ae6218973be 319 }else{
yangtzuli 4:3f80c0180e2f 320 ThisThread::sleep_for(1);
yangtzuli 3:2ae6218973be 321 }
yangtzuli 3:2ae6218973be 322 }
yangtzuli 3:2ae6218973be 323 }
yangtzuli 3:2ae6218973be 324
yangtzuli 2:38825726cb1b 325 /* 障害物回避走行 */
yangtzuli 2:38825726cb1b 326 void avoidance(void const *argument){
yangtzuli 3:2ae6218973be 327 while(1){
yangtzuli 3:2ae6218973be 328 if(mode==AVOIDANCE){
yangtzuli 5:3fffb364744b 329 pc.printf("avoidance\r\n");
yangtzuli 7:19ebd0851f49 330 watchsurrounding();
yangtzuli 7:19ebd0851f49 331 pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR); a:
yangtzuli 5:3fffb364744b 332
yangtzuli 5:3fffb364744b 333 if(flag_a == 0){ // 障害物がない場合
yangtzuli 7:19ebd0851f49 334 if(mode==AVOIDANCE)
yangtzuli 7:19ebd0851f49 335 run = ADVANCE; // 前進
yangtzuli 3:2ae6218973be 336 }
yangtzuli 5:3fffb364744b 337 else{ // 障害物がある場合
yangtzuli 3:2ae6218973be 338 i = 0;
yangtzuli 3:2ae6218973be 339 if(SC < 15){ // 正面15cm以内に障害物が現れた場合
yangtzuli 3:2ae6218973be 340 run = BACK; // 後退
yangtzuli 3:2ae6218973be 341 while(watch() < limit){ // 正面20cm以内に障害物がある間
yangtzuli 5:3fffb364744b 342 if(mode!=AVOIDANCE){
yangtzuli 5:3fffb364744b 343 break;
yangtzuli 5:3fffb364744b 344 }
yangtzuli 3:2ae6218973be 345 }
yangtzuli 3:2ae6218973be 346 run = STOP; // 停止
yangtzuli 2:38825726cb1b 347 }
yangtzuli 3:2ae6218973be 348 if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
yangtzuli 3:2ae6218973be 349 run = LEFT; // 左折
yangtzuli 5:3fffb364744b 350 while(i < 10){ // 進行方向確認
yangtzuli 5:3fffb364744b 351 if(mode!=AVOIDANCE){
yangtzuli 5:3fffb364744b 352 break;
yangtzuli 5:3fffb364744b 353 }
yangtzuli 3:2ae6218973be 354 if(watch() > limit){
yangtzuli 5:3fffb364744b 355 i++;
yangtzuli 5:3fffb364744b 356
yangtzuli 3:2ae6218973be 357 }
yangtzuli 3:2ae6218973be 358 else{
yangtzuli 3:2ae6218973be 359 i = 0;
yangtzuli 3:2ae6218973be 360 }
yangtzuli 3:2ae6218973be 361 }
yangtzuli 3:2ae6218973be 362 run = STOP; // 停止
yangtzuli 3:2ae6218973be 363 }
yangtzuli 3:2ae6218973be 364 else { // 全方向以外
yangtzuli 3:2ae6218973be 365 far = SC; // 正面を最も遠い距離に設定
yangtzuli 3:2ae6218973be 366 houkou = 1; // 進行方向を前に設定
yangtzuli 3:2ae6218973be 367 if(far < SLD || far < SL){ // 左または左前がより遠い場合
yangtzuli 3:2ae6218973be 368 if(SL < SLD){ // 左前が左より遠い場合
yangtzuli 3:2ae6218973be 369 far = SLD; // 左前を最も遠い距離に設定
yangtzuli 3:2ae6218973be 370 }
yangtzuli 3:2ae6218973be 371 else{ // 左が左前より遠い場合
yangtzuli 3:2ae6218973be 372 far = SL; // 左を最も遠い距離に設定
yangtzuli 3:2ae6218973be 373 }
yangtzuli 3:2ae6218973be 374 houkou = 2; // 進行方向を左に設定
yangtzuli 2:38825726cb1b 375 }
yangtzuli 3:2ae6218973be 376 if(far < SRD || far < SR){ // 右または右前がより遠い場合
yangtzuli 3:2ae6218973be 377 if(SR < SRD){ // 右前が右より遠い場合
yangtzuli 3:2ae6218973be 378 far = SRD; // 右前を最も遠い距離に設定
yangtzuli 3:2ae6218973be 379 }
yangtzuli 3:2ae6218973be 380 else{ // 右が右前よりも遠い場合
yangtzuli 3:2ae6218973be 381 far = SR; // 右を最も遠い距離に設定
yangtzuli 3:2ae6218973be 382 }
yangtzuli 3:2ae6218973be 383 houkou = 3; // 進行方向を右に設定
yangtzuli 3:2ae6218973be 384 }
yangtzuli 3:2ae6218973be 385 switch(houkou){ // 進行方向の場合分け
yangtzuli 3:2ae6218973be 386 case 1: // 前の場合
yangtzuli 3:2ae6218973be 387 run = ADVANCE; // 前進
yangtzuli 5:3fffb364744b 388 pc.printf("Advance\r\n");
yangtzuli 3:2ae6218973be 389 thread_sleep_for(100); // 1秒待つ
yangtzuli 3:2ae6218973be 390 break;
yangtzuli 3:2ae6218973be 391 case 2: // 左の場合
yangtzuli 3:2ae6218973be 392 run = LEFT; // 左折
yangtzuli 5:3fffb364744b 393 while(i < 10){ // 進行方向確認
yangtzuli 5:3fffb364744b 394 pc.printf("left\r\n");
yangtzuli 5:3fffb364744b 395 if(mode!=AVOIDANCE){
yangtzuli 5:3fffb364744b 396 break;
yangtzuli 5:3fffb364744b 397 }
yangtzuli 3:2ae6218973be 398 if(watch() > (far - 2)){
yangtzuli 3:2ae6218973be 399 i++;
yangtzuli 5:3fffb364744b 400
yangtzuli 3:2ae6218973be 401 }
yangtzuli 3:2ae6218973be 402 else{
yangtzuli 3:2ae6218973be 403 i = 0;
yangtzuli 3:2ae6218973be 404 }
yangtzuli 3:2ae6218973be 405 }
yangtzuli 3:2ae6218973be 406 run = STOP; // 停止
yangtzuli 3:2ae6218973be 407 break;
yangtzuli 3:2ae6218973be 408 case 3: // 右の場合
yangtzuli 3:2ae6218973be 409 run = RIGHT; // 右折
yangtzuli 5:3fffb364744b 410 while(i < 10){ // 進行方向確認
yangtzuli 5:3fffb364744b 411 pc.printf("right\r\n");
yangtzuli 5:3fffb364744b 412 if(mode!=AVOIDANCE){
yangtzuli 5:3fffb364744b 413 break;
yangtzuli 5:3fffb364744b 414 }
yangtzuli 3:2ae6218973be 415 if(watch() > (far - 2)){
yangtzuli 3:2ae6218973be 416 i++;
yangtzuli 5:3fffb364744b 417
yangtzuli 3:2ae6218973be 418 }
yangtzuli 3:2ae6218973be 419 else{
yangtzuli 3:2ae6218973be 420 i = 0;
yangtzuli 3:2ae6218973be 421 }
yangtzuli 3:2ae6218973be 422 }
yangtzuli 3:2ae6218973be 423 run = STOP; // 停止
yangtzuli 3:2ae6218973be 424 break;
yangtzuli 2:38825726cb1b 425 }
yangtzuli 2:38825726cb1b 426 }
yangtzuli 5:3fffb364744b 427 }
yangtzuli 5:3fffb364744b 428 pc.printf("こんにちは！\r\n");
yangtzuli 5:3fffb364744b 429 flag_a = 0; // フラグを0にセット
yangtzuli 3:2ae6218973be 430 pc.printf(" avoidance\r\n");
yangtzuli 3:2ae6218973be 431 }else{
yangtzuli 4:3f80c0180e2f 432 ThisThread::sleep_for(1);
yangtzuli 3:2ae6218973be 433 }
yangtzuli 2:38825726cb1b 434 }
yangtzuli 2:38825726cb1b 435 }
yangtzuli 2:38825726cb1b 436 int watch(){
yangtzuli 6:800a745c7f2e 437
yangtzuli 2:38825726cb1b 438 pc.printf("watch\r\n");
yangtzuli 2:38825726cb1b 439 trig = 0;
yangtzuli 2:38825726cb1b 440 ThisThread::sleep_for(5); // 5ms待つ
yangtzuli 2:38825726cb1b 441 trig = 1;
yangtzuli 2:38825726cb1b 442 ThisThread::sleep_for(15); // 15ms待つ
yangtzuli 2:38825726cb1b 443 trig = 0;
yangtzuli 5:3fffb364744b 444 while(echo.read() == 0){
yangtzuli 5:3fffb364744b 445 led1=1;
yangtzuli 5:3fffb364744b 446 if(mode!=AVOIDANCE){
yangtzuli 5:3fffb364744b 447 break;
yangtzuli 5:3fffb364744b 448 }
yangtzuli 5:3fffb364744b 449 }
yangtzuli 2:38825726cb1b 450 timer.start(); // 距離計測タイマースタート
yangtzuli 2:38825726cb1b 451 while(echo.read() == 1){
yangtzuli 2:38825726cb1b 452 }
yangtzuli 2:38825726cb1b 453 timer.stop(); // 距離計測タイマーストップ
yangtzuli 5:3fffb364744b 454 DT = (int)(timer.read_us()*0.01657); // 距離計算
yangtzuli 5:3fffb364744b 455 if(DT > 100){ // 検知範囲外なら400cmに設定
yangtzuli 5:3fffb364744b 456 DT = 100;
yangtzuli 2:38825726cb1b 457 }
yangtzuli 5:3fffb364744b 458
yangtzuli 2:38825726cb1b 459 timer.reset(); // 距離計測タイマーリセット
yangtzuli 3:2ae6218973be 460 //ThisThread::sleep_for(30); // 30ms待つ
yangtzuli 5:3fffb364744b 461 pc.printf("私はDTである。%d\r\n",DT);
yangtzuli 6:800a745c7f2e 462 led1=0;
yangtzuli 2:38825726cb1b 463 return DT;
yangtzuli 0:0d0037aabe41 464 }
yangtzuli 0:0d0037aabe41 465
yangtzuli 2:38825726cb1b 466 void watchsurrounding(){
yangtzuli 2:38825726cb1b 467 pc.printf("watchsurrounding\r\n");
yangtzuli 2:38825726cb1b 468 SC = watch();
yangtzuli 2:38825726cb1b 469 if(SC < limit){ // 正面20cm以内に障害物がある場合
yangtzuli 7:19ebd0851f49 470 if(mode==AVOIDANCE)
yangtzuli 7:19ebd0851f49 471 run = STOP; // 停止
yangtzuli 7:19ebd0851f49 472 else
yangtzuli 7:19ebd0851f49 473 return;
yangtzuli 2:38825726cb1b 474 }
yangtzuli 2:38825726cb1b 475 servo.pulsewidth_us(1925); // サーボを左に40度回転
yangtzuli 2:38825726cb1b 476 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 477 SLD = watch();
yangtzuli 2:38825726cb1b 478 if(SLD < limit){ // 左前20cm以内に障害物がある場合
yangtzuli 7:19ebd0851f49 479 if(mode==AVOIDANCE)
yangtzuli 7:19ebd0851f49 480 run = STOP; // 停止
yangtzuli 7:19ebd0851f49 481 else
yangtzuli 7:19ebd0851f49 482 return;
yangtzuli 2:38825726cb1b 483 }
yangtzuli 2:38825726cb1b 484 servo.pulsewidth_us(2400); // サーボを左に90度回転
yangtzuli 2:38825726cb1b 485 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 486 SL = watch();
yangtzuli 2:38825726cb1b 487 if(SL < limit){ // 左20cm以内に障害物がある場合
yangtzuli 7:19ebd0851f49 488 if(mode==AVOIDANCE)
yangtzuli 7:19ebd0851f49 489 run = STOP; // 停止
yangtzuli 7:19ebd0851f49 490 else
yangtzuli 7:19ebd0851f49 491 return;
yangtzuli 2:38825726cb1b 492 }
yangtzuli 2:38825726cb1b 493 servo.pulsewidth_us(1450);
yangtzuli 2:38825726cb1b 494 ThisThread::sleep_for(100);
yangtzuli 2:38825726cb1b 495 servo.pulsewidth_us(925); // サーボを右に40度回転
yangtzuli 2:38825726cb1b 496 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 497 SRD = watch();
yangtzuli 2:38825726cb1b 498 if(SRD < limit){ // 右前20cm以内に障害物がある場合
yangtzuli 7:19ebd0851f49 499 if(mode==AVOIDANCE)
yangtzuli 7:19ebd0851f49 500 run = STOP; // 停止
yangtzuli 7:19ebd0851f49 501 else
yangtzuli 7:19ebd0851f49 502 return;
yangtzuli 2:38825726cb1b 503 }
yangtzuli 2:38825726cb1b 504 servo.pulsewidth_us(500); // サーボを右に90度回転
yangtzuli 2:38825726cb1b 505 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 506 SR = watch();
yangtzuli 2:38825726cb1b 507 if(SR < limit){ // 右20cm以内に障害物がある場合
yangtzuli 7:19ebd0851f49 508 if(mode==AVOIDANCE)
yangtzuli 7:19ebd0851f49 509 run = STOP; // 停止
yangtzuli 7:19ebd0851f49 510 else
yangtzuli 7:19ebd0851f49 511 return;
yangtzuli 2:38825726cb1b 512 }
yangtzuli 2:38825726cb1b 513 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
yangtzuli 2:38825726cb1b 514 ThisThread::sleep_for(100); // 100ms待つ
yangtzuli 2:38825726cb1b 515 if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
yangtzuli 5:3fffb364744b 516 flag_a = 1; // フラグに1をセット
yangtzuli 2:38825726cb1b 517 }
yangtzuli 3:2ae6218973be 518 pc.printf(" watchsurrounding\r\n");
yangtzuli 3:2ae6218973be 519 }
yangtzuli 3:2ae6218973be 520
yangtzuli 3:2ae6218973be 521 void defaultView(){
yangtzuli 3:2ae6218973be 522 lcd.setAddress(0,0);
yangtzuli 3:2ae6218973be 523 lcd.printf("Battery:");
yangtzuli 3:2ae6218973be 524 lcd.setAddress(0,1);
yangtzuli 3:2ae6218973be 525 lcd.printf("Mode:");
yangtzuli 2:38825726cb1b 526 }
yangtzuli 2:38825726cb1b 527
yangtzuli 3:2ae6218973be 528 void display(){
yangtzuli 3:2ae6218973be 529 mutex.lock();
yangtzuli 3:2ae6218973be 530 lcd.setAddress(0,1);
yangtzuli 3:2ae6218973be 531 switch(mode){
yangtzuli 3:2ae6218973be 532 case ADVANCE:
yangtzuli 3:2ae6218973be 533 //strcpy(DispMode,"Mode:Advance");
yangtzuli 3:2ae6218973be 534 lcd.printf("Mode:Advance ");
yangtzuli 3:2ae6218973be 535 break;
yangtzuli 3:2ae6218973be 536 case RIGHT:
yangtzuli 3:2ae6218973be 537 //strcpy(DispMode,"Mode:Right");
yangtzuli 5:3fffb364744b 538 lcd.printf("Mode:TurnRight ");
yangtzuli 3:2ae6218973be 539 break;
yangtzuli 3:2ae6218973be 540 case LEFT:
yangtzuli 3:2ae6218973be 541 //strcpy(DispMode,"Mode:Left");
yangtzuli 5:3fffb364744b 542 lcd.printf("Mode:TurnLeft ");
yangtzuli 3:2ae6218973be 543 break;
yangtzuli 3:2ae6218973be 544 case BACK:
yangtzuli 3:2ae6218973be 545 //strcpy(DispMode,"Mode:Back");
yangtzuli 3:2ae6218973be 546 lcd.printf("Mode:Back ");
yangtzuli 3:2ae6218973be 547 break;
yangtzuli 3:2ae6218973be 548 case STOP:
yangtzuli 3:2ae6218973be 549 //strcpy(DispMode,"Mode:Stop");
yangtzuli 3:2ae6218973be 550 lcd.printf("Mode:Stop ");
yangtzuli 3:2ae6218973be 551 break;
yangtzuli 3:2ae6218973be 552 case READY:
yangtzuli 3:2ae6218973be 553 //strcpy(DispMode,"Mode:Stop");
yangtzuli 3:2ae6218973be 554 lcd.printf("Mode:Ready ");
yangtzuli 3:2ae6218973be 555 break;
yangtzuli 3:2ae6218973be 556 case LINE_TRACE:
yangtzuli 3:2ae6218973be 557 //strcpy(DispMode,"Mode:LineTrace");
yangtzuli 3:2ae6218973be 558 lcd.printf("Mode:LineTrace ");
yangtzuli 3:2ae6218973be 559 break;
yangtzuli 3:2ae6218973be 560 case AVOIDANCE:
yangtzuli 3:2ae6218973be 561 //strcpy(DispMode,"Mode:Avoidance");
yangtzuli 3:2ae6218973be 562 lcd.setAddress(0,1);
yangtzuli 3:2ae6218973be 563 lcd.printf("Mode:Avoidance ");
yangtzuli 3:2ae6218973be 564 break;
yangtzuli 3:2ae6218973be 565 case SPEED:
yangtzuli 3:2ae6218973be 566 switch(flag_sp){
yangtzuli 3:2ae6218973be 567 case(NORMAL):
yangtzuli 3:2ae6218973be 568 lcd.printf("Speed:Normal ");
yangtzuli 3:2ae6218973be 569 break;
yangtzuli 3:2ae6218973be 570 case(FAST):
yangtzuli 3:2ae6218973be 571 lcd.printf("Speed:Fast ");
yangtzuli 3:2ae6218973be 572 break;
yangtzuli 3:2ae6218973be 573 case(VERYFAST):
yangtzuli 3:2ae6218973be 574 lcd.printf("Speed:VeryFast ");
yangtzuli 3:2ae6218973be 575 break;
yangtzuli 3:2ae6218973be 576 }
yangtzuli 3:2ae6218973be 577 viewTimer.reset();
yangtzuli 3:2ae6218973be 578 viewTimer.start();
yangtzuli 3:2ae6218973be 579 break;
yangtzuli 3:2ae6218973be 580 }
yangtzuli 3:2ae6218973be 581 mutex.unlock();
yangtzuli 3:2ae6218973be 582 }
yangtzuli 3:2ae6218973be 583
yangtzuli 3:2ae6218973be 584 void lcdBacklight(void const *argument){
yangtzuli 3:2ae6218973be 585 if(flag_b == 1){
yangtzuli 3:2ae6218973be 586 lcd.setBacklight(TextLCD::LightOn);
yangtzuli 3:2ae6218973be 587 }else{
yangtzuli 3:2ae6218973be 588 lcd.setBacklight(TextLCD::LightOff);
yangtzuli 3:2ae6218973be 589 }
yangtzuli 3:2ae6218973be 590 flag_b = !flag_b;
yangtzuli 3:2ae6218973be 591 }
yangtzuli 2:38825726cb1b 592
yangtzuli 5:3fffb364744b 593 void bChange(){
yangtzuli 3:2ae6218973be 594 lcd.setBacklight(TextLCD::LightOn);
yangtzuli 3:2ae6218973be 595 while(1){
yangtzuli 3:2ae6218973be 596 pc.printf(" bChange1\r\n");
yangtzuli 4:3f80c0180e2f 597 b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10;
yangtzuli 5:3fffb364744b 598 //pc.printf("%f",battery.read());
yangtzuli 6:800a745c7f2e 599 if(b <= 0){//すべての機能停止(今はなし)
yangtzuli 4:3f80c0180e2f 600 b = 0;
yangtzuli 6:800a745c7f2e 601 //lcd.setBacklight(TextLCD::LightOff);
yangtzuli 6:800a745c7f2e 602 //run = STOP;
yangtzuli 6:800a745c7f2e 603 //exit(1);//電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。
yangtzuli 5:3fffb364744b 604 }else if(b > 50){
yangtzuli 5:3fffb364744b 605 b = 50;
yangtzuli 3:2ae6218973be 606 }
yangtzuli 3:2ae6218973be 607 mutex.lock();
yangtzuli 3:2ae6218973be 608 lcd.setAddress(0,0);
yangtzuli 2:38825726cb1b 609 lcd.printf("Battery:%3d%%",b);
yangtzuli 3:2ae6218973be 610 pc.printf(" bChange2\r\n");
yangtzuli 3:2ae6218973be 611 mutex.unlock();
yangtzuli 3:2ae6218973be 612 if(b <= 30){
yangtzuli 3:2ae6218973be 613 if(flag_t == 0){
yangtzuli 3:2ae6218973be 614 bTimer.start(500);
yangtzuli 3:2ae6218973be 615 flag_t = 1;
yangtzuli 3:2ae6218973be 616 }
yangtzuli 3:2ae6218973be 617 }else{
yangtzuli 3:2ae6218973be 618 if(flag_t == 1){
yangtzuli 3:2ae6218973be 619 bTimer.stop();
yangtzuli 3:2ae6218973be 620 lcd.setBacklight(TextLCD::LightOn);
yangtzuli 3:2ae6218973be 621 flag_t = 0;
yangtzuli 3:2ae6218973be 622 }
yangtzuli 3:2ae6218973be 623 }
yangtzuli 2:38825726cb1b 624 }
yangtzuli 4:3f80c0180e2f 625 ThisThread::sleep_for(500);
yangtzuli 2:38825726cb1b 626 }
yangtzuli 0:0d0037aabe41 627 int main() {
yangtzuli 3:2ae6218973be 628 mode = READY;
yangtzuli 3:2ae6218973be 629 beforeMode = READY;
yangtzuli 3:2ae6218973be 630 run = STOP;
yangtzuli 3:2ae6218973be 631 flag_sp = NORMAL;
yangtzuli 4:3f80c0180e2f 632 display();
yangtzuli 2:38825726cb1b 633
yangtzuli 0:0d0037aabe41 634 while(1){
yangtzuli 3:2ae6218973be 635 pc.printf(" main1\r\n");
yangtzuli 5:3fffb364744b 636 bChange();
yangtzuli 3:2ae6218973be 637 pc.printf(" main2\r\n");
yangtzuli 4:3f80c0180e2f 638 ThisThread::sleep_for(1);
yangtzuli 0:0d0037aabe41 639 }
yangtzuli 0:0d0037aabe41 640 }