![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
スレッド生成消去追加
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?
User | Revision | Line number | New 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 | } |