mbed_robotcar
/
robotcarfinish
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:
- 5:3fffb364744b
- Parent:
- 4:3f80c0180e2f
- Child:
- 6:800a745c7f2e
--- a/main.cpp Wed Jul 29 03:10:44 2020 +0000
+++ b/main.cpp Wed Jul 29 06:52:39 2020 +0000
@@ -36,7 +36,7 @@
/* ピン配置 */
ReceiverIR ir(p5); // リモコン操作
DigitalOut trig(p6); // 超音波センサtrigger
-DigitalOut echo(p7); // 超音波センサecho
+DigitalIn echo(p7); // 超音波センサecho
DigitalIn ss1(p8); // ライントレースセンサ(左)
DigitalIn ss2(p9); // ライントレースセンサ
DigitalIn ss3(p10); // ライントレースセンサ
@@ -87,7 +87,7 @@
int SR; // 右
int SLD; // 左前
int SRD; // 右前
-int flag = 0; // 障害物有無のフラグ
+int flag_a = 0; // 障害物有無のフラグ
const int limit = 20; // 障害物の距離のリミット(単位：cm)
int far; // 最も遠い距離
int houkou; // 進行方向(1:前 2:左 3:右)
@@ -103,19 +103,20 @@
void trace(void const *argument);
void watchsurrounding();
int watch();
-void bChange(/*void const *argument*/);
+void bChange();
void display();
void lcdBacklight(void const *argument);
Thread thread1(decodeIR, NULL, osPriorityRealtime);//+3
Thread thread2(motor, NULL, osPriorityHigh);//+2
Thread thread3(avoidance, NULL, osPriorityHigh);//+2
Thread thread4(trace, NULL, osPriorityHigh);//+2
-//Thread thread5(bChange, NULL, osPriorityBelowNormal);//-1
RtosTimer bTimer(lcdBacklight, osTimerPeriodic);
+DigitalOut led1(LED1);
- void decodeIR(void const *argument){
- while(1){
+
+void decodeIR(void const *argument){
+ while(1){
// 受信待ち
pc.printf("decodeIR\r\n");
if (ir.getState() == ReceiverIR::Received) {
@@ -186,17 +187,10 @@
}
}
if(viewTimer.read_ms()>=3000){
- //pc.printf("a");
viewTimer.stop();
- //defaultView();
viewTimer.reset();
display();
- //flaga=0;
}
- //pc.printf(" main2\r\n");
- //if(viewTimer.read_ms()==0){
- // display();
- //}
ThisThread::sleep_for(90);
}
}
@@ -303,24 +297,32 @@
void avoidance(void const *argument){
while(1){
if(mode==AVOIDANCE){
- pc.printf("avoidance\r\n");
- if(flag == 0){ // 障害物がない場合
+ pc.printf("avoidance\r\n");
+ pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR);
+
+ if(flag_a == 0){ // 障害物がない場合
run = ADVANCE; // 前進
}
- /*else{ // 障害物がある場合
+ else{ // 障害物がある場合
i = 0;
if(SC < 15){ // 正面15cm以内に障害物が現れた場合
run = BACK; // 後退
while(watch() < limit){ // 正面20cm以内に障害物がある間
-
+ if(mode!=AVOIDANCE){
+ break;
+ }
}
run = STOP; // 停止
}
if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
run = LEFT; // 左折
- while(i < 100){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
+ if(mode!=AVOIDANCE){
+ break;
+ }
if(watch() > limit){
- i++;
+ i++;
+
}
else{
i = 0;
@@ -352,13 +354,19 @@
switch(houkou){ // 進行方向の場合分け
case 1: // 前の場合
run = ADVANCE; // 前進
+ pc.printf("Advance\r\n");
thread_sleep_for(100); // 1秒待つ
break;
case 2: // 左の場合
run = LEFT; // 左折
- while(i < 100){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
+ pc.printf("left\r\n");
+ if(mode!=AVOIDANCE){
+ break;
+ }
if(watch() > (far - 2)){
i++;
+
}
else{
i = 0;
@@ -368,9 +376,14 @@
break;
case 3: // 右の場合
run = RIGHT; // 右折
- while(i < 100){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
+ pc.printf("right\r\n");
+ if(mode!=AVOIDANCE){
+ break;
+ }
if(watch() > (far - 2)){
i++;
+
}
else{
i = 0;
@@ -380,8 +393,9 @@
break;
}
}
- }*/
- flag = 0; // フラグを0にセット
+ }
+ pc.printf("こんにちは！\r\n");
+ flag_a = 0; // フラグを0にセット
watchsurrounding();
pc.printf(" avoidance\r\n");
}else{
@@ -390,25 +404,31 @@
}
}
int watch(){
+ led1=0;
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){
- //}
+ while(echo.read() == 0){
+ led1=1;
+ if(mode!=AVOIDANCE){
+ break;
+ }
+ }
timer.start(); // 距離計測タイマースタート
while(echo.read() == 1){
}
timer.stop(); // 距離計測タイマーストップ
- DT = timer.read_us()*0.01657; // 距離計算
- if(DT > 400){ // 検知範囲外なら400cmに設定
- DT = 400;
+ DT = (int)(timer.read_us()*0.01657); // 距離計算
+ if(DT > 100){ // 検知範囲外なら400cmに設定
+ DT = 100;
}
+
timer.reset(); // 距離計測タイマーリセット
//ThisThread::sleep_for(30); // 30ms待つ
- pc.printf(" watch\r\n");
+ pc.printf("私はDTである。%d\r\n",DT);
return DT;
}
@@ -447,7 +467,7 @@
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
ThisThread::sleep_for(100); // 100ms待つ
if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
- flag = 1; // フラグに1をセット
+ flag_a = 1; // フラグに1をセット
}
pc.printf(" watchsurrounding\r\n");
}
@@ -469,11 +489,11 @@
break;
case RIGHT:
//strcpy(DispMode,"Mode:Right");
- lcd.printf("Mode:Right ");
+ lcd.printf("Mode:TurnRight ");
break;
case LEFT:
//strcpy(DispMode,"Mode:Left");
- lcd.printf("Mode:Left ");
+ lcd.printf("Mode:TurnLeft ");
break;
case BACK:
//strcpy(DispMode,"Mode:Back");
@@ -524,35 +544,16 @@
flag_b = !flag_b;
}
-void bChange(/*void const *argument*/){
- /*while(1){
- pc.printf(" bChange1\r\n");
- lcd.setBacklight(TextLCD::LightOn);
- test = battery.read()*MAX_V;
- b = (int)((test - MIN_V)/0.107 + 0.5)*10;
- if(b < 0){
- b = 0;
- }else if(b > 100){
- b = 100;
- }
- mutex.lock();
- lcd.setAddress(0,0);
- lcd.printf("Battery:%3d%% ",b);
- pc.printf(" bChange2\r\n");
- mutex.unlock();
- }*/
+void bChange(){
lcd.setBacklight(TextLCD::LightOn);
while(1){
pc.printf(" bChange1\r\n");
b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10;
- pc.printf("%f",battery.read());
+ //pc.printf("%f",battery.read());
if(b < 0){//すべての機能停止(今はなし)
- /*lcd.setBacklight(TextLCD::LightOff);
- bTimer.stop();
- exit(1);*/
b = 0;
- }else if(b > 100){
- b = 100;
+ }else if(b > 50){
+ b = 50;
}
mutex.lock();
lcd.setAddress(0,0);
@@ -580,50 +581,11 @@
run = STOP;
flag_sp = NORMAL;
display();
- //defaultView();
while(1){
pc.printf(" main1\r\n");
- // 受信待ち
- /*switch(mode){
- case ADVANCE:
- run = ADVANCE;
- break;
- case RIGHT:
- run = RIGHT;
- break;
- case LEFT:
- run = LEFT;
- break;
- case BACK:
- run = BACK;
- break;
- case STOP:
- run = STOP;
- break;
- case READY:
- run = STOP;
- break;
- case SPEED:
- changeSpeed();
- display();
- mode = beforeMode;
- break;
- }*/
-
-/* if(viewTimer.read_ms()>=3000){
- pc.printf("a");
- viewTimer.stop();
- //defaultView();
- viewTimer.reset();
- //flaga=0;
- }
+ bChange();
pc.printf(" main2\r\n");
- if(viewTimer.read_ms()==0){
- display();
- }*/
- bChange();
- pc.printf(" main3\r\n");
ThisThread::sleep_for(1);
}
}
\ No newline at end of file