siqiang pan / Mbed OS robotcarWifi3-demo-orn

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

Dependencies:   RemoteIR TextLCD

main.cpp@2:38825726cb1b, 2020-07-27 (annotated)

Committer:
yangtzuli
Date:
Mon Jul 27 08:49:28 2020 +0000
Revision:
2:38825726cb1b
Parent:
1:5bb497a38344
Child:
3:2ae6218973be
kumitate

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 2:38825726cb1b 12 #define MIN_V 2.23
yangtzuli 2:38825726cb1b 13 #define MAX_V 3.3
yangtzuli 2:38825726cb1b 14
yangtzuli 2:38825726cb1b 15 //bChangeのいろいろ
yangtzuli 2:38825726cb1b 16 I2C i2c_lcd(p9, p10);
yangtzuli 2:38825726cb1b 17 TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
yangtzuli 2:38825726cb1b 18 int b = 0;
yangtzuli 2:38825726cb1b 19 AnalogIn battery(p15);
yangtzuli 2:38825726cb1b 20 double test;
yangtzuli 0:0d0037aabe41 21
yangtzuli 0:0d0037aabe41 22 Serial pc(USBTX, USBRX);
yangtzuli 0:0d0037aabe41 23 // ポートp15を赤外線受信モジュールの接続先に指定
yangtzuli 0:0d0037aabe41 24 ReceiverIR ir_rx(p15);
yangtzuli 0:0d0037aabe41 25 RemoteIR::Format format;
yangtzuli 0:0d0037aabe41 26 uint8_t buf[32];
yangtzuli 0:0d0037aabe41 27 uint32_t bitcount;
yangtzuli 1:5bb497a38344 28 uint32_t code;
yangtzuli 0:0d0037aabe41 29
yangtzuli 2:38825726cb1b 30 //障害物回避の設定
yangtzuli 2:38825726cb1b 31 DigitalOut trig(p6); // 超音波センサのtrigピンをp6に接続
yangtzuli 2:38825726cb1b 32 DigitalIn echo(p7); // 超音波センサのechoピンをp7に接続
yangtzuli 2:38825726cb1b 33 PwmOut servo(p25); // サーボコントロールピン(p25)
yangtzuli 2:38825726cb1b 34
yangtzuli 2:38825726cb1b 35 enum Mode{
yangtzuli 2:38825726cb1b 36 ADVANCE,
yangtzuli 2:38825726cb1b 37 RIGHT,
yangtzuli 2:38825726cb1b 38 LEFT,
yangtzuli 2:38825726cb1b 39 BACK,
yangtzuli 2:38825726cb1b 40 STOP,
yangtzuli 2:38825726cb1b 41 LINE_TRACE,
yangtzuli 2:38825726cb1b 42 AVOIDANCE,
yangtzuli 2:38825726cb1b 43 READY
yangtzuli 2:38825726cb1b 44 };
yangtzuli 2:38825726cb1b 45
yangtzuli 2:38825726cb1b 46 Mode run;
yangtzuli 2:38825726cb1b 47 Mode mode;
yangtzuli 2:38825726cb1b 48
yangtzuli 2:38825726cb1b 49 Timer timer; // 距離計測用タイマー
yangtzuli 0:0d0037aabe41 50
yangtzuli 2:38825726cb1b 51 /* 障害物検知用の変数設定 */
yangtzuli 2:38825726cb1b 52 int SC; // 正面
yangtzuli 2:38825726cb1b 53 int SL; // 左
yangtzuli 2:38825726cb1b 54 int SR; // 右
yangtzuli 2:38825726cb1b 55 int SLD; // 左前
yangtzuli 2:38825726cb1b 56 int SRD; // 右前
yangtzuli 2:38825726cb1b 57 int flag = 0; // 障害物有無のフラグ
yangtzuli 2:38825726cb1b 58 const int limit = 20; // 障害物の距離のリミット(単位：cm)
yangtzuli 2:38825726cb1b 59 int DT; // 距離
yangtzuli 2:38825726cb1b 60 int houkou; // 進行方向(1:前 2:左 3:右)
yangtzuli 2:38825726cb1b 61 int far; // 最も遠い距離
yangtzuli 2:38825726cb1b 62 int i; // ループ変数
yangtzuli 2:38825726cb1b 63
yangtzuli 2:38825726cb1b 64 void rimokon(void const *argument);
yangtzuli 2:38825726cb1b 65 void avoidance(void const *argument);
yangtzuli 2:38825726cb1b 66 void motor(void const *argument);
yangtzuli 2:38825726cb1b 67 void watchsurrounding();
yangtzuli 2:38825726cb1b 68 int watch();
yangtzuli 2:38825726cb1b 69 void bChange(void const *argument);
yangtzuli 2:38825726cb1b 70 Thread thread1(rimokon, NULL, osPriorityRealtime);
yangtzuli 2:38825726cb1b 71 Thread thread2(motor, NULL, osPriorityHigh);
yangtzuli 2:38825726cb1b 72 Thread thread3(avoidance, NULL, osPriorityAboveNormal);
yangtzuli 2:38825726cb1b 73 Thread thread4(bChange, NULL, osPriorityBelowNormal);
yangtzuli 2:38825726cb1b 74
yangtzuli 2:38825726cb1b 75 void rimokon(void const *argument){
yangtzuli 2:38825726cb1b 76 while(1){
yangtzuli 0:0d0037aabe41 77 // 受信待ち
yangtzuli 0:0d0037aabe41 78 if (ir_rx.getState() == ReceiverIR::Received) {
yangtzuli 0:0d0037aabe41 79 // コード受信
yangtzuli 0:0d0037aabe41 80 bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
yangtzuli 0:0d0037aabe41 81 if(bitcount > 1){
yangtzuli 0:0d0037aabe41 82 // 受信成功
yangtzuli 1:5bb497a38344 83 code=0;
yangtzuli 1:5bb497a38344 84 for(int j=0;j<4;j++){
yangtzuli 1:5bb497a38344 85 code+=(buf[j]<<(8*(3-j)));
yangtzuli 1:5bb497a38344 86 }
yangtzuli 1:5bb497a38344 87 pc.printf("%0x\r\n",code);
yangtzuli 0:0d0037aabe41 88 switch(code){
yangtzuli 1:5bb497a38344 89 case 0x40bf0ff0://入力切換
yangtzuli 1:5bb497a38344 90 pc.printf("入力切換\r\n");
yangtzuli 1:5bb497a38344 91 break;
yangtzuli 1:5bb497a38344 92 case 0x40bf12ed://電源
yangtzuli 1:5bb497a38344 93 pc.printf("電源\r\n");
yangtzuli 1:5bb497a38344 94 break;
yangtzuli 1:5bb497a38344 95 case 0x40bf7b84://地アナ
yangtzuli 1:5bb497a38344 96 pc.printf("地アナ\r\n");
yangtzuli 1:5bb497a38344 97 break;
yangtzuli 1:5bb497a38344 98 case 0x40bf7a85://地デジ
yangtzuli 1:5bb497a38344 99 pc.printf("地デジ\r\n");
yangtzuli 1:5bb497a38344 100 break;
yangtzuli 1:5bb497a38344 101 case 0x40bf7c83://BS
yangtzuli 1:5bb497a38344 102 pc.printf("BS\r\n");
yangtzuli 1:5bb497a38344 103 break;
yangtzuli 1:5bb497a38344 104 case 0x40bf7d82://CS
yangtzuli 1:5bb497a38344 105 pc.printf("CS\r\n");
yangtzuli 1:5bb497a38344 106 break;
yangtzuli 1:5bb497a38344 107
yangtzuli 1:5bb497a38344 108 case 0x40bf01fe://1
yangtzuli 1:5bb497a38344 109 pc.printf("1\r\n");
yangtzuli 1:5bb497a38344 110 break;
yangtzuli 1:5bb497a38344 111 case 0x40bf02fd://2
yangtzuli 1:5bb497a38344 112 pc.printf("2\r\n");
yangtzuli 1:5bb497a38344 113 break;
yangtzuli 1:5bb497a38344 114 case 0x40bf03fc://3
yangtzuli 1:5bb497a38344 115 pc.printf("3\r\n");
yangtzuli 1:5bb497a38344 116 break;
yangtzuli 1:5bb497a38344 117 case 0x40bf04fb://4
yangtzuli 1:5bb497a38344 118 pc.printf("4\r\n");
yangtzuli 1:5bb497a38344 119 break;
yangtzuli 1:5bb497a38344 120 case 0x40bf05fa://5
yangtzuli 1:5bb497a38344 121 pc.printf("5\r\n");
yangtzuli 1:5bb497a38344 122 break;
yangtzuli 1:5bb497a38344 123 case 0x40bf06f9://6
yangtzuli 1:5bb497a38344 124 pc.printf("6\r\n");
yangtzuli 1:5bb497a38344 125 break;
yangtzuli 1:5bb497a38344 126 case 0x40bf07f8://7
yangtzuli 1:5bb497a38344 127 pc.printf("7\r\n");
yangtzuli 1:5bb497a38344 128 break;
yangtzuli 1:5bb497a38344 129 case 0x40bf08f7://8
yangtzuli 1:5bb497a38344 130 pc.printf("8\r\n");
yangtzuli 1:5bb497a38344 131 break;
yangtzuli 1:5bb497a38344 132 case 0x40bf09f6://9
yangtzuli 1:5bb497a38344 133 pc.printf("9\r\n");
yangtzuli 1:5bb497a38344 134 break;
yangtzuli 1:5bb497a38344 135 case 0x40bf0af5://10
yangtzuli 1:5bb497a38344 136 pc.printf("10\r\n");
yangtzuli 1:5bb497a38344 137 break;
yangtzuli 1:5bb497a38344 138 case 0x40bf0bf4://11
yangtzuli 1:5bb497a38344 139 pc.printf("11\r\n");
yangtzuli 1:5bb497a38344 140 break;
yangtzuli 1:5bb497a38344 141 case 0x40bf0cf3://12
yangtzuli 1:5bb497a38344 142 pc.printf("12\r\n");
yangtzuli 1:5bb497a38344 143 break;
yangtzuli 1:5bb497a38344 144 case 0x40bf1be4://チャンネル↑
yangtzuli 1:5bb497a38344 145 pc.printf("チャンネル↑\r\n");
yangtzuli 1:5bb497a38344 146 break;
yangtzuli 1:5bb497a38344 147 case 0x40bf1fe0://チャンネル↓
yangtzuli 1:5bb497a38344 148 pc.printf("チャンネル↓\r\n");
yangtzuli 0:0d0037aabe41 149 break;
yangtzuli 1:5bb497a38344 150 case 0x40bf1ce3://画面表示
yangtzuli 1:5bb497a38344 151 pc.printf("画面表示\r\n");
yangtzuli 1:5bb497a38344 152 break;
yangtzuli 1:5bb497a38344 153 case 0x40bf10ef://消音
yangtzuli 1:5bb497a38344 154 pc.printf("消音\r\n");
yangtzuli 1:5bb497a38344 155 break;
yangtzuli 1:5bb497a38344 156 case 0x40bf27d8://クイック
yangtzuli 1:5bb497a38344 157 pc.printf("クイック\r\n");
yangtzuli 1:5bb497a38344 158 break;
yangtzuli 1:5bb497a38344 159 case 0x40bf1ae5://音量↑
yangtzuli 1:5bb497a38344 160 pc.printf("音量↑\r\n");
yangtzuli 1:5bb497a38344 161 break;
yangtzuli 1:5bb497a38344 162 case 0x40bf1ee1://音量↓
yangtzuli 1:5bb497a38344 163 pc.printf("音量↓\r\n");
yangtzuli 1:5bb497a38344 164 break;
yangtzuli 1:5bb497a38344 165 case 0x40be34cb://レグザリンク
yangtzuli 1:5bb497a38344 166 pc.printf("レグザリンク\r\n");
yangtzuli 1:5bb497a38344 167 break;
yangtzuli 1:5bb497a38344 168 case 0x40bf6e91://番組表
yangtzuli 1:5bb497a38344 169 pc.printf("番組表\r\n");
yangtzuli 1:5bb497a38344 170 break;
yangtzuli 1:5bb497a38344 171 case 0x40bf3bc4://戻る
yangtzuli 1:5bb497a38344 172 pc.printf("戻る\r\n");
yangtzuli 1:5bb497a38344 173 break;
yangtzuli 1:5bb497a38344 174 case 0x40bf3cc3://終了
yangtzuli 1:5bb497a38344 175 pc.printf("終了\r\n");
yangtzuli 1:5bb497a38344 176 break;
yangtzuli 1:5bb497a38344 177 case 0x40bf3ec1://↑
yangtzuli 1:5bb497a38344 178 pc.printf("↑\r\n");
yangtzuli 1:5bb497a38344 179 break;
yangtzuli 1:5bb497a38344 180 case 0x40bf3fc0://↓
yangtzuli 1:5bb497a38344 181 pc.printf("↓\r\n");
yangtzuli 0:0d0037aabe41 182 break;
yangtzuli 1:5bb497a38344 183 case 0x40bf5fa0://←
yangtzuli 2:38825726cb1b 184 mode=LINE_TRACE;
yangtzuli 1:5bb497a38344 185 pc.printf("←\r\n");
yangtzuli 1:5bb497a38344 186 break;
yangtzuli 1:5bb497a38344 187 case 0x40bf5ba4://→
yangtzuli 2:38825726cb1b 188 mode=AVOIDANCE;
yangtzuli 1:5bb497a38344 189 pc.printf("→\r\n");
yangtzuli 1:5bb497a38344 190 break;
yangtzuli 1:5bb497a38344 191 case 0x40bf3dc2://決定
yangtzuli 1:5bb497a38344 192 pc.printf("決定\r\n");
yangtzuli 1:5bb497a38344 193 break;
yangtzuli 1:5bb497a38344 194 case 0x40bf738c://青
yangtzuli 1:5bb497a38344 195 pc.printf("青\r\n");
yangtzuli 1:5bb497a38344 196 break;
yangtzuli 1:5bb497a38344 197 case 0x40bf748b://赤
yangtzuli 1:5bb497a38344 198 pc.printf("赤\r\n");
yangtzuli 1:5bb497a38344 199 break;
yangtzuli 1:5bb497a38344 200 case 0x40bf758a://緑
yangtzuli 1:5bb497a38344 201 pc.printf("緑\r\n");
yangtzuli 0:0d0037aabe41 202 break;
yangtzuli 1:5bb497a38344 203 case 0x40bf7689://黄
yangtzuli 1:5bb497a38344 204 pc.printf("黄\r\n");
yangtzuli 1:5bb497a38344 205 break;
yangtzuli 1:5bb497a38344 206 case 0x43bc14eb://dデータ
yangtzuli 1:5bb497a38344 207 pc.printf("dデータ\r\n");
yangtzuli 1:5bb497a38344 208 break;
yangtzuli 1:5bb497a38344 209 case 0x40bf50af://静止
yangtzuli 1:5bb497a38344 210 pc.printf("静止\r\n");
yangtzuli 1:5bb497a38344 211 break;
yangtzuli 1:5bb497a38344 212 case 0x40bf59a6://おまかせ映像
yangtzuli 1:5bb497a38344 213 pc.printf("おまかせ映像\r\n");
yangtzuli 1:5bb497a38344 214 break;
yangtzuli 1:5bb497a38344 215 case 0x40bf13ec://音声切換
yangtzuli 1:5bb497a38344 216 pc.printf("音声切換\r\n");
yangtzuli 1:5bb497a38344 217 break;
yangtzuli 1:5bb497a38344 218
yangtzuli 1:5bb497a38344 219
yangtzuli 0:0d0037aabe41 220 default:
yangtzuli 0:0d0037aabe41 221 ;
yangtzuli 0:0d0037aabe41 222 }
yangtzuli 0:0d0037aabe41 223 }
yangtzuli 0:0d0037aabe41 224 }
yangtzuli 2:38825726cb1b 225 ThisThread::sleep_for(10);
yangtzuli 2:38825726cb1b 226 }
yangtzuli 2:38825726cb1b 227 }
yangtzuli 2:38825726cb1b 228 void motor(void const *argument){
yangtzuli 2:38825726cb1b 229 while(1){
yangtzuli 2:38825726cb1b 230 pc.printf("motor\r\n");
yangtzuli 2:38825726cb1b 231 ThisThread::sleep_for(20);
yangtzuli 2:38825726cb1b 232 }
yangtzuli 2:38825726cb1b 233 }
yangtzuli 2:38825726cb1b 234 /* 障害物回避走行 */
yangtzuli 2:38825726cb1b 235 void avoidance(void const *argument){
yangtzuli 2:38825726cb1b 236
yangtzuli 2:38825726cb1b 237 while(1){
yangtzuli 2:38825726cb1b 238 if(mode==AVOIDANCE){
yangtzuli 2:38825726cb1b 239 pc.printf("avoidance\r\n");
yangtzuli 2:38825726cb1b 240 if(flag == 0){ // 障害物がない場合
yangtzuli 2:38825726cb1b 241 run = ADVANCE; // 前進
yangtzuli 2:38825726cb1b 242 }
yangtzuli 2:38825726cb1b 243 /* else{ // 障害物がある場合
yangtzuli 2:38825726cb1b 244 i = 0;
yangtzuli 2:38825726cb1b 245 if(SC < 15){ // 正面15cm以内に障害物が現れた場合
yangtzuli 2:38825726cb1b 246 run = BACK; // 後退
yangtzuli 2:38825726cb1b 247 while(watch() < limit){ // 正面20cm以内に障害物がある間
yangtzuli 2:38825726cb1b 248
yangtzuli 2:38825726cb1b 249 }
yangtzuli 2:38825726cb1b 250 run = STOP; // 停止
yangtzuli 2:38825726cb1b 251 }
yangtzuli 2:38825726cb1b 252 if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
yangtzuli 2:38825726cb1b 253 run = LEFT; // 左折
yangtzuli 2:38825726cb1b 254 while(i < 100){ // 進行方向確認
yangtzuli 2:38825726cb1b 255 if(watch() > limit){
yangtzuli 2:38825726cb1b 256 i++;
yangtzuli 2:38825726cb1b 257 }
yangtzuli 2:38825726cb1b 258 else{
yangtzuli 2:38825726cb1b 259 i = 0;
yangtzuli 2:38825726cb1b 260 }
yangtzuli 2:38825726cb1b 261 }
yangtzuli 2:38825726cb1b 262 run = STOP; // 停止
yangtzuli 2:38825726cb1b 263 }
yangtzuli 2:38825726cb1b 264 else { // 全方向以外
yangtzuli 2:38825726cb1b 265 far = SC; // 正面を最も遠い距離に設定
yangtzuli 2:38825726cb1b 266 houkou = 1; // 進行方向を前に設定
yangtzuli 2:38825726cb1b 267 if(far < SLD || far < SL){ // 左または左前がより遠い場合
yangtzuli 2:38825726cb1b 268 if(SL < SLD){ // 左前が左より遠い場合
yangtzuli 2:38825726cb1b 269 far = SLD; // 左前を最も遠い距離に設定
yangtzuli 2:38825726cb1b 270 }
yangtzuli 2:38825726cb1b 271 else{ // 左が左前より遠い場合
yangtzuli 2:38825726cb1b 272 far = SL; // 左を最も遠い距離に設定
yangtzuli 2:38825726cb1b 273 }
yangtzuli 2:38825726cb1b 274 houkou = 2; // 進行方向を左に設定
yangtzuli 2:38825726cb1b 275 }
yangtzuli 2:38825726cb1b 276 if(far < SRD || far < SR){ // 右または右前がより遠い場合
yangtzuli 2:38825726cb1b 277 if(SR < SRD){ // 右前が右より遠い場合
yangtzuli 2:38825726cb1b 278 far = SRD; // 右前を最も遠い距離に設定
yangtzuli 2:38825726cb1b 279 }
yangtzuli 2:38825726cb1b 280 else{ // 右が右前よりも遠い場合
yangtzuli 2:38825726cb1b 281 far = SR; // 右を最も遠い距離に設定
yangtzuli 2:38825726cb1b 282 }
yangtzuli 2:38825726cb1b 283 houkou = 3; // 進行方向を右に設定
yangtzuli 2:38825726cb1b 284 }
yangtzuli 2:38825726cb1b 285 switch(houkou){ // 進行方向の場合分け
yangtzuli 2:38825726cb1b 286 case 1: // 前の場合
yangtzuli 2:38825726cb1b 287 run = ADVANCE; // 前進
yangtzuli 2:38825726cb1b 288 thread_sleep_for(100); // 1秒待つ
yangtzuli 2:38825726cb1b 289 break;
yangtzuli 2:38825726cb1b 290 case 2: // 左の場合
yangtzuli 2:38825726cb1b 291 run = LEFT; // 左折
yangtzuli 2:38825726cb1b 292 while(i < 100){ // 進行方向確認
yangtzuli 2:38825726cb1b 293 if(watch() > (far - 2)){
yangtzuli 2:38825726cb1b 294 i++;
yangtzuli 2:38825726cb1b 295 }
yangtzuli 2:38825726cb1b 296 else{
yangtzuli 2:38825726cb1b 297 i = 0;
yangtzuli 2:38825726cb1b 298 }
yangtzuli 2:38825726cb1b 299 }
yangtzuli 2:38825726cb1b 300 run = STOP; // 停止
yangtzuli 2:38825726cb1b 301 break;
yangtzuli 2:38825726cb1b 302 case 3: // 右の場合
yangtzuli 2:38825726cb1b 303 run = RIGHT; // 右折
yangtzuli 2:38825726cb1b 304 while(i < 100){ // 進行方向確認
yangtzuli 2:38825726cb1b 305 if(watch() > (far - 2)){
yangtzuli 2:38825726cb1b 306 i++;
yangtzuli 2:38825726cb1b 307 }
yangtzuli 2:38825726cb1b 308 else{
yangtzuli 2:38825726cb1b 309 i = 0;
yangtzuli 2:38825726cb1b 310 }
yangtzuli 2:38825726cb1b 311 }
yangtzuli 2:38825726cb1b 312 run = STOP; // 停止
yangtzuli 2:38825726cb1b 313 break;
yangtzuli 2:38825726cb1b 314 }
yangtzuli 2:38825726cb1b 315 }
yangtzuli 2:38825726cb1b 316 }*/
yangtzuli 2:38825726cb1b 317 flag = 0; // フラグを0にセット
yangtzuli 2:38825726cb1b 318 watchsurrounding();
yangtzuli 2:38825726cb1b 319 }
yangtzuli 2:38825726cb1b 320 }
yangtzuli 2:38825726cb1b 321 }
yangtzuli 2:38825726cb1b 322 int watch(){
yangtzuli 2:38825726cb1b 323 pc.printf("watch\r\n");
yangtzuli 2:38825726cb1b 324 trig = 0;
yangtzuli 2:38825726cb1b 325 ThisThread::sleep_for(5); // 5ms待つ
yangtzuli 2:38825726cb1b 326 trig = 1;
yangtzuli 2:38825726cb1b 327 ThisThread::sleep_for(15); // 15ms待つ
yangtzuli 2:38825726cb1b 328 trig = 0;
yangtzuli 2:38825726cb1b 329 //while(echo.read() == 0){
yangtzuli 2:38825726cb1b 330 //}
yangtzuli 2:38825726cb1b 331 timer.start(); // 距離計測タイマースタート
yangtzuli 2:38825726cb1b 332 while(echo.read() == 1){
yangtzuli 2:38825726cb1b 333 }
yangtzuli 2:38825726cb1b 334 timer.stop(); // 距離計測タイマーストップ
yangtzuli 2:38825726cb1b 335 DT = timer.read_us()*0.01657; // 距離計算
yangtzuli 2:38825726cb1b 336 if(DT > 400){ // 検知範囲外なら400cmに設定
yangtzuli 2:38825726cb1b 337 DT = 400;
yangtzuli 2:38825726cb1b 338 }
yangtzuli 2:38825726cb1b 339 timer.reset(); // 距離計測タイマーリセット
yangtzuli 2:38825726cb1b 340 ThisThread::sleep_for(30); // 30ms待つ
yangtzuli 2:38825726cb1b 341 return DT;
yangtzuli 0:0d0037aabe41 342 }
yangtzuli 0:0d0037aabe41 343
yangtzuli 2:38825726cb1b 344 void watchsurrounding(){
yangtzuli 2:38825726cb1b 345 pc.printf("watchsurrounding\r\n");
yangtzuli 2:38825726cb1b 346 SC = watch();
yangtzuli 2:38825726cb1b 347 if(SC < limit){ // 正面20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 348 run = STOP; // 停止
yangtzuli 2:38825726cb1b 349 }
yangtzuli 2:38825726cb1b 350 servo.pulsewidth_us(1925); // サーボを左に40度回転
yangtzuli 2:38825726cb1b 351 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 352 SLD = watch();
yangtzuli 2:38825726cb1b 353 if(SLD < limit){ // 左前20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 354 run = STOP; // 停止
yangtzuli 2:38825726cb1b 355 }
yangtzuli 2:38825726cb1b 356 servo.pulsewidth_us(2400); // サーボを左に90度回転
yangtzuli 2:38825726cb1b 357 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 358 SL = watch();
yangtzuli 2:38825726cb1b 359 if(SL < limit){ // 左20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 360 run = STOP; // 停止
yangtzuli 2:38825726cb1b 361 }
yangtzuli 2:38825726cb1b 362 servo.pulsewidth_us(1450);
yangtzuli 2:38825726cb1b 363 ThisThread::sleep_for(100);
yangtzuli 2:38825726cb1b 364 servo.pulsewidth_us(925); // サーボを右に40度回転
yangtzuli 2:38825726cb1b 365 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 366 SRD = watch();
yangtzuli 2:38825726cb1b 367 if(SRD < limit){ // 右前20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 368 run = STOP; // 停止
yangtzuli 2:38825726cb1b 369 }
yangtzuli 2:38825726cb1b 370 servo.pulsewidth_us(500); // サーボを右に90度回転
yangtzuli 2:38825726cb1b 371 ThisThread::sleep_for(250); // 250ms待つ
yangtzuli 2:38825726cb1b 372 SR = watch();
yangtzuli 2:38825726cb1b 373 if(SR < limit){ // 右20cm以内に障害物がある場合
yangtzuli 2:38825726cb1b 374 run = STOP; // 停止
yangtzuli 2:38825726cb1b 375 }
yangtzuli 2:38825726cb1b 376 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
yangtzuli 2:38825726cb1b 377 ThisThread::sleep_for(100); // 100ms待つ
yangtzuli 2:38825726cb1b 378 if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
yangtzuli 2:38825726cb1b 379 flag = 1; // フラグに1をセット
yangtzuli 2:38825726cb1b 380 }
yangtzuli 2:38825726cb1b 381 }
yangtzuli 2:38825726cb1b 382
yangtzuli 2:38825726cb1b 383
yangtzuli 2:38825726cb1b 384 void bChange(void const *argument){
yangtzuli 2:38825726cb1b 385 while(1){
yangtzuli 2:38825726cb1b 386 //lcd.setBacklight(TextLCD::LightOn);
yangtzuli 2:38825726cb1b 387 test = battery.read()*MAX_V;
yangtzuli 2:38825726cb1b 388 b = (int)((test - MIN_V)/0.107 + 0.5)*10;
yangtzuli 2:38825726cb1b 389 lcd.locate(0,0);
yangtzuli 2:38825726cb1b 390 lcd.printf("Battery:%3d%%",b);
yangtzuli 2:38825726cb1b 391 pc.printf("bChange\r\n");
yangtzuli 2:38825726cb1b 392 }
yangtzuli 2:38825726cb1b 393 }
yangtzuli 0:0d0037aabe41 394 int main() {
yangtzuli 0:0d0037aabe41 395
yangtzuli 2:38825726cb1b 396
yangtzuli 2:38825726cb1b 397
yangtzuli 0:0d0037aabe41 398 while(1){
yangtzuli 2:38825726cb1b 399 /*if(button.read()==0){
yangtzuli 0:0d0037aabe41 400 rimokon();
yangtzuli 2:38825726cb1b 401 } */
yangtzuli 2:38825726cb1b 402 pc.printf("main\r\n");
yangtzuli 2:38825726cb1b 403 ThisThread::sleep_for(40);
yangtzuli 0:0d0037aabe41 404 }
yangtzuli 0:0d0037aabe41 405 }