siqiang pan / Mbed OS robotcar-team-0928

2

Dependencies:   RemoteIR TextLCD

main.cpp@3:2ae6218973be, 2020-07-29 (annotated)

Committer:
yangtzuli
Date:
Wed Jul 29 01:24:51 2020 +0000
Revision:
3:2ae6218973be
Parent:
2:38825726cb1b
Child:
4:3f80c0180e2f
first commit

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