Revision 4:d00236029c9d, committed 2018-04-02

Comitter:

Khanchana

Date:

Mon Apr 02 16:41:17 2018 +0000

Parent:

3:64bfd330beb4

Commit message:

SERVOOOO

Changed in this revision

diff -r 64bfd330beb4 -r d00236029c9d main.cpp
--- a/main.cpp	Mon Apr 02 11:37:43 2018 +0000
+++ b/main.cpp	Mon Apr 02 16:41:17 2018 +0000
@@ -5,7 +5,7 @@
 Serial pc(USBTX, USBRX);
 
 Thread thread;
- 
+
 Servo Servo1(D6);
 Servo Servo2(D8);
 Servo Servo3(D9);
@@ -13,151 +13,197 @@
 
 void servo_Right();
 void move();
+void cal_step_down();
+void cal_step_up();
 
-float pos_down = 1400;
-float pos_up = 1000;
 float pos_down_start = 1400;
 float pos_up_start = 1000;
-float pos_down_end = 1544.44;
-float pos_up_end = 1400;
-float state_count = 1;
-float round_count = 1;
-float step = 5;
+float stepmin = 5;
+float round = 5;
+
+float pos_down_left = 1400;
+float pos_up_left = 1000;
+float pos_down_end_left = 1700; //left down
+float pos_up_end_left = 1350; //left up
+float state_count_left = 1;
+float round_count_left = 1;
+float step_down_left;
+float step_up_left;
+
 float pos_down_right = 1400;
 float pos_up_right = 1000;
-float pos_down_start_right = 1400;
-float pos_up_start_right = 1000;
-float pos_down_end_right = 1544.44;
-float pos_up_end_right = 1500;
+float pos_down_end_right = 1650; //right down
+float pos_up_end_right = 1700; //right up
 float state_count_right = 1;
 float round_count_right = 1;
-float step_right = 6.25;
+float step_up_right;
+float step_down_right;
 
-int main(){
-    while(1){
-        wait(5);
-        move();
-    }
+int main()
+{
+    wait(5);
+    cal_step_down();
+    cal_step_up();
+    move();
 }
 
-void move() {
+void cal_step_down(){
+    if (pos_down_end_right > pos_down_end_left){
+        step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
+        step_down_left = stepmin;
+    } else if (pos_down_end_right < pos_down_end_left){
+        step_down_right = stepmin;
+        step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
+    } else{
+        step_down_right = stepmin;
+        step_down_left = stepmin;
+    }
+    /*pc.printf("pos_down_right");
+    pc.printf("%f\n",pos_down_end_right);
+    pc.printf("pos_down_left");
+    pc.printf("%f\n",pos_down_end_left);
+    pc.printf("step_down_right");
+    pc.printf("%f\n",step_down_right);
+    pc.printf("step_down_left");
+    pc.printf("%f\n",step_down_left);*/
+}
+
+void cal_step_up(){    
+    if (pos_up_end_right > pos_up_end_left){
+        step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
+        step_up_left = stepmin;
+    } else if (pos_up_end_right < pos_up_end_left){
+        step_up_right = stepmin;
+        step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
+    } else{
+        step_up_right = stepmin;
+        step_up_left = stepmin;
+    }
+    /*pc.printf("pos_up_right");
+    pc.printf("%f\n",pos_up_end_right);
+    pc.printf("pos_up_left");
+    pc.printf("%f\n",pos_up_end_left);
+    pc.printf("step_up_right");
+    pc.printf("%f\n",step_up_right);;
+    pc.printf("step_up_left");
+    pc.printf("%f\n",step_up_left);*/
+}
+
+void move(){
     pc.baud(9600);
     Servo1.Enable(1000,2000);
     Servo2.Enable(1000,2000);
     Servo3.Enable(1000,2000);
     Servo4.Enable(1000,2000);
-    while(1){
+    pc.printf("Start\n");
+    while(1) {
         servo_Right();
-        if(state_count == 1){
-            Servo1.SetPosition(pos_down);
-            //wait(0.001);
-            pos_down = pos_down + step;
-            if(pos_down >= pos_down_end + step and pos_up == pos_up_start){
-                state_count = 2;
+        if(state_count_left == 1) {
+            Servo1.SetPosition(pos_down_left);
+            wait(0.005);
+            pos_down_left = pos_down_left + step_down_left;
+            if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
+                state_count_left = 2;
             }
-            pc.printf("LAD");
-            pc.printf("%f\n",pos_down);
+            /*pc.printf("LAD");
+            pc.printf("%f\n",pos_down_left);
             pc.printf("LAP");
-            pc.printf("%f\n",pos_up);
-        }
-        else if(state_count == 2){
-            Servo2.SetPosition(pos_up);
-            //wait(0.001);
-            pos_up = pos_up + step;
-            if(pos_down >= pos_down_end + step and pos_up >= pos_up_end + step){
-                state_count = 3;
+            pc.printf("%f\n",pos_up_left);*/
+        } else if(state_count_left == 2) {
+            Servo2.SetPosition(pos_up_left);
+            wait(0.005);
+            pos_up_left = pos_up_left + step_up_left;
+            if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
+                state_count_left = 3;
+            }
+            /*pc.printf("LBD");
+            pc.printf("%f\n",pos_down_left);
+            pc.printf("LBP");
+            pc.printf("%f\n",pos_up_left);*/
+        } else if(state_count_left == 3) {
+            Servo1.SetPosition(pos_down_left);
+            wait(0.005);
+            pos_down_left = pos_down_left - step_down_left;
+            if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
+                state_count_left = 4;
             }
-            pc.printf("LBD");
-            pc.printf("%f\n",pos_down);
-            pc.printf("LBP");
-            pc.printf("%f\n",pos_up);
+            /*pc.printf("LCD");
+            pc.printf("%f\n",pos_down_left);
+            pc.printf("LCP");
+            pc.printf("%f\n",pos_up_left);*/
+        } else if(state_count_left == 4) {
+            Servo2.SetPosition(pos_up_left);
+            wait(0.005);
+            pos_up_left = pos_up_left - step_up_left;
+            if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
+                state_count_left = 0;
+            }
+            /*pc.printf("LDD");
+            pc.printf("%f\n",pos_down_left);
+            pc.printf("LDP");
+            pc.printf("%f\n",pos_up_left);*/
+        } else if (state_count_left == 0 and round_count_left < round) {
+            round_count_left = round_count_left+1;
+            state_count_left = 1;
+            pos_down_left = pos_down_start;
+            pos_up_left = pos_up_start;
+        } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right < round){
+            pc.printf("Finish");
+            break;        
         }
-        else if(state_count == 3){
-            Servo1.SetPosition(pos_down);
-            //wait(0.001);
-            pos_down = pos_down - step;
-            if(pos_down <= pos_down_start - step and pos_up >= pos_up_end + step){
-                state_count = 4;
-            }
-            pc.printf("LCD");
-            pc.printf("%f\n",pos_down);
-            pc.printf("LCP");
-            pc.printf("%f\n",pos_up);
-        }
-        else if(state_count == 4){
-            Servo2.SetPosition(pos_up);
-            //wait(0.001);
-            pos_up = pos_up - step;
-            if(pos_down <= pos_down_start - step and pos_up <= pos_up_start - step){
-                state_count = 0;
-            }
-            pc.printf("LDD");
-            pc.printf("%f\n",pos_down);
-            pc.printf("LDP");
-            pc.printf("%f\n",pos_up);
-        }
-        else if (state_count == 0 and round_count < 1){
-            round_count = round_count+1;
-            state_count = 1;
-            pos_down = 1400;
-            pos_up = 1000;
-        }  
     }
 }
 
