Diff: main.cpp

Revision:

0:812929a5d5ad

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 09 15:59:52 2018 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "rtos.h"
+
+Serial pc(USBTX, USBRX);
+ 
+Servo Servo1(D7);
+Servo Servo2(D6);
+Servo Servo3(D9);
+Servo Servo4(D8);
+Thread thread;
+
+int pos_up_start;
+int pos_up_end;
+int pos_down_start;
+int pos_down_end;
+
+void myservoright_thread() {       
+    for(int n = 0; n <= 5; n += 1){
+    Servo3.Enable(1000,20000);
+    Servo4.Disable();
+    for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
+            Servo3.SetPosition(pos);
+            wait(0.01);
+        }
+    Servo4.Enable(1000,20000);
+    Servo3.Disable();
+    for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
+            Servo4.SetPosition(pos);
+            wait(0.01);
+        }
+    Servo3.Enable(1000,20000);
+    Servo4.Disable();
+    for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
+            Servo3.SetPosition(pos); 
+            wait(0.01);
+        }
+    Servo4.Enable(1000,20000);
+    Servo3.Disable();
+    for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
+            Servo4.SetPosition(pos); 
+            wait(0.01);
+        }
+    }
+}
+
+void turtlewalk(int pos_down_start,int pos_down_end,int pos_up_start,int pos_up_end){
+    for(int n = 0; n <= 5; n += 1){
+    Servo1.Enable(1000,20000);
+    Servo2.Disable();
+    thread.start(myservoright_thread);
+    for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
+            Servo1.SetPosition(pos);
+            wait(0.01);
+        }
+    Servo2.Enable(1000,20000);
+    Servo1.Disable();
+    for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
+            Servo2.SetPosition(pos);
+            wait(0.01);
+        }
+    Servo1.Enable(1000,20000);
+    Servo2.Disable();
+    for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
+            Servo1.SetPosition(pos); 
+            wait(0.01);
+        }
+    Servo2.Enable(1000,20000);
+    Servo1.Disable();
+    for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
+            Servo2.SetPosition(pos); 
+            wait(0.01);
+        }
+    }
+}
+
+int main(){
+    
+    while(1) {
+        printf("Hello World! Turtlebot is READY\n");
+        printf("case 1-5\n");                 
+        switch(pc.getc()) {
+            case '1':
+                pos_down_start = 1400;
+                pos_down_end = 1700; 
+                pos_up_start = 1000; 
+                pos_up_end = 1700; 
+                break;
+            case '2':
+                pos_down_start = 1400;
+                pos_down_end = 1600; 
+                pos_up_start = 1000; 
+                pos_up_end = 1600;
+                break;
+            case '3':
+                pos_down_start = 1400;
+                pos_down_end = 1650; 
+                pos_up_start = 1000; 
+                pos_up_end = 1500;
+                break;
+            case '4':
+                pos_down_start = 1400;
+                pos_down_end = 1700; 
+                pos_up_start = 1000; 
+                pos_up_end = 1650;
+                break; 
+            case '5':
+                pos_down_start = 1400;
+                pos_down_end = 1600; 
+                pos_up_start = 1000; 
+                pos_up_end = 1550;
+                break;  
+        }
+    printf("position down motor start = %d\n", pos_down_start);
+    printf("position down motor end = %d\n", pos_down_end);
+    printf("position up motor start = %d\n", pos_up_start);
+    printf("position up motor end = %d\n", pos_up_end);
+    turtlewalk(pos_down_start,pos_down_end,pos_up_start,pos_up_end);
+    }
+}
\ No newline at end of file