File content as of revision 1:13164a15fbf6:

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

Serial pc(USBTX, USBRX);

Timer timer1;
Timer timerwalk;

Thread thread1;         
Thread thread2;

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

void IMU();
void Cal_si();
void Avg();
void Cal_Cg_linear();
void Cal_Cg_log();
void move();
void receive_hormone();
void cal_step_down(); //calculate step
void cal_step_up();
void servo();
void servo_Right();

int walking_time;
int i = 0;

float avg = 0.00;
float sum = 0.00;
float roll_data[10];
float Si = 0.00 ;
float Cg = 0.00;
float Cg_down = 0.00;
float Cg_up = 0;
float standardDeviation = 0;
float sd = 0; 
 
float pos_down_start = 1400.00;
float pos_up_start = 1000.00;
float down_degree = 90.00;
float up_degree = 45.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;
float pos_up_end_left; 
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;
float pos_up_end_right;
float state_count_right = 1;
float round_count_right = 1;
float step_up_right;
float step_down_right;

 
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                                                                                                                                //
int main() {                                                                                                                                    //
    pc.baud(1000000);                                                                                                                           //
    timer1.start(); // start timer counting                                                                                                     //                                                                                                                          
    pc.printf("PRESS '1'\n");                                                                                                                   //
    if (pc.getc() == '1'){                                                                                                                      //
        thread2.start(servo);                                                                                                                   //                   
    }                                                                                                                                           //     
}                                                                                                                                               //        
                                                                                                                                                //
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////Control SERVO////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void servo() {
    pc.printf("start\n"); 
    attitude_setup();
    thread1.start(IMU);
    //wait(0.01);
    timerwalk.start(); // start timer counting
    move();
    //pc.printf("%f\n",roll_min);
    //pc.printf("%f\n",roll_max);
}

void receive_hormone(){
    down_degree = 90.00*(1.00-(0.06*Cg_down)); 
    /*pc.printf("%f \t",Si);
    pc.printf("down\t"); 
    pc.printf("%f \t",down_degree);
    pc.printf("%f\t",Cg);*/
    up_degree = 45.00*(1.00+(0.7*Cg_up));
    /*pc.printf("up\t"); 
    pc.printf("%f \t",up_degree);
    pc.printf("%f\n",Cg);*/
    if(down_degree < 85){
        down_degree = 85;
        if(up_degree > 75){
        up_degree = 75;
        }
    }
}

void cal_step_down(){
    //pc.printf("down"); 
    //pc.printf("%f \n",down_degree);
    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(){
    //pc.printf("up"); 
    //pc.printf("%f \n",up_degree);    
    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,20000);
    Servo2.Enable(1000,20000);
    Servo3.Enable(1000,20000);
    Servo4.Enable(1000,20000);
    
    /*pc.printf("Si\t"); 
    pc.printf("%f \t",Si);
    pc.printf("down\t"); 
    pc.printf("%f \t",down_degree);
    pc.printf("%f\t",Cg);
    pc.printf("up\t"); 
    pc.printf("%f \t",up_degree);
    pc.printf("%f\n",Cg);*/
    while(1) {
        cal_step_down();
        cal_step_up();
        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){
            thread1.terminate();
            pc.printf("Finish \n");
            walking_time = timerwalk.read_ms(); 
            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;
        receive_hormone();
    }
}

///////////////////////////Control IMU////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void IMU(){
    pc.printf("roll\t");
    pc.printf("Si\t");
    pc.printf("Cg\t");
    pc.printf("down\t");
    pc.printf("up\n"); 
    
    while(1) {
        if (timer1.read_us()  >=10000)// 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  %.0f \t  %.0f \n\r",  roll,   pitch, yaw);
            
            //pc.printf("up\t"); 
            //pc.printf("%f \t",up_degree);
            //pc.printf("%f\t",Cg);
            //pc.printf("down\t"); 
            //pc.printf("%f \t",down_degree);
            //pc.printf("%f\n",Cg);
         
            timer1.reset(); // reset timer 
            Cal_si();
        }
    }
}

void Cal_si(){
    if(state_count_left == 4 or state_count_right == 4){
        roll_data[i] = roll;
        //pc.printf("%f\n",roll_data[i]);
        Avg();
        //pc.printf("Avg ");
        //pc.printf("%f\n",avg);
        Cal_Cg_linear();
        //pc.printf("roll\t");
        pc.printf("%f\t",roll_data[i]);
        //pc.printf("Si\t");
        pc.printf("%f\t",Si);
        //pc.printf("Cg\t");
        pc.printf("%f\t",Cg);
        //pc.printf("down\t"); 
        pc.printf("%f \t",down_degree);
        //pc.printf("up\t"); 
        pc.printf("%f \n",up_degree);
        if(i == 9){
            i = 0;
        }else{
            i = i+1;
        }
    }
}

void Avg(){
    sum = sum + roll;
    avg = sum/10;
    standardDeviation = (roll - avg)*(roll-avg); 
    sd = sqrt(standardDeviation/10);
    if(avg < 0){
        avg = avg*(-1);
    }
}



void Cal_Cg_linear(){
    Si = sd/0.6;
    sum = sum - roll_data[i];
    if(Si > 0){
        Cg = (0.8*Si)+(0.5*Cg);
        if(Cg > 1){
            Cg = 1;
        }
    }else{
        Cg = 0.00;
    }
    Cg_down = 1/0.1*exp(2*(Cg-2.081))-0.15;
    Cg_up = (0.8*log(Cg+0.4))+0.734;
}