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