-void servo_Right(){
-    if(state_count_right == 1){
-            Servo3.SetPosition(pos_down_right);
-            //wait(0.001);
-            pos_down_right = pos_down_right + step;
-            if(pos_down_right >= pos_down_end_right + step and pos_up_right == pos_up_start){
-                state_count_right = 2;
-            }
-            pc.printf("RAD");
-            pc.printf("%f\n",pos_down_right);
-            pc.printf("RAP");
-            pc.printf("%f\n",pos_up_right);
+void servo_Right()
+{
+    if(state_count_right == 1) {
+        Servo3.SetPosition(pos_down_right);
+        wait(0.005);
+        pos_down_right = pos_down_right + step_down_right;
+        if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) {
+            state_count_right = 2;
         }
-        else if(state_count_right == 2){
-            Servo4.SetPosition(pos_up_right);
-            //wait(0.001);
-            pos_up_right = pos_up_right + step_right;
-            if(pos_down_right >= pos_down_end_right + step and pos_up_right >= pos_up_end_right + step_right){
-                state_count_right = 3;
-            }
-            pc.printf("RBD");
-            pc.printf("%f\n",pos_down_right);
-            pc.printf("RBP");
-            pc.printf("%f\n",pos_up_right);
+        /*pc.printf("RAD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RAP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if(state_count_right == 2) {
+        Servo4.SetPosition(pos_up_right);
+        wait(0.005);
+        pos_up_right = pos_up_right + step_up_right;
+        if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
+            state_count_right = 3;
         }
-        else if(state_count_right == 3){
-            Servo3.SetPosition(pos_down_right);
-            //wait(0.001);
-            pos_down_right = pos_down_right - step;
-            if(pos_down_right <= pos_down_start_right - step and pos_up_right >= pos_up_end_right + step_right){
-                state_count_right = 4;
-            }
-            pc.printf("RCD");
-            pc.printf("%f\n",pos_down_right);
-            pc.printf("RCP");
-            pc.printf("%f\n",pos_up_right);
+        /*pc.printf("RBD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RBP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if(state_count_right == 3) {
+        Servo3.SetPosition(pos_down_right);
+        wait(0.005);
+        pos_down_right = pos_down_right - step_down_right;
+        if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) {
+            state_count_right = 4;
         }
-        else if(state_count_right == 4){
-            Servo4.SetPosition(pos_up_right);
-            //wait(0.001);
-            pos_up_right = pos_up_right - step_right;
-            if(pos_down_right <= pos_down_start_right - step and pos_up_right <= pos_up_start_right - step_right){
-                state_count_right = 0;
-            }
-            pc.printf("RDD");
-            pc.printf("%f\n",pos_down_right);
-            pc.printf("RDP");
-            pc.printf("%f\n",pos_up_right);
+        /*pc.printf("RCD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RCP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if(state_count_right == 4) {
+        Servo4.SetPosition(pos_up_right);
+        wait(0.005);
+        pos_up_right = pos_up_right - step_up_right;
+        if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) {
+            state_count_right = 0;
         }
-        else if (state_count_right == 0 and round_count_right < 1){
-            round_count_right = round_count_right+1;
-            state_count_right = 1;
-            pos_down_right = 1400;
-            pos_up_right = 1000;
-        }
+        /*pc.printf("RDD");
+        pc.printf("%f\n",pos_down_right);
+        pc.printf("RDP");
+        pc.printf("%f\n",pos_up_right);*/
+    } else if (state_count_right == 0 and round_count_right < round) {
+        round_count_right = round_count_right+1;
+        state_count_right = 1;
+        pos_down_right = pos_down_start;
+        pos_up_right = pos_up_start;
+    }
 }
\ No newline at end of file