mbed_robotcar
赤子を殺すより楽な作業よ
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 6:4817fe7b7654
- Parent:
- 5:5414f85108aa
- Child:
- 7:4a65f5a25c13
--- a/main.cpp Wed Jul 29 06:36:04 2020 +0000
+++ b/main.cpp Wed Jul 29 07:57:16 2020 +0000
@@ -15,11 +15,11 @@
/* マクロ定義、列挙型定義 */
#define MIN_V 2.0
#define MAX_V 2.67
-#define LOW 0 //
-#define HIGH 1 //
-#define NORMAL 0 // フラグ値
-#define FAST 1 // フラグ値
-#define VERYFAST 2 // フラグ値
+#define LOW 0
+#define HIGH 1
+#define NORMAL 0
+#define FAST 1
+#define VERYFAST 2
enum MODE{
READY = -1,
@@ -109,24 +109,20 @@
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 thread1(decodeIR, NULL, osPriorityRealtime); //+3
+Thread thread2(motor, NULL, osPriorityHigh); //+2
+Thread thread3(avoidance, NULL, osPriorityHigh); //+2
+Thread thread4(trace, NULL, osPriorityHigh); //+2
-/* タイマー割り込み */
+/* バックライト点滅タイマー割り込み */
RtosTimer bTimer(lcdBacklight, osTimerPeriodic);
/* リモコン受信スレッド */
void decodeIR(void const *argument){
while(1){
- // 受信待ち
- pc.printf("decodeIR\r\n");
- if (ir.getState() == ReceiverIR::Received) {
- // コード受信
- bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
- if(bitcount > 1){
- // 受信成功
+ if (ir.getState() == ReceiverIR::Received){ // 受信待ち
+ bitcount = ir.getData(&format, buf, sizeof(buf) * 8); // コード受信
+ if(bitcount > 1){ // 受信成功
code=0;
for(int j=0;j<4;j++){
code+=(buf[j]<<(8*(3-j)));
@@ -135,49 +131,41 @@
beforeMode=mode;
}
switch(code){
- case 0x40bf27d8://クイック
- pc.printf("mode = SPEED\r\n");
+ case 0x40bf27d8: //クイック
mode = SPEED;
changeSpeed();
display();
mode = beforeMode;
break;
- case 0x40be34cb://レグザリンク
- pc.printf("mode = LINE_TRACE\r\n");
+ case 0x40be34cb: //レグザリンク
mode=LINE_TRACE;
display();
break;
- case 0x40bf6e91://番組表
- pc.printf("mode = AVOIDANCE\r\n");
+ case 0x40bf6e91: //番組表
mode=AVOIDANCE;
display();
break;
- case 0x40bf3ec1://↑
- pc.printf("mode = ADVANCE\r\n");
+ case 0x40bf3ec1: //↑
mode = ADVANCE;
run = ADVANCE;
display();
break;
- case 0x40bf3fc0://↓
- pc.printf("mode = BACK\r\n");
+ case 0x40bf3fc0: //↓
mode = BACK;
run = BACK;
display();
break;
- case 0x40bf5fa0://←
- pc.printf("mode = LEFT\r\n");
+ case 0x40bf5fa0: //←
mode = LEFT;
run = LEFT;
display();
break;
- case 0x40bf5ba4://→
- pc.printf("mode = RIGHT\r\n");
+ case 0x40bf5ba4: //→
mode = RIGHT;
run = RIGHT;
display();
break;
- case 0x40bf3dc2://決定
- pc.printf("mode = STOP\r\n");
+ case 0x40bf3dc2: //決定
mode = STOP;
run = STOP;
display();
@@ -199,10 +187,6 @@
/* モータースレッド */
void motor(void const *argument){
while(1){
- pc.printf("motor\r\n");
- if(flag_sp >VERYFAST){
- flag_sp -= 3;
- }
switch(run){
case ADVANCE:
motorR1 = motorSpeed[flag_sp];
@@ -235,18 +219,17 @@
motorL2 = LOW;
break;
}
- pc.printf(" motor\r\n");
ThisThread::sleep_for(30);
}
}
/* スピード変更関数 */
void changeSpeed(){
- if(flag_sp == VERYFAST){
- flag_sp = NORMAL;
+ if(flag_sp == VERYFAST){ // 最速の場合
+ flag_sp = NORMAL; // 普通に設定
}
- else{
- flag_sp = flag_sp + 1;
+ else{ // 最速以外の場合
+ flag_sp = flag_sp + 1; // 1段階上げる
}
}
@@ -254,7 +237,6 @@
void trace(void const *argument){
while(1){
if(mode==LINE_TRACE){
- pc.printf("line trace\r\n");
// センサー値読み取り
int sensor1 = ss1;
int sensor2 = ss2;
@@ -292,7 +274,6 @@
run = LEFT;
break;
}
- pc.printf(" line trace\r\n");
ThisThread::sleep_for(30);
}else{
ThisThread::sleep_for(1);
@@ -304,8 +285,7 @@
void avoidance(void const *argument){
while(1){
if(mode==AVOIDANCE){
- pc.printf("avoidance\r\n");
- if(flag_a == 0){ // 障害物がない場合
+ if(flag_a == 0){ // 障害物がない場合
run = ADVANCE; // 前進
}
else{ // 障害物がある場合
@@ -313,13 +293,18 @@
if(SC < 15){ // 正面15cm以内に障害物が現れた場合
run = BACK; // 後退
while(watch() < limit){ // 正面20cm以内に障害物がある間
-
+ if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
+ break; // avoidanceから抜ける
+ }
}
run = STOP; // 停止
}
if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
run = LEFT; // 左折
- while(i < 10){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
+ if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
+ break; // avoidanceから抜ける
+ }
if(watch() > limit){
i++;
}
@@ -353,11 +338,14 @@
switch(houkou){ // 進行方向の場合分け
case 1: // 前の場合
run = ADVANCE; // 前進
- thread_sleep_for(100); // 1秒待つ
+ ThisThread::sleep_for(1000); // 1秒待つ
break;
case 2: // 左の場合
run = LEFT; // 左折
- while(i < 10){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
+ if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
+ break; // avoidanceから抜ける
+ }
if(watch() > (far - 2)){
i++;
}
@@ -369,7 +357,10 @@
break;
case 3: // 右の場合
run = RIGHT; // 右折
- while(i < 10){ // 進行方向確認
+ while(i < 10){ // 進行方向確認
+ if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら
+ braek; // avoidanceから抜ける
+ }
if(watch() > (far - 2)){
i++;
}
@@ -384,50 +375,45 @@
}
flag_a = 0; // 障害物有無フラグを0にセット
watchsurrounding();
- pc.printf(" avoidance\r\n");
}else{
- ThisThread::sleep_for(1);
+ ThisThread::sleep_for(1); // 1ms待つ
}
}
}
int watch(){
- 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){
+ }
timer.start(); // 距離計測タイマースタート
while(echo.read() == 1){
}
timer.stop(); // 距離計測タイマーストップ
DT = timer.read_us()*0.01657; // 距離計算
- if(DT > 400){ // 検知範囲外なら400cmに設定
- DT = 400;
+ if(DT > 100){ // 検知範囲外なら400cmに設定
+ DT = 100;
}
timer.reset(); // 距離計測タイマーリセット
- //ThisThread::sleep_for(30); // 30ms待つ
- pc.printf(" watch\r\n");
return DT;
}
void watchsurrounding(){
- pc.printf("watchsurrounding\r\n");
SC = watch();
if(SC < limit){ // 正面20cm以内に障害物がある場合
run = STOP; // 停止
}
servo.pulsewidth_us(1925); // サーボを左に40度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(250); // 250ms待つ
SLD = watch();
if(SLD < limit){ // 左前20cm以内に障害物がある場合
run = STOP; // 停止
}
servo.pulsewidth_us(2400); // サーボを左に90度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(250); // 250ms待つ
SL = watch();
if(SL < limit){ // 左20cm以内に障害物がある場合
run = STOP; // 停止
@@ -435,23 +421,22 @@
servo.pulsewidth_us(1450);
ThisThread::sleep_for(100);
servo.pulsewidth_us(925); // サーボを右に40度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(250); // 250ms待つ
SRD = watch();
if(SRD < limit){ // 右前20cm以内に障害物がある場合
run = STOP; // 停止
}
servo.pulsewidth_us(500); // サーボを右に90度回転
- ThisThread::sleep_for(250); // 250ms待つ
+ ThisThread::sleep_for(250); // 250ms待つ
SR = watch();
if(SR < limit){ // 右20cm以内に障害物がある場合
run = STOP; // 停止
}
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
- ThisThread::sleep_for(100); // 100ms待つ
+ 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");
}
/* ディスプレイ表示関数 */
@@ -515,9 +500,7 @@
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());
if(b < 0){//すべての機能停止(今はなし)
/*lcd.setBacklight(TextLCD::LightOff);
bTimer.stop();
@@ -530,7 +513,6 @@
mutex.lock();
lcd.setAddress(0,0);
lcd.printf("Battery:%3d%%",b);
- pc.printf(" bChange2\r\n");
mutex.unlock();
if(b <= 30){
if(flag_t == 0){
@@ -556,9 +538,7 @@
flag_sp = NORMAL;
display();
while(1){
- pc.printf(" main1\r\n");
bChange();
- pc.printf(" main3\r\n");
ThisThread::sleep_for(1);
}
}
\ No newline at end of file