Diff: main.cpp

Revision:

3:64bfd330beb4

Parent:

2:68dbcd5277e4

Child:

4:d00236029c9d

--- a/main.cpp	Sun Apr 01 14:12:15 2018 +0000
+++ b/main.cpp	Mon Apr 02 11:37:43 2018 +0000
@@ -3,63 +3,161 @@
 #include "rtos.h"
 
 Serial pc(USBTX, USBRX);
+
+Thread thread;
  
 Servo Servo1(D6);
 Servo Servo2(D8);
 Servo Servo3(D9);
 Servo Servo4(D10);
 
-void myservoLeft();
+void servo_Right();
+void move();
 
-int pos_down = 1400;
-int pos_up = 1000;
-int pos_down_end = 1700;
-int pos_up_end = 1700;
+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 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 state_count_right = 1;
+float round_count_right = 1;
+float step_right = 6.25;
 
-int main() {
+int main(){
+    while(1){
+        wait(5);
+        move();
+    }
+}
+
+void move() {
     pc.baud(9600);
-    wait(5);
     Servo1.Enable(1000,2000);
     Servo2.Enable(1000,2000);
     Servo3.Enable(1000,2000);
     Servo4.Enable(1000,2000);
     while(1){
-        myservoLeft();
-        if(pos_down <= pos_down_end and pos_up == 1000){
+        servo_Right();
+        if(state_count == 1){
             Servo1.SetPosition(pos_down);
-            pos_down = pos_down+2;
+            //wait(0.001);
+            pos_down = pos_down + step;
+            if(pos_down >= pos_down_end + step and pos_up == pos_up_start){
+                state_count = 2;
+            }
+            pc.printf("LAD");
+            pc.printf("%f\n",pos_down);
+            pc.printf("LAP");
+            pc.printf("%f\n",pos_up);
         }
-        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
+        else if(state_count == 2){
             Servo2.SetPosition(pos_up);
-            pos_up = pos_up+2;
+            //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("LBD");
+            pc.printf("%f\n",pos_down);
+            pc.printf("LBP");
+            pc.printf("%f\n",pos_up);
         }
-        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
+        else if(state_count == 3){
             Servo1.SetPosition(pos_down);
-            pos_down = pos_down-2;
+            //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(pos_down == 1398 and pos_up > 1000){
+        else if(state_count == 4){
             Servo2.SetPosition(pos_up);
-            pos_up = pos_up-2;
-        }           
+            //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 myservoLeft() 
-{   
-        if(pos_down <= pos_down_end and pos_up == 1000){
-            Servo3.SetPosition(pos_down);
-            pos_down = pos_down+2;
+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);
         }
-        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
-            Servo4.SetPosition(pos_up);
-            pos_up = pos_up+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);
         }
-        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
-            Servo3.SetPosition(pos_down);
-            pos_down = pos_down-2;
+        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);
         }
-        else if(pos_down == 1398 and pos_up > 1000){
-            Servo4.SetPosition(pos_up);
-            pos_up = pos_up-2;
-        }           
-}
+        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);
+        }
+        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;
+        }
+}
\ No newline at end of file