Diff: main.cpp

Revision:

27:d392a95f4799

Parent:

26:5fb1aa9cb7f0

Child:

28:c92912036b03

--- a/main.cpp	Mon May 06 06:28:41 2019 +0000
+++ b/main.cpp	Wed May 08 02:55:40 2019 +0000
@@ -10,7 +10,6 @@
 {
     //setup関連
     can1.frequency(1000000);
-    printf("file open\r\n");
     SetupMoves();
     set_gyro();
     int start_state=0;
@@ -28,6 +27,7 @@
     straight();
 //    mode選択
     while(mode4.read() == 1) {
+        int
         start_state = (get_dist_back()-10)/10;
         int tmp = start_state;
         if(start_state>6 || start_state<0) start_state = 0;
@@ -38,11 +38,11 @@
         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);
+    HandMove();
+    wait(0.5);
     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;
@@ -55,17 +55,22 @@
 
 void StateFlow(int i)
 {
+    motor_lo.setDutyLimit(0.3);
+    motor_li.setDutyLimit(0.3);
+    while(1)
+        
+//        straight();
     switch(i) {
         case 0://最初の直線
             printf("go straight!!!!!!!!!!\r\n");
-            motor_lo.setDutyLimit(0.3);
-            motor_li.setDutyLimit(0.3);
+            motor_lo.setDutyLimit(0.5);
+            motor_li.setDutyLimit(0.5);
             for(int i = 0; i < 25; i++) {
                 straightAndInfinity(0, 3);
             }
             printf("get distance mode");
             for(int i=0; i<1; ++i) {
-                while(get_dist_back() < 270) {//320
+                while(get_dist_back() < 290) {//320
                     straightAndInfinity(0, 3);
                 }
             }
@@ -76,8 +81,8 @@
         case 1://段差前
             printf("climb!!!!!!!!!!\r\n");
             //段差乗り越え
-            motor_lo.setDutyLimit(0.3);//0.5
-            motor_li.setDutyLimit(0.3);
+            motor_lo.setDutyLimit(0.2);//0.5
+            motor_li.setDutyLimit(0.2);
             while(get_dist_forward() < 60) {
                 straightAndInfinity(45, 5);
             }
@@ -85,13 +90,15 @@
             while(get_dist_back() > 80) straight();
             //段差後旋回
             printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+            motor_lo.setDutyLimit(0.4);//0.5
+            motor_li.setDutyLimit(0.4);
             turn(90.0);
         case 2://段差直後
             printf("go lope!!!!!!!!!!\r\n");
             printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back());
             //前進
-            motor_lo.setDutyLimit(0.6);
-            motor_li.setDutyLimit(0.6);
+            motor_lo.setDutyLimit(0.5);
+            motor_li.setDutyLimit(0.5);
             for(int i=0; i<3; ++i) {
                 while(get_dist_forward() > 60) {
                     straightAndInfinity(90.0, 5.0);
@@ -126,10 +133,10 @@
             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);
+            for(int i=0; i<15; ++i)straightAndInfinity(-90, 5);//straight();//climbAndInfinity(-90,5);
             printf("wall standby");
             while(get_dist_back() < 250) {
-                straight();
+//                straight();
                 climbAndInfinity(-90,5);
             }
             for(int i=0; i<1; ++i) {