Revision 9:71b780f8d345, committed 2021-12-21

Comitter:

Tomo1213

Date:

Tue Dec 21 00:59:40 2021 +0000

Parent:

8:dd676df43fe7

Commit message:

hobokansei

Changed in this revision

--- a/main.cpp	Mon Dec 20 23:49:12 2021 +0000
+++ b/main.cpp	Tue Dec 21 00:59:40 2021 +0000
@@ -19,7 +19,7 @@
 #define DEC_CLEAN 1000
 
 #define KICK 2000
-#define CLEAN_OFFSET 30
+#define CLEAN_OFFSET 3
 
 #define WALL 45
 #define WALL_MIN 25
@@ -32,7 +32,9 @@
 DigitalOut led4(LED4);
  
 Timer t;
+Timer Clean_right;
 double Time = 0;
+double Clean_right_time = 0;
 
 Serial pc(USBTX, USBRX);
 char Serialdata;
@@ -191,16 +193,21 @@
     Init_Pos = ActPos;//起動時の角度を保存
     t.reset();
     t.start();
+    Clean_right.reset();
+    
 
     //set_MODE_T();
 
     printf("\nstart\r\n");
+    LED = 0b1111;
 
     while(1){
                 
         Time = t.read();
+        Clean_right_time = Clean_right.read();
+        
         //pc.printf("state_1:%d  state_2:%d\r\n",state_1,state_2);
-        pc.printf("state_1:%d state_2:%d ACT:%d TMP:%d \r\n",state_1,state_2,ActPos,X_DIST_TMP);
+        pc.printf("state_1:%d state_2:%d ACT:%d TMP:%d \r\n",state_1,state_2,dist3,X_DIST_TMP);
         readActPos(1);
         ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
         if(ActPos > 8388608){
@@ -220,12 +227,15 @@
             }else{   
                 if(ActPos < (Init_Pos - KICK)){ //前入力検出
                     ride_count++; 
+                    LED = 0b0111;
                     state_1 = 1;
                 }else if(ActPos > (Init_Pos + KICK)){ //右入力検出
                     ride_count++;
+                    LED = LED = 0b1101;;
                     state_1 = 11;
                 }else{
                     set_MODE_T();
+                    LED = 0b1111;
                 }
             }
             
@@ -233,6 +243,7 @@
             if(dist1 < WALL && dist1 >= WALL_MIN){
                 vel_stop();
                 wait(GETOFF_TIME);
+                LED = 0b1011;
                 state_1 = 2;
             }else{
                 set_ACC(ACC_RIDE);//加速度設定
@@ -243,6 +254,7 @@
         }else if(state_1 == 2){//前進からの帰還フェーズ
             if(ActPos > -3000){
                 vel_stop();
+                LED = 0b1111;
                 t.reset();
                 t.start();
                 state_1 = 0;
@@ -254,6 +266,7 @@
             if(dist3 < WALL && dist3 >= WALL_MIN){
                 vel_stop();
                 wait(GETOFF_TIME);
+                LED = 0b1110;
                 state_1 = 12;
             }else{
                 set_ACC(ACC_RIDE);//加速度設定
@@ -264,6 +277,7 @@
         }else if(state_1 == 12){//右進からの帰還フェーズ
             if(ActPos < 3000){
                 vel_stop();
+                LED = 1111;
                 t.reset();
                 t.start();
                 state_1 = 0;
@@ -283,18 +297,17 @@
                     vel_forward_con(RPM_CLEAN);//前進速度指令
                 }   
             }else if(state_2 == 1){
-                if((X_DIST_TMP - dist3) > CLEAN_OFFSET){
-                    state_2 == 2;
+                if(dist3 < WALL && dist3 >= WALL_MIN){
+                    state_2 = 4;
                 }else{
-                    if(dist3 < WALL && dist3 >= WALL_MIN){
-                        state_2 = 4;
-                    }else{
-                        set_ACC(ACC_CLEAN);//加速度設定
-                        set_DEC(DEC_CLEAN);//減速度設定
-                        set_MODE_V();//速度制御モード送信
-                        vel_right(RPM_CLEAN);//右進速度指令
-                    }
+                    set_ACC(ACC_CLEAN);//加速度設定
+                    set_DEC(DEC_CLEAN);//減速度設定
+                    set_MODE_V();//速度制御モード送信
+                    vel_right(RPM_CLEAN);//右進速度指令
+                    wait(CLEAN_OFFSET);
+                    state_2 = 2;
                 }
+                
             }else if(state_2 == 2){
                 if(ActPos > -3000){
                     state_2 = 3;
@@ -305,18 +318,16 @@
                     vel_backward_con(RPM_CLEAN);//後進速度指令
                 }
             }else if(state_2 == 3){
-                if((X_DIST_TMP - dist3) > CLEAN_OFFSET){
-                    state_2 == 0;
+                if(dist3 < WALL && dist3 >= WALL_MIN){
+                    state_2 = 4;
                 }else{
-                    if(dist3 < WALL && dist3 >= WALL_MIN){
-                        state_2 = 4;
-                    }else{
                     set_ACC(ACC_CLEAN);//加速度設定
                     set_DEC(DEC_CLEAN);//減速度設定
                     set_MODE_V();//速度制御モード送信
                     vel_right(RPM_CLEAN);//右進速度指令
-                    }
-                } 
+                    wait(CLEAN_OFFSET);
+                    state_2 = 0;
+                }
             }else if(state_2 == 4){
                 if(ActPos < 3000){
                     state_2 = 5;