main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "rtos.h"
00004 
00005 Serial pc(USBTX, USBRX);
00006  
00007 Servo Servo1(D7);
00008 Servo Servo2(D6);
00009 Servo Servo3(D9);
00010 Servo Servo4(D8);
00011 Thread thread;
00012 
00013 int pos_up_start;
00014 int pos_up_end;
00015 int pos_down_start;
00016 int pos_down_end;
00017 
00018 void myservoright_thread() {       
00019     for(int n = 0; n <= 5; n += 1){
00020     Servo3.Enable(1000,20000);
00021     Servo4.Disable();
00022     for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
00023             Servo3.SetPosition(pos);
00024             wait(0.01);
00025         }
00026     Servo4.Enable(1000,20000);
00027     Servo3.Disable();
00028     for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
00029             Servo4.SetPosition(pos);
00030             wait(0.01);
00031         }
00032     Servo3.Enable(1000,20000);
00033     Servo4.Disable();
00034     for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
00035             Servo3.SetPosition(pos); 
00036             wait(0.01);
00037         }
00038     Servo4.Enable(1000,20000);
00039     Servo3.Disable();
00040     for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
00041             Servo4.SetPosition(pos); 
00042             wait(0.01);
00043         }
00044     }
00045 }
00046 
00047 void turtlewalk(int pos_down_start,int pos_down_end,int pos_up_start,int pos_up_end){
00048     for(int n = 0; n <= 5; n += 1){
00049     Servo1.Enable(1000,20000);
00050     Servo2.Disable();
00051     thread.start(myservoright_thread);
00052     for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) {
00053             Servo1.SetPosition(pos);
00054             wait(0.01);
00055         }
00056     Servo2.Enable(1000,20000);
00057     Servo1.Disable();
00058     for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) {
00059             Servo2.SetPosition(pos);
00060             wait(0.01);
00061         }
00062     Servo1.Enable(1000,20000);
00063     Servo2.Disable();
00064     for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) {
00065             Servo1.SetPosition(pos); 
00066             wait(0.01);
00067         }
00068     Servo2.Enable(1000,20000);
00069     Servo1.Disable();
00070     for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) {
00071             Servo2.SetPosition(pos); 
00072             wait(0.01);
00073         }
00074     }
00075 }
00076 
00077 int main(){
00078     
00079     while(1) {
00080         printf("Hello World! Turtlebot is READY\n");
00081         printf("case 1-5\n");                 
00082         switch(pc.getc()) {
00083             case '1':
00084                 pos_down_start = 1400;
00085                 pos_down_end = 1700; 
00086                 pos_up_start = 1000; 
00087                 pos_up_end = 1700; 
00088                 break;
00089             case '2':
00090                 pos_down_start = 1400;
00091                 pos_down_end = 1600; 
00092                 pos_up_start = 1000; 
00093                 pos_up_end = 1600;
00094                 break;
00095             case '3':
00096                 pos_down_start = 1400;
00097                 pos_down_end = 1650; 
00098                 pos_up_start = 1000; 
00099                 pos_up_end = 1500;
00100                 break;
00101             case '4':
00102                 pos_down_start = 1400;
00103                 pos_down_end = 1700; 
00104                 pos_up_start = 1000; 
00105                 pos_up_end = 1650;
00106                 break; 
00107             case '5':
00108                 pos_down_start = 1400;
00109                 pos_down_end = 1600; 
00110                 pos_up_start = 1000; 
00111                 pos_up_end = 1550;
00112                 break;  
00113         }
00114     printf("position down motor start = %d\n", pos_down_start);
00115     printf("position down motor end = %d\n", pos_down_end);
00116     printf("position up motor start = %d\n", pos_up_start);
00117     printf("position up motor end = %d\n", pos_up_end);
00118     turtlewalk(pos_down_start,pos_down_end,pos_up_start,pos_up_end);
00119     }
00120 }