Diff: main.cpp

Revision:

25:c740e6fd5dab

Parent:

24:9ee1440c6703

Child:

26:5fb1aa9cb7f0

--- a/main.cpp	Fri May 03 10:00:20 2019 +0000
+++ b/main.cpp	Mon May 06 04:03:00 2019 +0000
@@ -4,112 +4,190 @@
 #include "sensors.h"
 #include "moves.h"
 
-
+void StateFlow(int i);
 int main()
 {
     can1.frequency(1000000);
     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) {
-        get_dist_forward();
-        get_dist_back();
-        wait(0.1);
+    
+    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.3);
-    motor_li.setDutyLimit(0.3);
+    motor_lo.setDutyLimit(0.1);
+    motor_li.setDutyLimit(0.1);
     straight();
+    
+    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;
+        
+    }
+    
     wait_gerege();
     hand_mode = GEREGE;
-    set_gyro();
+    wait(1.0);
+    
+
     //直進スタート
-    motor_lo.setDutyLimit(0.6);
-    motor_li.setDutyLimit(0.6);
-    for(int i = 0; i < 25; i++)
-        straightAndInfinity(0, 5);
-        
-    for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
-        lowpassfilter[i].SetOldWeight(0);
-    for(int i=0; i<2; ++i) {
-        while(get_dist_back() < 320) {
-            straightAndInfinity(0, 5);
-        }
-    }
-    //段差前旋回
-    motor_lo.setDutyLimit(0.3);//0.5
-    motor_li.setDutyLimit(0.3);
-    turn(35.0);
-    //段差乗り越え
-    motor_lo.setDutyLimit(0.3);//0.5
-    motor_li.setDutyLimit(0.3);
-    while(get_dist_back() < 80) {
-        straightAndInfinity(0, 5);
+    /*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");
+
+    StateFlow(start_state);
+
+}
+
+void StateFlow(int i)
+{
+    switch(i) {
+        case 0://最初の直線
+            printf("go straight!!!!!!!!!!\r\n");
+            
+            motor_lo.setDutyLimit(0.6);
+            motor_li.setDutyLimit(0.6);
+            
+            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) {
+                while(get_dist_back() < 270) {//320
+                    straightAndInfinity(0, 3);
+                }
+            }
+            //段差前旋回
+            
+            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) {
+                straightAndInfinity(45, 5);
+            }
+            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());
+            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);
+            for(int i=0; i<3; ++i) {
+                while(get_dist_forward() > 60) {
+                    straightAndInfinity(90.0, 5.0);
+                }
+            }
+            //ロープ前旋回
+            printf("before roop deg:%.3f\r\n",degree0);
+            turn(0);
+
+            //ロープ位置ジャストまで前進
+
+            for(int i=0; i<3; ++i) {
+                straightAndInfinity(0.0, 5.0);
+            }
+            for(int i=0; i<2; ++i) {
+                while(get_dist_back() < 100) {
+                    straightAndInfinity(0.0, 5.0);
+                }
+            }
+
+            //wait(10);
+
+            //ロープ
+            while(mode4.read() == 0) {
+                straightAndInfinity(0, 5);
+                //straight();
+            }
+        case 3://坂
+            printf("uhai stand by ok!!!!!!!!!!\r\n");
+            while(get_dist_back() > 40.0) {
+                wait(0.01);
+            }
+            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;
+            straight();
+            printf("uhai!!!!!!!!!!!!!!!");
     }
-    for(int i= 0; i<10; ++i) straight();
-    //filter切る
-    for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
-        lowpassfilter[i].SetOldWeight(0);
-    while(get_dist_back() > 80) straight();
-    //filter軽く
-    for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
-        lowpassfilter[i].SetOldWeight(kOldWeightLight);
-    //段差後旋回
-    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
-    turn(90.0);
-    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
-    //前進
-    motor_lo.setDutyLimit(0.5);
-    motor_li.setDutyLimit(0.5);
-    for(int i=0; i<3; ++i) {
-        while(get_dist_forward() > 65) {
-            straightAndInfinity(90.0, 5.0);
-        }
-    }
-    //ロープ前旋回
-    printf("before roop deg:%.3f\r\n",degree0);
-    turn(0);
-
-    //ロープ
-    while(mode4.read() == 0) {
-        straightAndInfinity(0, 5);
-    }
-    printf("go to uhai deg:%.3f forward dist:%.3f \r\n",degree0,get_dist_forward());
-    for(int i=0; i<3; ++i) {
-        while(get_dist_forward() > 65) { //65
-            straightAndInfinity(0, 5);
-        }
-    }
-    printf("turn");
-    turn(-90);
-
-    while(get_dist_back() > 10.0) {}
-    while(get_dist_back() < 30.0) {}
-
-    printf("last spart!!!!!!!!");
-    motor_lo.setDutyLimit(0.2);//0.5
-    motor_li.setDutyLimit(0.2);
-
-    for(int i=0; i<15; ++i)straightAndInfinity(-90, 5);
-    printf("wall standby");
-    while(get_dist_back() < 250) {
-        straightAndInfinity(-90, 5);
-//        DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
-    }
-    for(int i=0; i<2; ++i) {
-        while(get_dist_forward() > 65) {
-            straightAndInfinity(-90, 5);
-//            DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
-        }
-    }
-    hand_mode = GOAL;
-    straight();
-    printf("uhai!!!!!!!!!!!!!!!");
 }
\ No newline at end of file