Diff: main.cpp

Revision:

32:aee87dcaf7ca

Parent:

31:ed4249a288d7

Child:

33:2dbbe198adaf

diff -r ed4249a288d7 -r aee87dcaf7ca main.cpp
--- a/main.cpp	Wed May 08 11:00:09 2019 +0000
+++ b/main.cpp	Mon May 13 03:44:50 2019 +0000
@@ -5,6 +5,11 @@
 #include "moves.h"
 
 void StateFlow(int i);
+void SetMode();
+void Start();
+void NoHandSignal();
+int RightOrLeft = 0;
+int start_state=0;
 
 int main()
 {
@@ -12,7 +17,14 @@
     can1.frequency(1000000);
     SetupMoves();
     set_gyro();
-    int start_state=0;
+    
+    /*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);    
+    }*/
+    printf("hand read:%d\r\n",hand.read());
+    
     printf("reset standby\r\n");
     reset();//足をリセット位置に移動
     printf("bus standby\r\n");
@@ -25,31 +37,32 @@
     motor_lo.setDutyLimit(0.1);
     motor_li.setDutyLimit(0.1);
     straight();
+    
+    
 //    mode選択
-    while(mode4.read() == 1) {
-        start_state = (get_dist_back()-10)/10;
-        if(start_state>6 || start_state<0) start_state = 0;
-        int tmp = 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();
+    //SetMode();
+
+    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");
     }
-    printf("mode is %d\r\n", start_state);
-    wait_gerege();
-    hand_mode = GEREGE;
-    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;
-    else if(start_state == 3) theta0 = -degree/100.0 + 90;
-    else printf("state_error");
-    FileOpen();
+    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;
+        else if(start_state == 3) theta0 = -degree/100.0 - 90;
+        else printf("state_error");
+    }
+    //FileOpen();
+    printf("LR:%d StateFlow:%d\r\n", RightOrLeft,start_state);
     StateFlow(start_state);
-    FileClose();
+    //FileClose();
 }
 
 void StateFlow(int i)
@@ -57,46 +70,58 @@
     switch(i) {
         case 0://最初の直線
             printf("go straight!!!!!!!!!!\r\n");
-            motor_lo.setDutyLimit(0.5);
-            motor_li.setDutyLimit(0.5);
-            for(int i = 0; i < 25; i++) {
-                straightAndInfinity(0, 3);
+            motor_lo.setDutyLimit(0.7);
+            motor_li.setDutyLimit(0.7);
+            for(int i = 0; i < 30; i++) {
+                straightAndInfinity(0, 7);
             }
             printf("get distance mode");
             for(int i=0; i<1; ++i) {
-                while(get_dist_back() < 290) {//320
-                    straightAndInfinity(0, 3);
+                while(get_dist_back() < 320) {//300
+                    straightAndInfinity(0, 7);
                 }
             }
             //段差前旋回
             motor_lo.setDutyLimit(0.5);//0.5
             motor_li.setDutyLimit(0.5);
-            turn(35.0);
+            
+            if(RightOrLeft == 0) turn(35.0);
+            else turn(-35.0);
         case 1://段差前
             printf("climb!!!!!!!!!!\r\n");
             //段差乗り越え
-            motor_lo.setDutyLimit(0.2);//0.5
-            motor_li.setDutyLimit(0.2);
+            motor_lo.setDutyLimit(0.3);//0.5
+            motor_li.setDutyLimit(0.3);
+        
             while(get_dist_forward() < 60) {
-                straightAndInfinity(45, 5);
+                if(RightOrLeft == 0) straightAndInfinity(45, 5);
+                else straightAndInfinity(-45, 5);
             }
-            phasing(45.0);
+            if(RightOrLeft == 0) phasing(45.0);
+            else phasing(-45.0);
+            
+            
+            
             for(int i= 0; i<14; ++i) straight();
             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);
+            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");
             printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back());
             //前進
-            motor_lo.setDutyLimit(0.5);
-            motor_li.setDutyLimit(0.5);
+            motor_lo.setDutyLimit(0.7);
+            motor_li.setDutyLimit(0.7);
             for(int i=0; i<3; ++i) {
                 while(get_dist_forward() > 60) {
-                    straightAndInfinity(90.0, 5.0);
+                    if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
+                    else straightAndInfinity(-90.0, 7.0);
                 }
             }
             //ロープ前旋回
@@ -104,45 +129,98 @@
             turn(0);
             //ロープ位置ジャストまで前進
             for(int i=0; i<3; ++i) {
-                straightAndInfinity(0.0, 5.0);
+                straightAndInfinity(0.0, 7.0);
             }
             for(int i=0; i<2; ++i) {
                 while(get_dist_back() < 100) {
-                    straightAndInfinity(0.0, 5.0);
+                    straightAndInfinity(0.0, 7.0);
                 }
             }
             phasing(0.0);
             //ロープ
-            while(mode4.read() == 0) {
-                straightAndInfinity(0, 5);
+            while(switch_modes[2].read() == 0) {
+                straightAndInfinity(0, 7);
                 //straight();
             }
+            printf("uhai stand by ok!!!!!!!!!!\r\n");
+            NoHandSignal();
         case 3://坂
-            printf("uhai stand by ok!!!!!!!!!!\r\n");
-            while(get_dist_back() > 40.0) {
-                wait(0.01);
+            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)
+            {
+                if(RightOrLeft == 0) straightAndInfinity(-90, 15);//straight();//climbAndInfinity(-90,5);
+                else straightAndInfinity(90, 15);
             }
-            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<20; ++i)straightAndInfinity(-90, 5);//straight();//climbAndInfinity(-90,5);
             printf("wall standby");
             while(get_dist_back() < 250) {
 //                straight();
-                climbAndInfinity(-90,5);
+                if(RightOrLeft == 0) climbAndInfinity(-90,15);
+                else climbAndInfinity(90,15);
             }
             for(int i=0; i<1; ++i) {
                 while(get_dist_forward() > 60) {
                     //straight();
-                    climbAndInfinity(-90,5);
+                    if(RightOrLeft == 0) climbAndInfinity(-90,15);
+                    else climbAndInfinity(90,15);
                 }
             }
             hand_mode = GOAL;
             stop();
-            printf("uhai!!!!!!!!!!!!!!!");
+            printf("uhai!!!!!!!!!!!!!!!/r/n");
+    }
+}
+void Start()
+{
+    if(hand.read()==0)//開始時すでにスイッチが押されていた場合
+    {
+        
+        SetMode();
+        NoHandSignal();
+    }
+    else
+    {
+        SetMode();
+        wait_gerege();
+        hand_mode = GEREGE;
+        HandMove();
+        wait(0.5);
+    }   
+}
+void SetMode()
+{
+    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(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) 
+    {
+        wait(0.01);
+    }
+    while(get_dist_back() < 40.0) 
+    {
+        wait(0.01);
     }
 }
\ No newline at end of file