Diff: main.cpp

Revision:

1:da79761252bc

Parent:

0:596de9a5c573

Child:

2:68dbcd5277e4

--- a/main.cpp	Tue Jan 30 11:01:49 2018 +0000
+++ b/main.cpp	Tue Jan 30 11:54:34 2018 +0000
@@ -1,29 +1,68 @@
 #include "mbed.h"
 #include "Servo.h"
+#include "rtos.h"
  
-Servo Servo1(D6);
-Servo Servo2(D7);
+Servo Servo1(D7);
+Servo Servo2(D6);
+Servo Servo3(D9);
+Servo Servo4(D8);
+Thread thread;
+
+void myservoright_thread() {       
+    while(1){
+    Servo3.Enable(1000,20000);
+    Servo4.Disable();
+    for (int pos = 1400; pos <= 1700; pos += 5) {
+            Servo3.SetPosition(pos);
+            wait(0.01);
+        }
+    Servo4.Enable(1000,20000);
+    Servo3.Disable();
+    for (int pos = 1000; pos <= 1700; pos += 5) {
+            Servo4.SetPosition(pos);
+            wait(0.01);
+        }
+    Servo3.Enable(1000,20000);
+    Servo4.Disable();
+    for (int pos = 1700; pos >= 1400; pos -= 5) {
+            Servo3.SetPosition(pos); 
+            wait(0.01);
+        }
+    Servo4.Enable(1000,20000);
+    Servo3.Disable();
+    for (int pos = 1700; pos >= 1000; pos -= 5) {
+            Servo4.SetPosition(pos); 
+            wait(0.01);
+        }
+    }
+}
  
 int main() {
+    while(1){
     Servo1.Enable(1000,20000);
-    Servo2.Enable(1000,20000);
     Servo2.Disable();
-    for (int pos = 1000; pos < 1500; pos += 25) {
+    thread.start(myservoright_thread);
+    for (int pos = 1400; pos <= 1700; pos += 5) {
             Servo1.SetPosition(pos);
-            wait(0.05);
-        }
-    for (int pos = 1500; pos > 1000; pos -= 25) {
-            Servo1.SetPosition(pos); 
-            wait(0.05);
+            wait(0.01);
         }
     Servo2.Enable(1000,20000);
     Servo1.Disable();
-    for (int pos = 1000; pos < 1500; pos += 25) {
+    for (int pos = 1000; pos <= 1700; pos += 5) {
             Servo2.SetPosition(pos);
-            wait(0.05);
+            wait(0.01);
         }
-    for (int pos = 1500; pos > 1000; pos -= 25) {
+    Servo1.Enable(1000,20000);
+    Servo2.Disable();
+    for (int pos = 1700; pos >= 1400; pos -= 5) {
+            Servo1.SetPosition(pos); 
+            wait(0.01);
+        }
+    Servo2.Enable(1000,20000);
+    Servo1.Disable();
+    for (int pos = 1700; pos >= 1000; pos -= 5) {
             Servo2.SetPosition(pos); 
-            wait(0.05);
+            wait(0.01);
         }
+    }
 }
\ No newline at end of file