siqiang pan / Mbed OS robotcarWifi3-demo-orn

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

Dependencies:   RemoteIR TextLCD

main.cpp@17:f7259ab2fe86, 2020-08-04 (annotated)

Committer:
yangtzuli
Date:
Tue Aug 04 08:43:58 2020 +0000
Revision:
17:f7259ab2fe86
Parent:
16:ffc732a3cf92
Child:
18:6cca64c7dbc3
mbed_robotcar

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