SS2
test
Diff: main.cpp
- Revision:
- 5:b6daf80a7237
- Parent:
- 4:04ca6a211b85
--- a/main.cpp Tue Dec 12 00:34:36 2017 +0000
+++ b/main.cpp Mon Jan 15 07:02:03 2018 +0000
@@ -1,6 +1,11 @@
#include "mbed.h"
#include "hcsr04.h"
+
+AnalogIn photoref_L(A1); // フォトリフレクタ左
+AnalogIn photoref_M(A2); // フォトリフレクタ中央
+AnalogIn photoref_R(A3); // フォトリフレクタ右
+
PwmOut mypwm4(D4);
PwmOut mypwm6(D6);
PwmOut mypwm2(D2);
@@ -24,15 +29,16 @@
#define STATE_E (4)
#define STATE_A2 (5)
#define STATE_A3 (6)
-
+#define STATE_A4 (7)
+#define STATE_D2 (8)
-char mode,fase;
+char mode,fase,linetrace;
int wait_flag;
-int timer;
+double timer,measL,measM,measR;
void tflg(){
- timer = t.read();
+ timer = t.read(); //タイマー値読み込み
}
/*void flg(){
usensor.start();
@@ -53,102 +59,180 @@
mypwm2.period_ms(100);
mypwm3.period_ms(100);
-timerflag.attach(&tflg, 1.0);
+timerflag.attach(&tflg, 0.1);
/*flag.attach(&flg, 1.0);*/
while(1){
usensor1.start();
-usensor2.start();
- wait(0.2);
+usensor2.start();
dist1=usensor1.get_dist_cm();
dist2=usensor2.get_dist_cm();
- /*printf("dist1: %ld\n",dist1); */
+ measL = photoref_L.read();
+ measM = photoref_M.read();
+ measR = photoref_R.read();
+ wait(0.1);
+ printf("dist1: %ld\n",dist1);
printf("dist2: %ld\n",dist2);
-
+ printf("fase: %ld\n",fase);
+ printf("timer: %lf\n",timer);
- /* switch(timer)
-{
- case 8.0:
- wait_flag = 1;
- mode = STATE_A;
- break;
- case 12.0:
- wait_flag = 1;
- mode = STATE_B;
- break;
- case 15.5:
- wait_flag = 1;
- mode = STATE_A2;
- break;
- case 22.0:
- wait_flag = 1;
- mode = STATE_D;
- break;
- case 30.0:
- wait_flag = 1;
- mode = STATE_A;
- break;
- case 37.0:
- wait_flag = 1;
- mode = STATE_C;
- break;
- case 42.0:
- wait_flag = 1;
- mode = STATE_A;
- break;
-
-}*/
+
+
switch (fase)
- {
- case 0:
- /*printf("timer: %d\n", timer);*/
- if(timer > 5){
- /*if(dist2 > 8){
- mode = STATE_A3; //hidariに
- }else if(dist2 < 10){
- mode = STATE_A2; //migiに
- }else{mode = STATE_A;}
- }*/if(dist1 > 45 && dist1<250){
- fase = 1;
+ { case 0:
+ if(timer > 5.0){
+ mode = STATE_A2;
+ if(dist1 > 45 && dist1 < 100){
+ fase = 1;
+ t.stop();
+ t.reset();
+ }
+ }
+ break;
+ /* switch(linetrace){
+ case 0:
+ if(timer > 5 && measL>0.2 && measM>0.2 && measR>0.2){
+ mode = STATE_A4;
+ }if((timer > 5 && measL<0.2 && measM<0.2) || (timer > 5 && measM<0.2 && measR<0.2)){
+ linetrace = 1;
+ t.stop();
+ t.reset();
+ }
+ break;
+
+ case 1:
+ t.start();
+ if(measL<0.2 && measM>0.2){
+ mode = STATE_A3;
+ }else if(measM<0.2 && measL>0.2){
+ mode = STATE_A2;
+ }else if(measL>0.2 && measM>0.2){
+ if(dist2 < 10){
+ mode = STATE_A2;
+ }else if(dist2 > 10){
+ mode = STATE_A3;
+ }
+ }else if((timer > 1 && measL<0.2 && measM<0.2) || (timer > 1 && measM<0.2 && measR<0.2) ){
+ linetrace = 2;
+ t.stop();
+ t.reset();
+ }
+ break;
+
+ case 2:
+ t.start();
+ mode = STATE_B;
+ if((timer > 2 && measL<0.2 && measM<0.2) || (timer > 1 && measM<0.2 && measR<0.2)){
+ fase = 2;
t.stop();
t.reset();
- t.start();
- }
- break;
+ } //if閉じ
+ } //switch閉じ
+ break;*/
+
case 1:
- if(timer == 0){
- wait_flag = 1;
- mode = STATE_B;
- }else if(timer > 2 && dist1 > 60){
+ if(timer == 0){
+ wait_flag = 1;
+ mode = STATE_B;
+ t.start();
+ }else if(timer > 2.0 && dist1 > 60 && dist1 < 250){
wait_flag = 1;
mode = STATE_A;
+ t.stop();
t.reset();
- t.stop();
fase = 2;
- t.start();
}
break;
case 2:
- if(timer == 6.0){
+ t.start();
+ if(timer > 10.0 && timer < 11.0){
wait_flag = 1;
mode = STATE_D;
- }else if(timer == 16.0){
+ fase = 3;
+ t.stop();
+ t.reset();
+ }/*else if (timer > 10.0 && timer < 11.0 && dist2 < 8 ){
+ wait_flag = 1;
+ mode = STATE_D2;
+ fase = 3;
+ t.stop();
+ t.reset();
+ }*/
+ break;
+ case 3:
+ t.start();
+ /* if(timer > 15.0 && timer < 16.0 && dist2 > 8 && dist2 < 100){
+ wait_flag = 1;
+ mode = STATE_A4;
+ fase = 4;
+ t.stop();
+ t.reset();
+ }else */
+ if (timer > 15.0 && timer < 16.0){
+ wait_flag = 1;
+ mode = STATE_A3;
+ fase = 4;
+ t.stop();
+ t.reset();
+ }
+ break;
+
+ case 4:
+ /*if(measL <0.2 || measM <0.2 || measR <0.2){
+ wait_flag = 1;
+ mode = STATE_C;
+ fase = 5;
+ }*/
+ if(dist1 > 45 && dist1 < 100){
+ fase = 5;}
+
+ break;
+ case 5:
+ wait_flag = 1;
+ mode = STATE_C;
+ t.start();
+ if(timer > 2.0 && dist1 > 60 && dist1 < 250){
+ t.stop();
+ t.reset();
+ fase = 6;
+ }
+ break;
+
+ case 6:
+ t.start();
+ if(timer <2.0){
+ wait_flag = 1;
+ mode = STATE_A;
+ }else{
+ wait_flag = 1;
+ mode = STATE_A4;}
+ break;
+
+
+
+
+
+
+
+
+
+ /* else if(timer > 20.0 && timer < 21.0){
wait_flag = 1;
mode = STATE_A;
- }else if(timer == 26.0){
+ }else if(timer > 30.0 && timer < 31.0){
wait_flag = 1;
mode = STATE_D;
- }else if(timer == 36.0){
+ }else if(timer > 40.0 && timer < 41.0){
wait_flag = 1;
mode = STATE_A;
- }else if(timer == 46.0){
+ }else if(timer > 50.0 && timer < 51.0){
wait_flag = 1;
mode = STATE_D;}
+ break; */
}
-
// 状態が切り替わるときは一時停止
if(wait_flag==1)
{
@@ -170,77 +254,96 @@
// STATE_A : 前進（左：正転 右：正転）
case STATE_A:
myled = 1; // LED点灯
- // 左モータの制御
- mypwm4.pulsewidth_ms(70);
+ // 右モータの制御
+ mypwm4.pulsewidth_ms(40);
mypwm6.pulsewidth_ms(0);
- // 右モータの制御
- mypwm2.pulsewidth_ms(70);
+ // 左モータの制御
+ mypwm2.pulsewidth_ms(38);
mypwm3.pulsewidth_ms(0);
break;
// STATE_A2 : 右に曲がる（左：正転 右：正転）
case STATE_A2:
myled = 1; // LED点灯
- // 左モータの制御
- mypwm4.pulsewidth_ms(50);
+ // 右モータの制御
+ mypwm4.pulsewidth_ms(55);
mypwm6.pulsewidth_ms(0);
- // 右モータの制御
- mypwm2.pulsewidth_ms(80);
+ // 左モータの制御
+ mypwm2.pulsewidth_ms(38);
mypwm3.pulsewidth_ms(0);
break;
// STATE_A3 : hidariに曲がる（左：正転 右：正転）
case STATE_A3:
myled = 1; // LED点灯
+ // 右モータの制御
+ mypwm4.pulsewidth_ms(40);
+ mypwm6.pulsewidth_ms(0);
// 左モータの制御
- mypwm4.pulsewidth_ms(80);
+ mypwm2.pulsewidth_ms(55);
+ mypwm3.pulsewidth_ms(0);
+ break;
+ case STATE_A4:
+ myled = 1; // LED点灯
+ // 右モータの制御
+ mypwm4.pulsewidth_ms(50);
mypwm6.pulsewidth_ms(0);
+ // 左モータの制御
+ mypwm2.pulsewidth_ms(40);
+ mypwm3.pulsewidth_ms(0);
+ break;
+ // STATE_B : 左旋回（左：正転 右：逆転）
+ case STATE_B:
+ myled = 0; // LED点灯
// 右モータの制御
- mypwm2.pulsewidth_ms(10);
- mypwm3.pulsewidth_ms(0);
+ mypwm4.pulsewidth_ms(30);
+ mypwm6.pulsewidth_ms(0);
+ // 左モータの制御
+ mypwm2.pulsewidth_ms(0);
+ mypwm3.pulsewidth_ms(30);
break;
- // STATE_B : 右旋回（左：正転 右：逆転）
- case STATE_B:
- myled = 1; // LED点灯
- // 左モータの制御
- mypwm4.pulsewidth_ms(100);
- mypwm6.pulsewidth_ms(0);
+ // STATE_C : 右旋回（左：逆転 右：正転）
+ case STATE_C:
+ myled = 0; // LED点灯
// 右モータの制御
- mypwm2.pulsewidth_ms(0);
- mypwm3.pulsewidth_ms(100);
- break;
-
- // STATE_C : 左旋回（左：逆転 右：正転）
- case STATE_C:
- myled = 1; // LED点灯
+ mypwm4.pulsewidth_ms(0);
+ mypwm6.pulsewidth_ms(40);
// 左モータの制御
- mypwm4.pulsewidth_ms(0);
- mypwm6.pulsewidth_ms(100);
- // 右モータの制御
- mypwm2.pulsewidth_ms(100);
+ mypwm2.pulsewidth_ms(40);
mypwm3.pulsewidth_ms(0);
break;
// STATE_D : 後退（左：逆転 右：逆転）
case STATE_D:
myled = 1; // LED点灯
- // 左モータの制御
+ // 右モータの制御
mypwm4.pulsewidth_ms(0);
- mypwm6.pulsewidth_ms(80);
- // 右モータの制御
+ mypwm6.pulsewidth_ms(50);
+ // 左モータの制御
mypwm2.pulsewidth_ms(0);
- mypwm3.pulsewidth_ms(83);
+ mypwm3.pulsewidth_ms(48);
break;
+ // STATE_D : 後退（左：逆転 右：逆転）
+ case STATE_D2:
+ myled = 1; // LED点灯
+ // 右モータの制御
+ mypwm4.pulsewidth_ms(0);
+ mypwm6.pulsewidth_ms(47);
+ // 左モータの制御
+ mypwm2.pulsewidth_ms(0);
+ mypwm3.pulsewidth_ms(48);
+ break;
// STATE_E : ブレーキ（左：ブレーキ 右：ブレーキ）
case STATE_E:
myled = 0; // LED消灯
- // 左モータの制御
+ // 右モータの制御
mypwm4.pulsewidth_ms(60);
mypwm6.pulsewidth_ms(60);
- // 右モータの制御
+ // 左モータの制御
mypwm2.pulsewidth_ms(60);
mypwm3.pulsewidth_ms(60);
break;
}
}
}
+