tzuli yang
/
robotcar_do_while3
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 17:f7259ab2fe86
- Parent:
- 16:ffc732a3cf92
- Child:
- 18:6cca64c7dbc3
diff -r ffc732a3cf92 -r f7259ab2fe86 main.cpp
--- a/main.cpp Tue Aug 04 05:04:11 2020 +0000
+++ b/main.cpp Tue Aug 04 08:43:58 2020 +0000
@@ -58,7 +58,7 @@
int beforeMode; // 前回のモード
int flag_sp = 0; // スピード変化フラグ
Timer viewTimer; // スピ―ド変更時に3秒計測タイマー
-float motorSpeed[9] = {0.7, 0.8, 0.9, 0.75, 0.85, 0.95, 0.8, 0.9, 1.0};
+float motorSpeed[9] = {0.5, 0.6, 0.7, 0.75, 0.85, 0.95, 0.8, 0.9, 1.0};
// モーター速度設定(後半はライントレース用)
Mutex mutex; // ミューテックス
@@ -94,6 +94,7 @@
int far; // 最も遠い距離
int houkou; // 進行方向(1:前 2:左 3:右)
int i; // ループ変数
+int t1,t2=0;
@@ -122,7 +123,7 @@
void decodeIR(void const *argument){
while(1){
// 受信待ち
- pc.printf("decodeIR\r\n");
+ //pc.printf("decodeIR\r\n");
if (ir.getState() == ReceiverIR::Received){ // コード受信
bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
if(bitcount > 1){ // 受信成功
@@ -136,14 +137,14 @@
//pc.printf("%0x\r\n",code);
switch(code){
case 0x40bf27d8: // クイック
- pc.printf("mode = SPEED\r\n");
+ //pc.printf("mode = SPEED\r\n");
mode = SPEED; // スピードモード
changeSpeed(); // 速度変更
display(); // ディスプレイ表示
mode = beforeMode; // 現在のモードに前回のモードを設定
break;
case 0x40be34cb: // レグザリンク
- pc.printf("mode = LINE_TRACE\r\n");
+ //pc.printf("mode = LINE_TRACE\r\n");
if(trace_thread->get_state() == Thread::Deleted){
delete trace_thread;
trace_thread = new Thread(trace);
@@ -153,7 +154,7 @@
display(); // ディスプレイ表示
break;
case 0x40bf6e91: // 番組表
- pc.printf("mode = AVOIDANCE\r\n");
+ //pc.printf("mode = AVOIDANCE\r\n");
if(avoi_thread->get_state() == Thread::Deleted){
delete avoi_thread;
avoi_thread = new Thread(avoidance);
@@ -164,31 +165,31 @@
display(); // ディスプレイ表示
break;
case 0x40bf3ec1: // ↑
- pc.printf("mode = ADVANCE\r\n");
+ //pc.printf("mode = ADVANCE\r\n");
mode = ADVANCE; // 前進モード
run = ADVANCE; // 前進
display(); // ディスプレイ表示
break;
case 0x40bf3fc0: // ↓
- pc.printf("mode = BACK\r\n");
+ //pc.printf("mode = BACK\r\n");
mode = BACK; // 後退モード
run = BACK; // 後退
display(); // ディスプレイ表示
break;
case 0x40bf5fa0: // ←
- pc.printf("mode = LEFT\r\n");
+ //pc.printf("mode = LEFT\r\n");
mode = LEFT; // 左折モード
run = LEFT; // 左折
display(); // ディスプレイ表示
break;
case 0x40bf5ba4: // →
- pc.printf("mode = RIGHT\r\n");
+ //pc.printf("mode = RIGHT\r\n");
mode = RIGHT; // 右折モード
run = RIGHT; // 右折
display(); // ディスプレイ表示
break;
case 0x40bf3dc2: // 決定
- pc.printf("mode = STOP\r\n");
+ //pc.printf("mode = STOP\r\n");
mode = STOP; // 停止モード
run = STOP; // 停止
display(); // ディスプレイ表示
@@ -209,14 +210,14 @@
viewTimer.reset(); // タイマーリセット
display(); // ディスプレイ表示
}
- ThisThread::sleep_for(90); // 90ms待つ
+ ThisThread::sleep_for(60); // 90ms待つ
}
}
/* モーター制御スレッド */
void motor(void const *argument){
while(1){
- pc.printf("motor\r\n");
+ //pc.printf("motor\r\n");
/* 走行状態の場合分け */
switch(run){
/* 前進 */
@@ -258,7 +259,7 @@
if(flag_sp > VERYFAST){ // スピード変更フラグが2より大きいなら
flag_sp -= 3 * (flag_sp / 3); // スピード変更フラグ調整
}
- pc.printf(" motor\r\n");
+ //pc.printf(" motor\r\n");
ThisThread::sleep_for(30); // 30ms待つ
}
}
@@ -301,50 +302,50 @@
/* センサー値によって場合分け */
switch(sensorNum){
case 1:
- if(mode == LINE_TRACE){ // 現在ライントレースモードなら
+ //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
run = ADVANCE; // 低速で前進
- }
- pc.printf("ADVANCE");
+ //}
+ //pc.printf("ADVANCE");
break;
case 2:
- if(mode == LINE_TRACE){ // 現在ライントレースモードなら
+ //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
run = RIGHT; // 低速で右折
- }
- pc.printf("RIGHT");
+ //}
+ //pc.printf("RIGHT");
break;
case 3:
- if(mode == LINE_TRACE){ // 現在ライントレースモードなら
+ //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
run = LEFT; // 低速で左折
- }
- pc.printf("LEFT");
+ //}
+ //pc.printf("LEFT");
break;
case 4:
- if(mode==LINE_TRACE){ // 現在ライントレースモードなら
+ //if(mode==LINE_TRACE){ // 現在ライントレースモードなら
flag_sp = flag_sp % 3 + 3;
run = RIGHT; // 中速で右折
- }
- pc.printf("RIGHT");
+ //}
+ //pc.printf("RIGHT");
break;
case 5:
- if(mode == LINE_TRACE){ // 現在ライントレースモードなら
+ //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
flag_sp = flag_sp % 3 + 3;
run = LEFT; // 中速で左折
- }
- pc.printf("LEFT");
+ //}
+ //pc.printf("LEFT");
break;
case 6:
- if(mode == LINE_TRACE){ // 現在ライントレースモードなら
+ //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
flag_sp = flag_sp % 3 + 6;
run = RIGHT; // 高速で右折
- }
- pc.printf("RIGHT");
+ //}
+ //pc.printf("RIGHT");
break;
case 7:
- if(mode == LINE_TRACE){ // 現在ライントレースモードなら
+ //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
flag_sp = flag_sp % 3 + 6;
run = LEFT; // 高速で左折
- }
- pc.printf("LEFT");
+ //}
+ //pc.printf("LEFT");
break;
}
ThisThread::sleep_for(30); // 30ms待つ
@@ -358,7 +359,7 @@
void avoidance(/*void const *argument*/){
while(1){
//if(mode == AVOIDANCE){ // 現在障害物回避モードなら
- pc.printf("avoidance\r\n");
+ //pc.printf("avoidance\r\n");
watchsurrounding();
pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR);
@@ -379,7 +380,7 @@
}
if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
run = LEFT; // 左折
- while(i < 10){ // 進行方向確認
+ while(i < 3){ // 進行方向確認
/*if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら
break;
} */
@@ -417,13 +418,13 @@
switch(houkou){ // 進行方向の場合分け
case 1: // 前の場合
run = ADVANCE; // 前進
- pc.printf("Advance\r\n");
+ //pc.printf("Advance\r\n");
ThisThread::sleep_for(1000); // 1秒待つ
break;
case 2: // 左の場合
run = LEFT; // 左折
- while(i < 10){ // 進行方向確認
- pc.printf("left\r\n");
+ while(i < 3){ // 進行方向確認
+ //pc.printf("left\r\n");
/*if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら
break;
}*/
@@ -438,8 +439,8 @@
break;
case 3: // 右の場合
run = RIGHT; // 右折
- while(i < 10){ // 進行方向確認
- pc.printf("right\r\n");
+ while(i < 3){ // 進行方向確認
+ //pc.printf("right\r\n");
/*if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら
break;
}*/
@@ -455,9 +456,9 @@
}
}
}
- pc.printf("こんにちは！\r\n");
+ //pc.printf("こんにちは！\r\n");
flag_a = 0; // 障害物有無フラグを0にセット
- pc.printf(" avoidance\r\n");
+ //pc.printf(" avoidance\r\n");
/*}else{
ThisThread::sleep_for(1); // 1ms待つ
}*/
@@ -466,28 +467,40 @@
/* 距離計測関数 */
int watch(){
- pc.printf("watch\r\n");
+ //pc.printf("watch\r\n");
trig = 0;
ThisThread::sleep_for(5); // 5ms待つ
trig = 1;
ThisThread::sleep_for(15); // 15ms待つ
trig = 0;
- while(echo.read() == 0){
+ timer.start();
+ t1=timer.read_ms();
+ t2=timer.read_ms();
+ while(echo.read() == 0&&(t1-t2)<10){
+
+ t1=timer.read_ms();
led1 = 1;
if(mode!=AVOIDANCE){ // 現在障害物回避モードでないなら
break;
}
}
- timer.start(); // 距離計測タイマースタート
- while(echo.read() == 1){
+ timer.stop();
+ timer.reset();
+ if((t1-t2)<10){
+ timer.start(); // 距離計測タイマースタート
+ while(echo.read() == 1){
+ }
+ timer.stop(); // 距離計測タイマーストップ
+ DT = (int)(timer.read_us()*0.01657); // 距離計算
+ if(DT > 100){ // 検知範囲外なら100cmに設定
+ DT = 100;
+ }
+ timer.reset(); // 距離計測タイマーリセット
}
- timer.stop(); // 距離計測タイマーストップ
- DT = (int)(timer.read_us()*0.01657); // 距離計算
- if(DT > 100){ // 検知範囲外なら100cmに設定
- DT = 100;
+
+ if((t1-t2)>=10){
+ DT=100;
}
-
- timer.reset(); // 距離計測タイマーリセット
pc.printf("私はDTである。%d\r\n",DT);
led1 = 0;
return DT;
@@ -495,7 +508,7 @@
/* 障害物検知関数 */
void watchsurrounding(){
- pc.printf("watchsurrounding\r\n");
+ //pc.printf("watchsurrounding\r\n");
SC = watch();
if(SC < limit){ // 正面20cm以内に障害物がある場合
if(mode == AVOIDANCE) // 現在障害物回避モードなら
@@ -504,49 +517,49 @@
return;
}
servo.pulsewidth_us(1925); // サーボを左に40度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(25); // 250ms待つ
SLD = watch();
if(SLD < limit){ // 左前20cm以内に障害物がある場合
- if(mode == AVOIDANCE) // 現在障害物回避モードなら
+ //if(mode == AVOIDANCE) // 現在障害物回避モードなら
run = STOP; // 停止
- else
- return;
+ //else
+ //return;
}
servo.pulsewidth_us(2400); // サーボを左に90度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(25); // 250ms待つ
SL = watch();
if(SL < limit){ // 左20cm以内に障害物がある場合
- if(mode == AVOIDANCE) // 現在障害物回避モードなら
+ //if(mode == AVOIDANCE) // 現在障害物回避モードなら
run = STOP; // 停止
- else
- return;
+ //else
+ //return;
}
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
- ThisThread::sleep_for(100); // 100ms待つ
+ ThisThread::sleep_for(10); // 100ms待つ
servo.pulsewidth_us(925); // サーボを右に40度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(25); // 250ms待つ
SRD = watch();
if(SRD < limit){ // 右前20cm以内に障害物がある場合
- if(mode == AVOIDANCE) // 現在障害物回避モードなら
+ //if(mode == AVOIDANCE) // 現在障害物回避モードなら
run = STOP; // 停止
- else
- return;
+ //else
+ //return;
}
servo.pulsewidth_us(500); // サーボを右に90度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(25); // 250ms待つ
SR = watch();
if(SR < limit){ // 右20cm以内に障害物がある場合
- if(mode == AVOIDANCE) // 現在障害物回避モードなら
+ //if(mode == AVOIDANCE) // 現在障害物回避モードなら
run = STOP; // 停止
- else
- return;
+ //else
+ //return;
}
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
- ThisThread::sleep_for(100); // 100ms待つ
+ ThisThread::sleep_for(10); // 100ms待つ
if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
flag_a = 1; // 障害物有無フラグに1をセット
}
- pc.printf(" watchsurrounding\r\n");
+ //pc.printf(" watchsurrounding\r\n");
}
/* ディスプレイ表示関数 */
@@ -624,7 +637,7 @@
/* バッテリー残量更新関数 */
void bChange(){
- pc.printf(" bChange1\r\n");
+ //pc.printf(" bChange1\r\n");
b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10;
if(b <= 0){ // バッテリー残量0%なら全ての機能停止
b = 0;
@@ -635,7 +648,7 @@
mutex.lock(); // ミューテックスロック
lcd.setAddress(0,0);
lcd.printf("Battery:%3d%%",b); // バッテリー残量表示
- pc.printf(" bChange2\r\n");
+ //pc.printf(" bChange2\r\n");
mutex.unlock(); // ミューテックスアンロック
if(b <= 30){ // バッテリー残量30%以下なら
if(flag_t == 0){ // バックライトタイマーフラグが0なら
@@ -666,9 +679,9 @@
display(); // ディスプレイ表示
while(1){
- pc.printf(" main1\r\n");
+ //pc.printf(" main1\r\n");
bChange(); // バッテリー残量更新
- pc.printf(" main2\r\n");
+ //pc.printf(" main2\r\n");
ThisThread::sleep_for(1); // 1ms待つ
}
}
\ No newline at end of file