Diff: main.cpp

Revision:

8:dd676df43fe7

Parent:

7:459f81b0b6c6

Child:

9:71b780f8d345

--- a/main.cpp	Mon Dec 20 23:43:11 2021 +0000
+++ b/main.cpp	Mon Dec 20 23:49:12 2021 +0000
@@ -19,7 +19,7 @@
 #define DEC_CLEAN 1000
 
 #define KICK 2000
-#define CLEAN_OFFSET 2000
+#define CLEAN_OFFSET 30
 
 #define WALL 45
 #define WALL_MIN 25
@@ -178,7 +178,7 @@
     int state_2 = 0;    
     int ride_count = 0;
 
-    int X_POS_TMP = 0;
+    int X_DIST_TMP = 0;
     int dist1_ori = 0;
     int dist2_ori = 0;
 
@@ -200,7 +200,7 @@
                 
         Time = t.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_POS_TMP);
+        pc.printf("state_1:%d state_2:%d ACT:%d TMP:%d \r\n",state_1,state_2,ActPos,X_DIST_TMP);
         readActPos(1);
         ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
         if(ActPos > 8388608){
@@ -274,7 +274,7 @@
         }else if(state_1 == 20){//消毒モード
             if(state_2 == 0){
                 if(dist1 < WALL && dist1 >= WALL_MIN){
-                    X_POS_TMP = ActPos;
+                    X_DIST_TMP = dist3;
                     state_2 = 1;
                 }else{
                     set_ACC(ACC_CLEAN);//加速度設定
@@ -283,7 +283,7 @@
                     vel_forward_con(RPM_CLEAN);//前進速度指令
                 }   
             }else if(state_2 == 1){
-                if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET){
+                if((X_DIST_TMP - dist3) > CLEAN_OFFSET){
                     state_2 == 2;
                 }else{
                     if(dist3 < WALL && dist3 >= WALL_MIN){
@@ -305,7 +305,7 @@
                     vel_backward_con(RPM_CLEAN);//後進速度指令
                 }
             }else if(state_2 == 3){
-                if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET){
+                if((X_DIST_TMP - dist3) > CLEAN_OFFSET){
                     state_2 == 0;
                 }else{
                     if(dist3 < WALL && dist3 >= WALL_MIN){