File content as of revision 2:436ed0069b61:

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

Serial pc(USBTX, USBRX);
//Serial bt(A7,A2);
Timer timer1; 
Thread thread1;         

Thread thread2;

void IMU()
{
    while(1) {
    if (timer1.read_us()  >=1000)// read time in ms
        {
            attitude_get();
            pc.printf(" %f \t", ax*10 ); 
            pc.printf("  %f \t", ay*10 ); 
            pc.printf("  %f \t", az*10 - 10); //cm/s*s

            pc.printf(" %f \t", gx ); 
            pc.printf("  %f \t", gy );  
            pc.printf("  %f  \t", gz );  //deg/s    */
            pc.printf("%.0f\t  %.0f \t   \n\r",  roll,   pitch ); 

            timer1.reset(); // reset timer 
            }
            }
}

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

void servoleft();
void move();

int pos_down = 1400;
int pos_up = 1000;
int pos_down_end = 1700;
int pos_up_end = 1500;  
int edit_up = 1400;
                                                                                                                   
int state_count = 1;
int state_count2 = 1;
int round_count = 1;
int round_count2 = 1;
int main() {
    pc.baud(1000000); 
    //pc.printf("malin");
    
    timer1.start(); // start timer counting
    if (pc.getc() == '1')
    {
        thread2.start(move);
        //thread1.start(IMU); 
    }
    
}
void servoleft(){
        
        if(state_count2 == 1){
            Servo1.SetPosition(pos_down);
            //pos_down = pos_down+5;
            wait(0.01);
            pc.printf("left %d\n",pos_down);
            if(pos_down == pos_down_end+5 and pos_up == 1000){
                state_count2 = 2;
            }
        }
        else if(state_count2 == 2){
            Servo2.SetPosition(pos_up);
            //pos_up = pos_up+5;
            wait(0.01);
            pc.printf("stage2");
            if(pos_down == pos_down_end+5 and pos_up == edit_up+5){
                state_count2 = 3;
            }
        }
        else if(state_count2 == 3){
            Servo1.SetPosition(pos_down);
            //pos_down = pos_down-5;
            wait(0.01);
            pc.printf("stage3");
            if(pos_down == 1395 and pos_up == edit_up+5){
                state_count2 = 4;
            }
        }
        else if(state_count2 == 4){
            Servo2.SetPosition(pos_up);
            //pos_up = pos_up-5;
            wait(0.01); 
            pc.printf("stage4");
            if(pos_down == 1395 and pos_up == 995){
                state_count2 = 0;
            }       
        }
         else if (state_count2 == 0 and round_count2 < 10){
            round_count2 = round_count2+1;
            state_count2 = 1;
            pos_down = 1400;
            pos_up = 1000;
        }          
         else {
             pc.printf("1stop"); 
             //thread1.stop(IMU);
             } 
    

    }
void move() {
    attitude_setup();
    Servo1.Enable(1000,2000);
    Servo2.Enable(1000,2000);
    Servo3.Enable(1000,2000);
    Servo4.Enable(1000,2000);
    //pc.printf("start");
    //pc.printf("start");
    //pc.printf("start");
    //pc.printf("start");
    pc.printf("start");
    while(1){
    servoleft();        
        if(state_count == 1){
            Servo3.SetPosition(pos_down);
            pos_down = pos_down+5;
            wait(0.01);
            pc.printf("right %d\n",pos_down);
            if(pos_down == pos_down_end+5 and pos_up == 1000){
                state_count = 2;
            }
        }
        else if(state_count == 2){
            Servo4.SetPosition(pos_up);
            pos_up = pos_up+5;
            wait(0.01);
            if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){
                state_count = 3;
            }
        }
        else if(state_count == 3){
            Servo3.SetPosition(pos_down);
            pos_down = pos_down-5;
            wait(0.01);
            if(pos_down == 1395 and pos_up == pos_up_end+5){
                state_count = 4;
            }
        }
        else if(state_count == 4){
            Servo4.SetPosition(pos_up);
            pos_up = pos_up-5;
            wait(0.01); 
            if(pos_down == 1395 and pos_up == 995){
                state_count = 0;
            }       
        }
         else if (state_count == 0 and round_count < 10){
            round_count = round_count+1;
            state_count = 1;
            pos_down = 1400;
            pos_up = 1000;
        }          
         else {
             pc.printf("stop"); 
             } 
    }
}