File content as of revision 1:852156b5cca1:

#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 myservoLeft();
void move();

int pos_down = 1400;
int pos_up = 1000;
int pos_down_end = 1700;
int pos_up_end = ;  
int final_posdown = 1700 ;             
int final_posup = 1466.66;                                                                                                                    
int state_count = 1;
int round_count = 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 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){
        
        if(state_count == 1){
            Servo1.SetPosition(pos_down);
            Servo3.SetPosition(pos_down);
            pos_down = pos_down+5;
            wait(0.01);
            if(pos_down >= pos_down_end+5 and pos_up >= 1000){
                state_count = 2;
            }
        }
        else if(state_count == 2){
            Servo2.SetPosition(pos_up);
            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){
            Servo1.SetPosition(pos_down);
            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){
            Servo2.SetPosition(pos_up);
            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"); 
             } 
    }
}