Diff: main.cpp

Revision:

38:89d2a9e6c96f

Parent:

35:04699b49c463

Child:

39:7f3cc8f47710

Child:

44:39dfefb929eb

Child:

45:6df20a8f4b21

Child:

48:d9f3ce701aca

--- a/main.cpp	Wed May 15 09:08:31 2019 +0000
+++ b/main.cpp	Wed May 15 09:57:11 2019 +0000
@@ -17,14 +17,14 @@
     can1.frequency(1000000);
     SetupMoves();
     set_gyro();
-    
+
     /*while(1)
     {
         printf("switch 1:%d  2:%d  3:%d  LR:%d \r\n", switch_modes[0].read(),switch_modes[1].read(),switch_modes[2].read(),switch_LR.read());
-        wait(0.1);    
+        wait(0.1);
     }*/
     printf("hand read:%d\r\n",hand.read());
-    
+
     printf("reset standby\r\n");
     reset();//足をリセット位置に移動
     printf("bus standby\r\n");
@@ -37,22 +37,19 @@
     motor_lo.setDutyLimit(0.1);
     motor_li.setDutyLimit(0.1);
     straight();
-    
-    
+
+
 //    mode選択
     Start();
     //SetMode();
 
-    if(RightOrLeft == 0)
-    {
+    if(RightOrLeft == 0) {
         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");
-    }
-    else
-    {
+    } else {
         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;
@@ -70,8 +67,8 @@
     switch(i) {
         case 0://最初の直線
             printf("go straight!!!!!!!!!!\r\n");
-            motor_lo.setDutyLimit(0.7);
-            motor_li.setDutyLimit(0.7);
+            motor_lo.setDutyLimit(0.6);
+            motor_li.setDutyLimit(0.6);
             for(int i = 0; i < 30; i++) {
                 straightAndInfinity(0, 7);
             }
@@ -84,36 +81,40 @@
             //段差前旋回
             motor_lo.setDutyLimit(0.5);//0.5
             motor_li.setDutyLimit(0.5);
-            
+
             if(RightOrLeft == 0) turn(35.0);
             else turn(-35.0);
         case 1://段差前
             printf("climb!!!!!!!!!!\r\n");
-            //段差乗り越え        
+            //段差乗り越え
             while(get_dist_forward() < 60) {
                 if(RightOrLeft == 0) straightAndInfinity(45, 5);
                 else straightAndInfinity(-45, 5);
             }
             if(RightOrLeft == 0) phasing(45.0);
             else phasing(-45.0);
-            
-            motor_lo.setDutyLimit(0.3);//0.5
-            motor_li.setDutyLimit(0.3);
-            
+
+            motor_lo.setDutyLimit(0.2);//0.5
+            motor_li.setDutyLimit(0.2);
+
             for(int i= 0; i<14; ++i) straight();
             while(get_dist_back() > 80) straight();
             //段差後旋回
             motor_lo.setDutyLimit(0.5);//0.5
             motor_li.setDutyLimit(0.5);
-            
+
             if(RightOrLeft == 0) turn(85.0);
             else turn(-85.0);
-            
+
         case 2://段差直後
             printf("go lope!!!!!!!!!!\r\n");
             //前進
-            motor_lo.setDutyLimit(0.7);
-            motor_li.setDutyLimit(0.7);
+            motor_lo.setDutyLimit(0.6);
+            motor_li.setDutyLimit(0.6);
+            for(int i=0; i<13; ++i) {
+                if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
+                else straightAndInfinity(-90.0, 7.0);
+            }
             for(int i=0; i<3; ++i) {
                 while(get_dist_forward() > 60) {
                     if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
@@ -142,13 +143,12 @@
             NoHandSignal();
         case 3://坂
             printf("last spart!!!!!!!!");
-            
+
             if(RightOrLeft == 0) theta0 = -degree/100.0+90;
             else theta0 = -degree/100.0-90;
             motor_lo.setDutyLimit(0.2);//0.3
             motor_li.setDutyLimit(0.2);
-            for(int i=0; i<20; ++i)
-            {
+            for(int i=0; i<20; ++i) {
                 if(RightOrLeft == 0) straightAndInfinity(-90, 15);//straight();//climbAndInfinity(-90,5);
                 else straightAndInfinity(90, 15);
             }
@@ -174,15 +174,12 @@
 }
 void Start()
 {
-    if(hand.read()==0)//開始時すでにスイッチが押されていた場合
-    {
-        
+    if(hand.read()==0) { //開始時すでにスイッチが押されていた場合
+
         SetMode();
         hand_mode = G_CLOSE;
         NoHandSignal();
-    }
-    else
-    {
+    } else {
         SetMode();
         hand_mode = G_OPEN;
         stop();//ここで開くことをcanでslaveに送る
@@ -190,38 +187,35 @@
         hand_mode = G_CLOSE;
         HandMove();
         wait(0.5);
-    }   
+    }
 }
 void SetMode()
 {
-    while(get_dist_back() > 50.0) 
-    {
+    while(get_dist_back() > 50.0) {
         led1 = led2 = led3 = led4 = 0;
         if(switch_modes[2].read()) start_state=3;
         else if(switch_modes[1].read()) start_state=2;
         else if(switch_modes[0].read()) start_state=1;
         else start_state=0;
-        
-        if(switch_LR.read()) RightOrLeft = 1;    
-        else RightOrLeft = 0; 
-        
+
+        if(switch_LR.read()) RightOrLeft = 1;
+        else RightOrLeft = 0;
+
         if(start_state==0)led4 = 1;
         else if(start_state==1)led3 = 1;
         else if(start_state==2)led2 = 1;
         else if(start_state==3)led1 = 1;
-        
+
     }
     printf("mode is %d  RightOrLeft is %d \r\n", start_state, RightOrLeft);
     led1 = led2 = led3 = led4 = 1;
 }
 void NoHandSignal()
 {
-    while(get_dist_back() > 40.0) 
-    {
+    while(get_dist_back() > 40.0) {
         wait(0.01);
     }
-    while(get_dist_back() < 40.0) 
-    {
+    while(get_dist_back() < 40.0) {
         wait(0.01);
     }
 }
\ No newline at end of file