File content as of revision 4:d00236029c9d:

#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();
void cal_step_down();
void cal_step_up();

float pos_down_start = 1400;
float pos_up_start = 1000;
float stepmin = 5;
float round = 5;

float pos_down_left = 1400;
float pos_up_left = 1000;
float pos_down_end_left = 1700; //left down
float pos_up_end_left = 1350; //left up
float state_count_left = 1;
float round_count_left = 1;
float step_down_left;
float step_up_left;

float pos_down_right = 1400;
float pos_up_right = 1000;
float pos_down_end_right = 1650; //right down
float pos_up_end_right = 1700; //right up
float state_count_right = 1;
float round_count_right = 1;
float step_up_right;
float step_down_right;

int main()
{
    wait(5);
    cal_step_down();
    cal_step_up();
    move();
}

void cal_step_down(){
    if (pos_down_end_right > pos_down_end_left){
        step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start);
        step_down_left = stepmin;
    } else if (pos_down_end_right < pos_down_end_left){
        step_down_right = stepmin;
        step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start);
    } else{
        step_down_right = stepmin;
        step_down_left = stepmin;
    }
    /*pc.printf("pos_down_right");
    pc.printf("%f\n",pos_down_end_right);
    pc.printf("pos_down_left");
    pc.printf("%f\n",pos_down_end_left);
    pc.printf("step_down_right");
    pc.printf("%f\n",step_down_right);
    pc.printf("step_down_left");
    pc.printf("%f\n",step_down_left);*/
}

void cal_step_up(){    
    if (pos_up_end_right > pos_up_end_left){
        step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start);
        step_up_left = stepmin;
    } else if (pos_up_end_right < pos_up_end_left){
        step_up_right = stepmin;
        step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start);
    } else{
        step_up_right = stepmin;
        step_up_left = stepmin;
    }
    /*pc.printf("pos_up_right");
    pc.printf("%f\n",pos_up_end_right);
    pc.printf("pos_up_left");
    pc.printf("%f\n",pos_up_end_left);
    pc.printf("step_up_right");
    pc.printf("%f\n",step_up_right);;
    pc.printf("step_up_left");
    pc.printf("%f\n",step_up_left);*/
}

void move(){
    pc.baud(9600);
    Servo1.Enable(1000,2000);
    Servo2.Enable(1000,2000);
    Servo3.Enable(1000,2000);
    Servo4.Enable(1000,2000);
    pc.printf("Start\n");
    while(1) {
        servo_Right();
        if(state_count_left == 1) {
            Servo1.SetPosition(pos_down_left);
            wait(0.005);
            pos_down_left = pos_down_left + step_down_left;
            if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) {
                state_count_left = 2;
            }
            /*pc.printf("LAD");
            pc.printf("%f\n",pos_down_left);
            pc.printf("LAP");
            pc.printf("%f\n",pos_up_left);*/
        } else if(state_count_left == 2) {
            Servo2.SetPosition(pos_up_left);
            wait(0.005);
            pos_up_left = pos_up_left + step_up_left;
            if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
                state_count_left = 3;
            }
            /*pc.printf("LBD");
            pc.printf("%f\n",pos_down_left);
            pc.printf("LBP");
            pc.printf("%f\n",pos_up_left);*/
        } else if(state_count_left == 3) {
            Servo1.SetPosition(pos_down_left);
            wait(0.005);
            pos_down_left = pos_down_left - step_down_left;
            if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) {
                state_count_left = 4;
            }
            /*pc.printf("LCD");
            pc.printf("%f\n",pos_down_left);
            pc.printf("LCP");
            pc.printf("%f\n",pos_up_left);*/
        } else if(state_count_left == 4) {
            Servo2.SetPosition(pos_up_left);
            wait(0.005);
            pos_up_left = pos_up_left - step_up_left;
            if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) {
                state_count_left = 0;
            }
            /*pc.printf("LDD");
            pc.printf("%f\n",pos_down_left);
            pc.printf("LDP");
            pc.printf("%f\n",pos_up_left);*/
        } else if (state_count_left == 0 and round_count_left < round) {
            round_count_left = round_count_left+1;
            state_count_left = 1;
            pos_down_left = pos_down_start;
            pos_up_left = pos_up_start;
        } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right < round){
            pc.printf("Finish");
            break;        
        }
    }
}

void servo_Right()
{
    if(state_count_right == 1) {
        Servo3.SetPosition(pos_down_right);
        wait(0.005);
        pos_down_right = pos_down_right + step_down_right;
        if(pos_down_right >= pos_down_end_right + step_down_right 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.005);
        pos_up_right = pos_up_right + step_up_right;
        if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_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.005);
        pos_down_right = pos_down_right - step_down_right;
        if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_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.005);
        pos_up_right = pos_up_right - step_up_right;
        if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_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 < round) {
        round_count_right = round_count_right+1;
        state_count_right = 1;
        pos_down_right = pos_down_start;
        pos_up_right = pos_up_start;
    }
}