Diff: main.cpp

Revision:

2:68dbcd5277e4

Parent:

1:da79761252bc

Child:

3:64bfd330beb4

--- a/main.cpp	Tue Jan 30 11:54:34 2018 +0000
+++ b/main.cpp	Sun Apr 01 14:12:15 2018 +0000
@@ -1,68 +1,65 @@
 #include "mbed.h"
 #include "Servo.h"
 #include "rtos.h"
+
+Serial pc(USBTX, USBRX);
  
-Servo Servo1(D7);
-Servo Servo2(D6);
+Servo Servo1(D6);
+Servo Servo2(D8);
 Servo Servo3(D9);
-Servo Servo4(D8);
-Thread thread;
+Servo Servo4(D10);
+
+void myservoLeft();
 
-void myservoright_thread() {       
+int pos_down = 1400;
+int pos_up = 1000;
+int pos_down_end = 1700;
+int pos_up_end = 1700;
+
+int main() {
+    pc.baud(9600);
+    wait(5);
+    Servo1.Enable(1000,2000);
+    Servo2.Enable(1000,2000);
+    Servo3.Enable(1000,2000);
+    Servo4.Enable(1000,2000);
     while(1){
-    Servo3.Enable(1000,20000);
-    Servo4.Disable();
-    for (int pos = 1400; pos <= 1700; pos += 5) {
-            Servo3.SetPosition(pos);
-            wait(0.01);
+        myservoLeft();
+        if(pos_down <= pos_down_end and pos_up == 1000){
+            Servo1.SetPosition(pos_down);
+            pos_down = pos_down+2;
         }
-    Servo4.Enable(1000,20000);
-    Servo3.Disable();
-    for (int pos = 1000; pos <= 1700; pos += 5) {
-            Servo4.SetPosition(pos);
-            wait(0.01);
+        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
+            Servo2.SetPosition(pos_up);
+            pos_up = pos_up+2;
         }
-    Servo3.Enable(1000,20000);
-    Servo4.Disable();
-    for (int pos = 1700; pos >= 1400; pos -= 5) {
-            Servo3.SetPosition(pos); 
-            wait(0.01);
+        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
+            Servo1.SetPosition(pos_down);
+            pos_down = pos_down-2;
         }
-    Servo4.Enable(1000,20000);
-    Servo3.Disable();
-    for (int pos = 1700; pos >= 1000; pos -= 5) {
-            Servo4.SetPosition(pos); 
-            wait(0.01);
-        }
+        else if(pos_down == 1398 and pos_up > 1000){
+            Servo2.SetPosition(pos_up);
+            pos_up = pos_up-2;
+        }           
     }
 }
- 
-int main() {
-    while(1){
-    Servo1.Enable(1000,20000);
-    Servo2.Disable();
-    thread.start(myservoright_thread);
-    for (int pos = 1400; pos <= 1700; pos += 5) {
-            Servo1.SetPosition(pos);
-            wait(0.01);
+
+void myservoLeft() 
+{   
+        if(pos_down <= pos_down_end and pos_up == 1000){
+            Servo3.SetPosition(pos_down);
+            pos_down = pos_down+2;
         }
-    Servo2.Enable(1000,20000);
-    Servo1.Disable();
-    for (int pos = 1000; pos <= 1700; pos += 5) {
-            Servo2.SetPosition(pos);
-            wait(0.01);
+        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
+            Servo4.SetPosition(pos_up);
+            pos_up = pos_up+2;
         }
-    Servo1.Enable(1000,20000);
-    Servo2.Disable();
-    for (int pos = 1700; pos >= 1400; pos -= 5) {
-            Servo1.SetPosition(pos); 
-            wait(0.01);
+        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
+            Servo3.SetPosition(pos_down);
+            pos_down = pos_down-2;
         }
-    Servo2.Enable(1000,20000);
-    Servo1.Disable();
-    for (int pos = 1700; pos >= 1000; pos -= 5) {
-            Servo2.SetPosition(pos); 
-            wait(0.01);
-        }
-    }
-}
\ No newline at end of file
+        else if(pos_down == 1398 and pos_up > 1000){
+            Servo4.SetPosition(pos_up);
+            pos_up = pos_up-2;
+        }           
+}