File content as of revision 3:64bfd330beb4:

#include "mbed.h"
#include "Servo.h"
#include "rtos.h"

Serial pc(USBTX, USBRX);

Thread thread;
 
Servo Servo1(D6);
Servo Servo2(D8);
Servo Servo3(D9);
Servo Servo4(D10);

void servo_Right();
void move();

float pos_down = 1400;
float pos_up = 1000;
float pos_down_start = 1400;
float pos_up_start = 1000;
float pos_down_end = 1544.44;
float pos_up_end = 1400;
float state_count = 1;
float round_count = 1;
float step = 5;
float pos_down_right = 1400;
float pos_up_right = 1000;
float pos_down_start_right = 1400;
float pos_up_start_right = 1000;
float pos_down_end_right = 1544.44;
float pos_up_end_right = 1500;
float state_count_right = 1;
float round_count_right = 1;
float step_right = 6.25;

int main(){
    while(1){
        wait(5);
        move();
    }
}

void move() {
    pc.baud(9600);
    Servo1.Enable(1000,2000);
    Servo2.Enable(1000,2000);
    Servo3.Enable(1000,2000);
    Servo4.Enable(1000,2000);
    while(1){
        servo_Right();
        if(state_count == 1){
            Servo1.SetPosition(pos_down);
            //wait(0.001);
            pos_down = pos_down + step;
            if(pos_down >= pos_down_end + step and pos_up == pos_up_start){
                state_count = 2;
            }
            pc.printf("LAD");
            pc.printf("%f\n",pos_down);
            pc.printf("LAP");
            pc.printf("%f\n",pos_up);
        }
        else if(state_count == 2){
            Servo2.SetPosition(pos_up);
            //wait(0.001);
            pos_up = pos_up + step;
            if(pos_down >= pos_down_end + step and pos_up >= pos_up_end + step){
                state_count = 3;
            }
            pc.printf("LBD");
            pc.printf("%f\n",pos_down);
            pc.printf("LBP");
            pc.printf("%f\n",pos_up);
        }
        else if(state_count == 3){
            Servo1.SetPosition(pos_down);
            //wait(0.001);
            pos_down = pos_down - step;
            if(pos_down <= pos_down_start - step and pos_up >= pos_up_end + step){
                state_count = 4;
            }
            pc.printf("LCD");
            pc.printf("%f\n",pos_down);
            pc.printf("LCP");
            pc.printf("%f\n",pos_up);
        }
        else if(state_count == 4){
            Servo2.SetPosition(pos_up);
            //wait(0.001);
            pos_up = pos_up - step;
            if(pos_down <= pos_down_start - step and pos_up <= pos_up_start - step){
                state_count = 0;
            }
            pc.printf("LDD");
            pc.printf("%f\n",pos_down);
            pc.printf("LDP");
            pc.printf("%f\n",pos_up);
        }
        else if (state_count == 0 and round_count < 1){
            round_count = round_count+1;
            state_count = 1;
            pos_down = 1400;
            pos_up = 1000;
        }  
    }
}

void servo_Right(){
    if(state_count_right == 1){
            Servo3.SetPosition(pos_down_right);
            //wait(0.001);
            pos_down_right = pos_down_right + step;
            if(pos_down_right >= pos_down_end_right + step and pos_up_right == pos_up_start){
                state_count_right = 2;
            }
            pc.printf("RAD");
            pc.printf("%f\n",pos_down_right);
            pc.printf("RAP");
            pc.printf("%f\n",pos_up_right);
        }
        else if(state_count_right == 2){
            Servo4.SetPosition(pos_up_right);
            //wait(0.001);
            pos_up_right = pos_up_right + step_right;
            if(pos_down_right >= pos_down_end_right + step and pos_up_right >= pos_up_end_right + step_right){
                state_count_right = 3;
            }
            pc.printf("RBD");
            pc.printf("%f\n",pos_down_right);
            pc.printf("RBP");
            pc.printf("%f\n",pos_up_right);
        }
        else if(state_count_right == 3){
            Servo3.SetPosition(pos_down_right);
            //wait(0.001);
            pos_down_right = pos_down_right - step;
            if(pos_down_right <= pos_down_start_right - step and pos_up_right >= pos_up_end_right + step_right){
                state_count_right = 4;
            }
            pc.printf("RCD");
            pc.printf("%f\n",pos_down_right);
            pc.printf("RCP");
            pc.printf("%f\n",pos_up_right);
        }
        else if(state_count_right == 4){
            Servo4.SetPosition(pos_up_right);
            //wait(0.001);
            pos_up_right = pos_up_right - step_right;
            if(pos_down_right <= pos_down_start_right - step and pos_up_right <= pos_up_start_right - step_right){
                state_count_right = 0;
            }
            pc.printf("RDD");
            pc.printf("%f\n",pos_down_right);
            pc.printf("RDP");
            pc.printf("%f\n",pos_up_right);
        }
        else if (state_count_right == 0 and round_count_right < 1){
            round_count_right = round_count_right+1;
            state_count_right = 1;
            pos_down_right = 1400;
            pos_up_right = 1000;
        }
}