File content as of revision 5:d5c2e8f852fd:

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

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

void IMU()
{
    while(1) {
    if (timer1.read_us()  >=1000)// read time in ms
        {
            attitude_get();
            //pc.printf("IMU \n");
            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  %.0f \n\r",  roll,   pitch, yaw); 

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

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

int value;
int  walking_time; 

void servo_Right();
void move();
void cal_step_down();
void cal_step_up();
void servo();
void getvalue();
 
float pos_down_start = 1400.00;
float pos_up_start = 1000.00;
float down_degree = 0.00 ;
float up_degree = 0.00 ; 
float stepmin = 1;
float round = 5;
float waittime = 0.001 ; 
 
float pos_down_left = 1400.00;
float pos_up_left = 1000.00;
float pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); //left down  //90 ใน80 นอก(45)+7 ใน85 นอก+5
float pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); //left up// 15 , 30+10 , 45-1.75 , 60-5 , 75-5 , 45+5
float state_count_left = 1;
float round_count_left = 1;
float step_down_left;
float step_up_left;
 
float pos_down_right = 1400.00;
float pos_up_right = 1000.00;
float pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)))  ; //right down   //99
float pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); //right up// 15 , 30-10 , 45+1.75 , 60+5 , 75+5 , 45-5
float state_count_right = 1;
float round_count_right = 1;
float step_up_right;
float step_down_right;
 


int main() {
    pc.baud(1000000); 
    //pc.printf("malin");
    timer1.start(); // start timer counting
    //if (pc.getc() == '1')
     //{
      thread2.start(servo);
      //thread1.start(IMU); 
     //}   
}
void cal_step_down(){
    pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); 
    pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree)))  ; 
    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(){    
    pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); 
    pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); 
    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(){
    Servo1.Enable(1000,2000);
    Servo2.Enable(1000,2000);
    Servo3.Enable(1000,2000);
    Servo4.Enable(1000,2000);
    while(1) {
        servo_Right();
        if(state_count_left == 1) {
            Servo1.SetPosition(pos_down_left);
            wait(waittime);
            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(waittime);
            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(waittime);
            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(waittime);
            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 \n");
            walking_time = timerwalk.read_ms(); 
            thread1.terminate();
            pc.printf("Walking time = %d  \n", walking_time); 
            break;         
        }
    }
}
 
void servo_Right()
{
    if(state_count_right == 1) {
        Servo3.SetPosition(pos_down_right);
        wait(waittime);
        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(waittime);
        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(waittime);
        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(waittime);
        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;
    }
}
            
    
void servo() {
    attitude_setup();
    getvalue();
    pc.printf("start\n"); 
    thread1.start(IMU); 
    timerwalk.start(); // start timer counting
    //pc.printf("%f \n",down_degree );
    //pc.printf("%f \n",up_degree );
    cal_step_down();
    cal_step_up();
    //pc.printf("%f \n",down_degree );
    //pc.printf("%f \n",up_degree );
    move();
}

void getvalue() {
    pc.printf("case 1 = 90-15 \n");
    pc.printf("case 2 = 90-30 \n");
    pc.printf("case 3 = 90-45 \n");
    pc.printf("case 4 = 90-60 \n");
    pc.printf("case 5 = 90-75 \n");
    pc.printf("case 6 = 80-45 \n");
    pc.printf("case 7 = 85-45 \n");
    pc.printf("case 8 = 95-45 \n");
    pc.printf("case 9 = 100-45 \n");
        value = pc.getc() ; 
        switch (value) {
         case '1': { 
            down_degree = 90.00 ;
            up_degree = 15.00 ; 
            break;
            }
        case '2': {
            down_degree = 90.00 ;
            up_degree = 30.00 ; 
            break;
            }
        case '3': {
            down_degree = 90.00 ;
            up_degree = 45.00 ; 
            break;
            }
        case '4': {
            down_degree = 90.00 ;
            up_degree = 60.00 ; 
            break;
            }
        case '5': {
            down_degree = 90.00 ;
            up_degree = 75.00 ; 
            break;
            }
        case '6': {
            down_degree = 80.00 ;
            up_degree = 45.00 ; 
            break;
            }
        case '7': {
            down_degree = 85.00 ;
            up_degree = 45.00 ; 
            break;
            }
        case '8': {
            down_degree = 95.00 ;
            up_degree = 45.00 ; 
            break;
            }
        case '9': {
            down_degree = 100.00 ;
            up_degree = 45.00 ; 
            break;
            }
        break;
}
    //pc.printf("%f \n",down_degree );
    //pc.printf("%f \n",up_degree );
    
    }