Diff: main.cpp

Revision:

26:5fb1aa9cb7f0

Parent:

25:c740e6fd5dab

Child:

27:d392a95f4799

--- a/main.cpp	Mon May 06 04:03:00 2019 +0000
+++ b/main.cpp	Mon May 06 06:28:41 2019 +0000
@@ -5,86 +5,52 @@
 #include "moves.h"
 
 void StateFlow(int i);
+
 int main()
 {
+    //setup関連
     can1.frequency(1000000);
+    printf("file open\r\n");
     SetupMoves();
     set_gyro();
     int start_state=0;
-
     printf("reset standby\r\n");
-
-    /*while(1) {
-        printf("forward:%.3f back:%.3f\r\n", get_dist_forward(), get_dist_back());
-        wait(0.01);
-    }*/
-
-    reset();
-    
-    /*
-    
-    while(1)
-    {
-        motor_lo.setDutyLimit(0.3);
-        motor_li.setDutyLimit(0.3);
-        onestraight();
-    }
-    */
-    
+    reset();//足をリセット位置に移動
     printf("bus standby\r\n");
     while(1) {
         if(bus_in.read() == 1) break;
     }
     printf("bus is %d\r\n", bus_in.read());
     wait(0.5);
+    //一歩動かして初期位置へ
     motor_lo.setDutyLimit(0.1);
     motor_li.setDutyLimit(0.1);
     straight();
-    
+//    mode選択
     while(mode4.read() == 1) {
         start_state = (get_dist_back()-10)/10;
         int tmp = start_state;
         if(start_state>6 || start_state<0) start_state = 0;
-        //printf("%d\r\n",start_state);
         led2 = start_state/4;
         start_state = start_state%4;
         led3 = start_state/2;
         start_state = start_state%2;
         led4 = start_state;
         start_state = tmp;
-        
     }
-    
+    start_state = 0;//debug用
+    printf("mode is %d\r\n", start_state);
     wait_gerege();
     hand_mode = GEREGE;
     wait(1.0);
-    
-
-    //直進スタート
-    /*while(1)
-    {
-        printf("angle:%.3f\r\n", degree0);
-        wait(0.1);
-    }*/
-
-    /*
-    motor_lo.setDutyLimit(0.2);
-    motor_li.setDutyLimit(0.2);
-    while(1)
-    {
-        backLeft();
-    }*/
-    
-
-    
     if(start_state == 0) theta0 = -degree/100.0;
     else if(start_state == 1) theta0 = -degree/100.0 - 45;
     else if(start_state == 2) theta0 = -degree/100.0 - 90;
     else if(start_state == 3) theta0 = -degree/100.0 + 90;
     else printf("state_error");
-
+    FileOpen();
     StateFlow(start_state);
-
+    FileClose();
 }
 
 void StateFlow(int i)
@@ -92,13 +58,10 @@
     switch(i) {
         case 0://最初の直線
             printf("go straight!!!!!!!!!!\r\n");
-            
-            motor_lo.setDutyLimit(0.6);
-            motor_li.setDutyLimit(0.6);
-            
+            motor_lo.setDutyLimit(0.3);
+            motor_li.setDutyLimit(0.3);
             for(int i = 0; i < 25; i++) {
                 straightAndInfinity(0, 3);
-                //printf("not dist mode angle:%.3f backdist:%.2f ec:%d\r\n", degree0, get_dist_back(), ec_lo.getCount());
             }
             printf("get distance mode");
             for(int i=0; i<1; ++i) {
@@ -107,14 +70,12 @@
                 }
             }
             //段差前旋回
-            
             motor_lo.setDutyLimit(0.5);//0.5
             motor_li.setDutyLimit(0.5);
             turn(35.0);
         case 1://段差前
             printf("climb!!!!!!!!!!\r\n");
             //段差乗り越え
-            
             motor_lo.setDutyLimit(0.3);//0.5
             motor_li.setDutyLimit(0.3);
             while(get_dist_forward() < 60) {
@@ -139,9 +100,7 @@
             //ロープ前旋回
             printf("before roop deg:%.3f\r\n",degree0);
             turn(0);
-
             //ロープ位置ジャストまで前進
-
             for(int i=0; i<3; ++i) {
                 straightAndInfinity(0.0, 5.0);
             }
@@ -150,9 +109,6 @@
                     straightAndInfinity(0.0, 5.0);
                 }
             }
-
-            //wait(10);
-
             //ロープ
             while(mode4.read() == 0) {
                 straightAndInfinity(0, 5);
@@ -166,24 +122,20 @@
             while(get_dist_back() < 40.0) {
                 wait(0.01);
             }
-
             printf("last spart!!!!!!!!");
             theta0 = -degree/100.0+90;
             motor_lo.setDutyLimit(0.3);//0.3
             motor_li.setDutyLimit(0.3);
-
             for(int i=0; i<15; ++i)straight();//climbAndInfinity(-90,5);
             printf("wall standby");
             while(get_dist_back() < 250) {
                 straight();
                 climbAndInfinity(-90,5);
-//        DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
             }
             for(int i=0; i<1; ++i) {
                 while(get_dist_forward() > 70) {
                     //straight();
                     climbAndInfinity(-90,5);
-//            DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
                 }
             }
             hand_mode = GOAL;