Diff: main.cpp

Revision:

4:3f59e4f5ca30

Parent:

3:df0c9883e403

Child:

5:1fa5aa097af5

--- a/main.cpp	Sat Mar 30 01:47:42 2019 +0000
+++ b/main.cpp	Mon Apr 01 02:20:07 2019 +0000
@@ -53,6 +53,7 @@
 double posi_r=0,posi_l=0;
 double pre_out_r=0,pre_out_l=0;
 bool turn_mode=0;
+double turn_target=0;
 
 
 
@@ -316,7 +317,6 @@
         Ki_r=Ki_r_n;
         Kd_r=Kd_r_n;
     }
-    move_r=1;
     double deviation = target - posi_r;
     //printf("%f,%f\r\n",posi_r,deviation);
     pile_r += deviation;
@@ -327,6 +327,12 @@
         pile_r=0;
         move_r=0;
         reach_target_r=1;
+    }else if(turn_mode==1 && fabs(turn_target-degree0)<5){
+        out_r(0);
+        pile_r=0;
+        move_r=0;
+        reach_target_r=1;
+        turn_mode=0;
     }
 }
 
@@ -342,7 +348,6 @@
         Ki_l=Ki_l_n;
         Kd_l=Kd_l_n;
     }
-    move_l=1;
     double deviation = target - posi_l;
     pile_l += deviation;
     out_l(deviation * Kp_l - (posi_l - distance_l_old) / pid_time * Kd_l + pile_l *  Ki_l * pid_time);
@@ -352,6 +357,12 @@
         pile_l=0;
         move_l=0;
         reach_target_l=1;
+    }else if(turn_mode==1 && fabs(turn_target-degree0)<5){
+        out_l(0);
+        pile_l=0;
+        move_l=0;
+        reach_target_l=1;
+        turn_mode=0;
     }
 }
 
@@ -515,40 +526,34 @@
 double a=140,b=70;
 void turn_2(double target){
     turn_mode=1;
+    turn_target=target;
     if(target-degree0>0){
         move(a,a);
-        while(1){
+        while(turn_mode){
             move(a+b,-(a+b));
-            if(fabs(target-degree0)<5)break;
             move(a,-a);
-            if(fabs(target-degree0)<5)break;
-            //printf("%f\r\n",degree0);
         }
     }else{    
         move(a,a);
-        while(1){
+        while(turn_mode){
             move(-(a+b),a+b);
-            if(fabs(target-degree0)<5)break;
             move(-a,a);
-            if(fabs(target-degree0)<5)break;
-            //printf("%f\r\n",degree0);
         }
     }
-    turn_mode=0;
 }
 
 void sanddune()
 {
     move(160,160);
-    down(long_r);
-    down(long_l);
+    down(long_o);
+    down(long_i);
     wait(1);
 
-    up(long_l);
+    up(long_i);
     wait(leg_wait_time_l);
     target_r_out=50;
-    //target_l_out=50;
-    target_l_out=270;
+    target_l_out=50;
+    //target_l_out=270;
     reach_target_r=0;
     reach_target_l=0;
     move_r=1;
@@ -556,18 +561,18 @@
     while(reach_target_r==0 || reach_target_l==0){
         wait(0.01);
     }
-    down(long_l);
+    down(long_i);
     //down(l_o_s);
     //down(r_i_s);
     wait(leg_wait_time_l);
     reach_target_r=0;
     reach_target_l=0;
 
-    up(long_r);
+    up(long_o);
     wait(leg_wait_time_l);
     target_r_out=250;
-    //target_l_out=250;
-    target_l_out=70;
+    target_l_out=250;
+    //target_l_out=70;
     reach_target_r=0;
     reach_target_l=0;
     move_r=1;
@@ -575,12 +580,12 @@
     while(reach_target_r==0 || reach_target_l==0){
         wait(0.01);
     }
-    down(long_r);
+    down(long_o);
     wait(leg_wait_time_l);
     reach_target_r=0;
     reach_target_l=0;
 
-    up(long_l);
+    up(long_i);
     //up(l_o_s);
     //up(r_i_s);
     wait(leg_wait_time_l);
@@ -593,16 +598,16 @@
     while(reach_target_r==0 || reach_target_l==0){
         wait(0.01);
     }
-    down(long_l);
+    down(long_i);
     wait(leg_wait_time_l);
     reach_target_r=0;
     reach_target_l=0;
 
-    up(long_r);
+    up(long_o);
     wait(leg_wait_time_l);
     target_r_out=220;
-    //target_l_out=220;
-    target_l_out=100;
+    target_l_out=220;
+    //target_l_out=100;
     reach_target_r=0;
     reach_target_l=0;
     move_r=1;
@@ -610,18 +615,18 @@
     while(reach_target_r==0 || reach_target_l==0){
         wait(0.01);
     }
-    down(long_r);
+    down(long_o);
     //down(r_o_s);
     //down(l_i_s);
     wait(leg_wait_time_l);
     reach_target_r=0;
     reach_target_l=0;
     
-    up(long_l);
+    up(long_i);
     wait(leg_wait_time_l);
     target_r_out=80;
-    //target_l_out=80;
-    target_l_out=240;
+    target_l_out=80;
+    //target_l_out=240;
     reach_target_r=0;
     reach_target_l=0;
     move_r=1;
@@ -629,14 +634,14 @@
     while(reach_target_r==0 || reach_target_l==0){
         wait(0.01);
     }
-    down(long_l);
+    down(long_i);
     wait(leg_wait_time_l);
     reach_target_r=0;
     reach_target_l=0;
     
     //up(r_o_s);
     //up(l_i_s);
-    up(long_r);
+    up(long_o);
     wait(leg_wait_time_l);
     target_r_out=0;
     target_l_out=0;
@@ -647,7 +652,7 @@
     while(reach_target_r==0 || reach_target_l==0){
         wait(0.01);
     }
-    down(long_r);
+    down(long_o);
     wait(leg_wait_time_l);
     reach_target_r=0;
     reach_target_l=0;
@@ -656,22 +661,22 @@
 void tussock()
 {
     for(int i=0; i<3; i++) {
-        up(long_l);
+        up(long_i);
         wait(leg_wait_time);
         target_r_out=360;
         target_l_out=360;
         while(reach_target_r==0 || reach_target_l==0);
-        down(long_l);
+        down(long_i);
         wait(leg_wait_time);
         reach_target_r=0;
         reach_target_l=0;
 
-        up(long_r);
+        up(long_o);
         wait(leg_wait_time);
         target_r_out=10;
         target_l_out=10;
         while(reach_target_r==0 || reach_target_l==0);
-        down(long_r);
+        down(long_o);
         wait(leg_wait_time);
         reach_target_r=0;
         reach_target_l=0;