Diff: main.cpp

Revision:

0:3af6fae90816

Child:

1:852156b5cca1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Apr 01 14:20:53 2018 +0000
@@ -0,0 +1,65 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "rtos.h"
+
+Serial pc(USBTX, USBRX);
+ 
+Servo Servo1(D6);
+Servo Servo2(D8);
+Servo Servo3(D9);
+Servo Servo4(D10);
+
+void myservoLeft();
+
+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){
+        myservoLeft();
+        if(pos_down <= pos_down_end and pos_up == 1000){
+            Servo1.SetPosition(pos_down);
+            pos_down = pos_down+2;
+        }
+        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
+            Servo2.SetPosition(pos_up);
+            pos_up = pos_up+2;
+        }
+        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
+            Servo1.SetPosition(pos_down);
+            pos_down = pos_down-2;
+        }
+        else if(pos_down == 1398 and pos_up > 1000){
+            Servo2.SetPosition(pos_up);
+            pos_up = pos_up-2;
+        }           
+    }
+}
+
+void myservoLeft() 
+{   
+        if(pos_down <= pos_down_end and pos_up == 1000){
+            Servo3.SetPosition(pos_down);
+            pos_down = pos_down+2;
+        }
+        else if(pos_down == pos_down_end+2 and pos_up <= 1700){
+            Servo4.SetPosition(pos_up);
+            pos_up = pos_up+2;
+        }
+        else if(pos_down >= 1400 and pos_up == pos_up_end+2){
+            Servo3.SetPosition(pos_down);
+            pos_down = pos_down-2;
+        }
+        else if(pos_down == 1398 and pos_up > 1000){
+            Servo4.SetPosition(pos_up);
+            pos_up = pos_up-2;
+        }           
+}