Diff: main.cpp

Revision:

53:4673b85e1d2a

Parent:

52:dc3d83eb31eb

Child:

55:1b1e5839dde1

--- a/main.cpp	Mon May 20 04:26:15 2019 +0000
+++ b/main.cpp	Mon May 20 10:27:02 2019 +0000
@@ -6,7 +6,7 @@
 #include <stdlib.h>
 
 
-//#define BADRULEMODE
+#define BADRULEMODE
 
 void StateFlow(int i);
 void SetMode();
@@ -26,6 +26,11 @@
     SetupMoves();
     set_gyro();
     printf("hand read:%d\r\n",hand.read());
+    /*while(1) 
+    {
+        printf("forward:%.2f back:%.2f\r\n",get_dist_forward(),get_dist_back());
+        wait(0.01);
+    }*/
     
     printf("reset standby\r\n");
     reset();//足をリセット位置に移動
@@ -57,15 +62,18 @@
     }
 #endif
 #ifdef BADRULEMODE
+    printf("BAD RULE!!!!!!!!!\r\n");
     if(RightOrLeft == 0) {
         if(start_state == 0) theta0 = -degree/100.0;
         else if(start_state == 1) {
             theta0 = -degree/100.0 - 90;
+            printf("degree is %.2f for 90\r\n", degree0);
             motor_lo.setDutyLimit(0.6);//0.5
             motor_li.setDutyLimit(0.6);
             turn(55.0);
         } else if(start_state == 2) {
             theta0 = -degree/100.0 - 135;
+            printf("degree is %.2f for 135\r\n", degree0);
             motor_lo.setDutyLimit(0.6);//0.5
             motor_li.setDutyLimit(0.6);
             turn(100.0);
@@ -75,12 +83,14 @@
         if(start_state == 0) theta0 = -degree/100.0;
         else if(start_state == 1) {
             theta0 = -degree/100.0 + 90;
+            printf("degree is %.2f for -90\r\n", degree0);
             //段差前旋回
             motor_lo.setDutyLimit(0.6);//0.5
             motor_li.setDutyLimit(0.6);
             turn(-55.0);
         } else if(start_state == 2) {
             theta0 = -degree/100.0 + 135;
+            printf("degree is %.2f for -135\r\n", degree0);
             motor_lo.setDutyLimit(0.6);//0.5
             motor_li.setDutyLimit(0.6);
             turn(-100.0);
@@ -125,11 +135,10 @@
             motor_lo.setDutyLimit(params[3].duty);
             motor_li.setDutyLimit(params[3].duty);
             //段差乗り越え
-            while(get_dist_forward() < params[3].condition) {
+            while(get_dist_forward() > params[3].condition) {
                 if(RightOrLeft == 0) straightAndInfinity(params[3].argument[0], params[3].argument[1]);
                 else straightAndInfinity(-params[3].argument[0], params[3].argument[1]);
             }
-            
             motor_lo.setDutyLimit(params[4].duty);
             motor_li.setDutyLimit(params[4].duty);
             if(RightOrLeft == 0) phasing(params[4].argument[0]);
@@ -142,7 +151,14 @@
             motor_li.setDutyLimit(params[6].duty);
             printf("start finding back block\r\n"); 
             while(get_dist_back() > params[6].condition) straight();
-            
+            straight();
+            int i=0;
+            while(1)
+            {
+                i++;
+                led1 = i%2;
+                wait(0.5);
+            }
             //段差後旋回
             motor_lo.setDutyLimit(params[7].duty);
             motor_li.setDutyLimit(params[7].duty);
@@ -194,8 +210,8 @@
             motor_lo.setDutyLimit(params[13].duty);
             motor_li.setDutyLimit(params[13].duty);
             while(switch_modes[2].read() == 0) {
-                //straightAndInfinity(params[13].argument[0], params[13].argument[1]);
-                straight();
+                lopeAndInfinity(params[13].argument[0], params[13].argument[1]);
+                //straight();
             }
             printf("uhai stand by ok!!!!!!!!!!\r\n");
             NoHandSignal();