Diff: main.cpp.orig

Revision:

28:c92912036b03

diff -r d392a95f4799 -r c92912036b03 main.cpp.orig
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Wed May 08 07:48:01 2019 +0000
@@ -0,0 +1,193 @@
+#include "mbed.h"
+#include "debug.h"
+#include "microinfinity.h"
+#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)
+    {
+        motor_lo.setDutyLimit(0.3);
+        motor_li.setDutyLimit(0.3);
+        onestraight();
+    }
+    */
+    
+    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();
+    
+    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;
+    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");
+
+    StateFlow(start_state);
+
+}
+
+void StateFlow(int i)
+{
+    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);
+                //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() < 290) {//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.5);
+            motor_li.setDutyLimit(0.5);
+            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!!!!!!!!!!!!!!!");
+    }
+}
\ No newline at end of file