File content as of revision 0:812929a5d5ad:

#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);
    }
}