mbed_robotcar
linetrace saikyou
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 40:75e1ad7c27e4
- Parent:
- 39:054c17d10c55
- Child:
- 41:3c58a4be1199
--- a/main.cpp Wed Aug 19 01:53:11 2020 +0000
+++ b/main.cpp Wed Aug 19 06:07:00 2020 +0000
@@ -58,7 +58,7 @@
int beforeMode; // 前回のモード
int flag_sp = 0; // スピード変化フラグ
Timer viewTimer; // スピ―ド変更時に3秒計測タイマー
-float motorSpeed[9] = {0.6, 0.7, 0.8, 0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
+float motorSpeed[9] = {0.4, 0.7, 0.8, 0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
// モーター速度設定(後半はライントレース用)
Mutex mutex; // ミューテックス
@@ -121,7 +121,8 @@
void changeSpeed();
void avoidance(/*void const *argument*/);
void trace(/*void const *argument*/);
-void watchsurrounding();
+void watchsurrounding3();
+void watchsurrounding5();
int watch();
void bChange();
void display();
@@ -361,7 +362,7 @@
/* 障害物回避走行スレッド */
void avoidance(){
while(1){
- watchsurrounding();
+ watchsurrounding3();
pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR);
if(flag_a == 0){ // 障害物がない場合
run = ADVANCE; // 前進
@@ -369,14 +370,31 @@
i = 0;
if(SC < 15){ // 正面15cm以内に障害物が現れた場合
run = BACK; // 後退
- while(watch() < limit){ // 正面20cm以内に障害物がある間
+ int cnt_kyori=0;
+ int kyori = watch();
+ while(kyori < limit){ // 正面20cm以内に障害物がある間
+ if(kyori==-1){
+ cnt_kyori++;
+ if(cnt_kyori>15){
+ cnt_kyori=0;
+ break;
+ }
+ }
+ kyori = watch();
}
- run = STOP; // 停止
+ /*while(i < 30){ // 正面20cm以内に障害物がある間
+ if(watch() < limit){
+ break;
+ }
+ i++;
+ }
+ i = 0;*/
+ run = STOP; // 停止
}
+ watchsurrounding5();
if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
run = LEFT; // 左折
while(i < 1){ // 進行方向確認
- flag_sp = flag_sp % 3 + 6;
if(watch() > limit){
i++;
}else{
@@ -406,30 +424,46 @@
switch(houkou){ // 進行方向の場合分け
case 1: // 前の場合
run = ADVANCE; // 前進
- ThisThread::sleep_for(1000); // 1秒待つ
+ ThisThread::sleep_for(500); // 0.5秒待つ
break;
- case 2: // 左の場合
-
+ case 2: // 左の場合
run = LEFT; // 左折
- while(i < 1){ // 進行方向確認
- flag_sp = flag_sp % 3 + 6;
+ //int kyori = watch();
+ //int kyori_f=0;
+ while(i < 20){ // 進行方向確認
+ /*if(kyori > (far - 2) || kyori_f == 2){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
+ break; // ループ+
+ }else if(kyori==-1){
+ kyori_f++;
+ }else{
+ kyori_f = 0;
+ i++;
+ }*/
if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
- i++; // ループ+
+ break; // ループ+
}else{
- i = 0;
+ i++;
}
}
run = STOP; // 停止
break;
- case 3: // 右の場合
-
+ case 3: // 右の場合
run = RIGHT; // 右折
- while(i < 1){ // 進行方向確認
- flag_sp = flag_sp % 3 + 6;
+ int kyori = watch();
+ int kyori_f=0;
+ while(i < 20){ // 進行方向確認
+ /*if(kyori > (far - 2) || kyori_f == 2){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
+ break; // ループ+
+ }else if(kyori==-1){
+ kyori_f++;
+ }else{
+ kyori_f = 0;
+ i++;
+ }*/
if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
- i++; // ループ+
+ break; // ループ+
}else{
- i = 0;
+ i++;
}
}
run = STOP; // 停止
@@ -438,6 +472,15 @@
}
}
flag_a = 0; // 障害物有無フラグを0にセット
+ if(SLD < 29){ // 正面15cm以内に障害物が現れた場合
+ run = RIGHT; // 右折
+ ThisThread::sleep_for(100); // 100ms待つ
+ run = STOP; // 停止
+ }else if(SRD < 29){
+ run = LEFT; // 左折
+ ThisThread::sleep_for(100); // 100ms待つ
+ run = STOP; // 停止
+ }
}
}
@@ -476,14 +519,15 @@
}
/* 障害物検知関数 */
-void watchsurrounding(){
+void watchsurrounding3(){
//servo.pulsewidth_us(1450); // サーボを中央位置に戻す
//ThisThread::sleep_for(200); // 100ms待つ
SC = watch();
if(SC < limit){ // 正面20cm以内に障害物がある場合
if(SC!=-1){
run = STOP; // 停止
- flag_a = 1;
+ flag_a = 1;
+ return;
}
}
servo.pulsewidth_us(1925); // サーボを左に40度回転
@@ -492,21 +536,16 @@
if(SLD < limit){ // 左前20cm以内に障害物がある場合
run = STOP; // 停止
flag_a = 1;
- }
- servo.pulsewidth_us(2400); // サーボを左に90度回転
- ThisThread::sleep_for(100); // 250ms待つ
- SL = watch();
- if(SL < limit){ // 左20cm以内に障害物がある場合
- run = STOP; // 停止
- flag_a = 1;
+ return;
}
servo.pulsewidth_us(1450);
- ThisThread::sleep_for(250);
+ ThisThread::sleep_for(100);
SC = watch();
if(SC < limit){
if(SC!=-1){
run = STOP; // 停止
- flag_a = 1;
+ flag_a = 1;
+ return;
}
}
servo.pulsewidth_us(925); // サーボを右に40度回転
@@ -515,19 +554,37 @@
if(SRD < limit){ // 右前20cm以内に障害物がある場合
run = STOP; // 停止
flag_a = 1;
+ return;
}
+ servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ ThisThread::sleep_for(100); // 100ms待つ
+ /*if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
+ flag_a = 1; // 障害物有無フラグに1をセット
+ }*/
+}
+
+/* 障害物検知関数 */
+void watchsurrounding5(){
+ //servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ //ThisThread::sleep_for(200); // 100ms待つ
+ SC = watch();
+ servo.pulsewidth_us(1925); // サーボを左に40度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SLD = watch();
+ servo.pulsewidth_us(2400); // サーボを左に90度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SL = watch();
+ servo.pulsewidth_us(1450);
+ ThisThread::sleep_for(250);
+ SC = watch();
+ servo.pulsewidth_us(925); // サーボを右に40度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SRD = watch();
servo.pulsewidth_us(500); // サーボを右に90度回転
ThisThread::sleep_for(100); // 250ms待つ
SR = watch();
- if(SR < limit){ // 右20cm以内に障害物がある場合
- run = STOP; // 停止
- flag_a = 1;
- }
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
ThisThread::sleep_for(250); // 100ms待つ
- /*if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
- flag_a = 1; // 障害物有無フラグに1をセット
- }*/
}
/* ディスプレイ表示関数 */