Diff: main.cpp

Revision:

13:29e71a2fd11e

Parent:

12:9a5de6adae9c

Child:

14:1a3a673d85e3

--- a/main.cpp	Thu May 02 04:52:47 2019 +0000
+++ b/main.cpp	Thu May 02 07:20:17 2019 +0000
@@ -275,7 +275,7 @@
 void curveLeft();
 void turnRight();
 void curveRight();
-void turn(float turn_degree);//回転角, 収束許容誤差
+void turn(float target_degree);//回転角, 収束許容誤差
 void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
 
 void wait_gerege();
@@ -303,6 +303,7 @@
 int main()
 {
     setup();
+    
     pid_lo.setTolerance(10);
     pid_li.setTolerance(10);
     motor_lo.setEncoder(&ec_lo);
@@ -337,50 +338,80 @@
     //Sample
     //スタート直進
     printf("dist:%.3f\r\n", get_dist_forward());
-    while(get_dist_back() < 300)
-    {
-        straightAndInfinity(0, 5);
-        //wait(0.01);
-        printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+    /*
+    for(int i=0;i<3;++i){
+        while(get_dist_back() < 280)
+        {
+            straightAndInfinity(0, 5);
+            //wait(0.01);
+            printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+        }
     }
     //printf("back dist:%.3f\r\n", get_dist_back());
     //段差前旋回
     motor_lo.setDutyLimit(0.4);//0.5
     motor_li.setDutyLimit(0.4);
-    turn(45.0);
+    turn(40.0);
     //段差乗り越え
     for(int i= 0;i<5;++i) straight();
     while(get_dist_back() > 40) straight();
     //段差後旋回
     printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
-    turn(45.0);
+    turn(90.0);
     printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
     //前進
     motor_lo.setDutyLimit(0.5);//0.5
     motor_li.setDutyLimit(0.5);
-    while(get_dist_forward() > 60) 
+    for(int i=0;i<3;++i)
     {
-        straightAndInfinity(90.0, 5.0);
-        printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+        while(get_dist_forward() > 65) 
+        {
+            straightAndInfinity(90.0, 5.0);
+            printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+        }
     }
     //ロープ前旋回
-    turn(-90);
+    printf("before roop deg:%.3f\n\r",degree0);
+    turn(0);
     //ロープ
-    while(get_dist_forward() > 60)
+    while(mode4.read() == 0)
+    {
+        
+        straightAndInfinity(0, 5);
+    }
+    */
+    printf("go to uhai deg:%.3f\n\r",degree0);
+    for(int i=0;i<3;++i)
     {
-     straightAndInfinity(0, 5);
-     //printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+        while(get_dist_forward() > 65)
+        {
+            straightAndInfinity(0, 5);
+        //printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+        }
+    }
+    turn(-90);
+    
+    
+    motor_lo.setDutyLimit(0.3);//0.5
+    motor_li.setDutyLimit(0.3);
+    
+    for(int i=0;i<15;++i)straightAndInfinity(-90, 5);
+    printf("wall standby");
+    for(int i=0;i<3;++i)
+    {
+        while(get_dist_forward() > 60)
+        {
+            straightAndInfinity(-90, 5);
+            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+        }
     }
     
-    turn(-90);
-    
     hand_mode = GOAL;
-    
     straight();
+    printf("uhai!!!!!!!!!!!!!!!");
 }
-void turn(float turn_degree)//turn_degreeを超えるまで旋回し続ける関数
+void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
 {
-    float target_degree = degree0 + turn_degree;
     if(target_degree - degree0 > 0)
     {
         while(target_degree - degree0 > 0)
@@ -391,6 +422,7 @@
         while(target_degree - degree0 < 0)
         turnRight();
     }
+    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
 }
 void straightAndInfinity(float target_degree, float tolerance_degree)
 {
@@ -489,7 +521,7 @@
     hand.mode(PullUp);
     switch_lo.mode(PullUp);
     switch_li.mode(PullUp);
-    switch4.mode(PullUp);
+    mode4.mode(PullUp);
 
 }