File content as of revision 5:08334c6a42ca:

//////////////////////////////////////////////////////////////////
// project:   TurtleBot Project                                 //
// code v.:   0.0                                               //  
// board  :   NUCLEO-F303KB                                     //
// date   :   19/6/2018                                         //
// code by:   Worasuchad Haomachai                              //
// detail :                                                     //
////////////////////////////////////////////////////////////////// 
 
///////////////////////// init    ////////////////////////////////
//////////////////////////////////////////////////////////////////
#include "mbed.h"
#include "Servo.h"
#include "rtos.h"
#include "attitude.h"
#include "math.h"

Serial pc(USBTX, USBRX);                            //Serial Port

Timer timer1;
Timer timerwalk;
Thread thread1;         
Thread thread2;

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

///////////////////////// prototype func   ///////////////////////
//////////////////////////////////////////////////////////////////
void IMU();
void servo();
void servoFirstState();
void cal_step_down();
void cal_step_up();
void servo_Left();
void servo_Right();

void receive_hormone();
float calculateSD(float data[]);
float calculateMean(float meanData[]);
float HormoneCon(float );
bool checkIMUFirst(float , float , float );

////// debug func ////// 
void printStateGait();

///////////////////////// variable ///////////////////////////////
//////////////////////////////////////////////////////////////////
// home param
int walking_time;
int initCheck = 0;
float waittime = 0.001 ;
float round = 6;

// hormone param
float Cg_down = 0.00;
float Cg_up = 0;
float Cg = 0.00, CgPrevious = 0.00;

// servo motor param
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.5;
 
// servo left side
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;
// servo right side
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;

////// debug param //////
// print state gait
int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0;
/////////////////////////    main     ////////////////////////////
//////////////////////////////////////////////////////////////////                                                                                                                                               
int main() 
{                                                                                                                                    
    pc.baud(115200);                                                                                                                           
    timer1.start();                                     // start timer counting
    attitude_setup();                                   // IMU setup
    //thread1.start(IMU);                                 // IMU thread start                                                                                                                                                                                                                                 
    pc.printf(" Please press! '1' to start..\n\r");                                                                                                                   
    if (pc.getc() == '1')
    {
        thread1.start(IMU);                                 // IMU thread start                                                                                                                      
        //thread2.start(servo);                           // servo thread start                                                                                                                                      
    }                                                                      
}

/////////////////////////    IMU     /////////////////////////////
//////////////////////////////////////////////////////////////////  
void IMU()
{
    int i;
    float ArrayOfRoll[10], ArrayOfPitch[10], ArrayOfYaw[10];
    float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw;
    float DiffOfRP = 0.0f;
/*  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()  >= 500)                     // read time in 0.5 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  %f\t  %f\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);

            ////////////////////////// Signal Pre-Process every 10 ms //////////////////////
            ////////////////////////////////////////////////////////////////////////////////
            if(i < 10)
            {
                ArrayOfRoll[i] = roll;
                ArrayOfPitch[i] = pitch;
                ArrayOfYaw[i] = yaw;
                //pc.printf("i = %i  ,ArrayOfRoll = %.2f,  roll= %.2f\n\r",i, ArrayOfRoll[i], roll);
                i++;
            }
            else  // every 5 ms
            {
                ////// print state of gait /////
                //printStateGait();
                
                // the accleration 
                //pc.printf("%.3f\t\t", gx*PI/180.0f);
                //pc.printf("%.3f\t\t", gy*PI/180.0f);
                //pc.printf("%.3f\t\t", gz*PI/180.0f);
                
                // the gyro value
                //pc.printf("%.3f\t\t", ax * 9.81f );         // convert g to m/s^2
                //pc.printf("%.3f\t\t", ay * 9.81f);          // convert g to m/s^2
                //pc.printf("%.3f\n\r", ( az - 1 ) * 9.81f);  // m/s^2 and 1g for eliminating earth gravity 
                
                // diff roll pitch
                if(state_count_left == 4 and state_count_right == 4 )
                {
                    
                    DiffOfRP = sqrt( pow(roll,2) + pow(pitch,2) );
                    pc.printf("%.3f\n\r", DiffOfRP);
                    DiffOfRP = 0.0f;
                }
                //////////// roll //////////////
                SDOfRoll = calculateSD(ArrayOfRoll);
                //pc.printf("%.3f\t\t", SDOfRoll); 
                //pc.printf("%.3f\t\t", roll); 
                
                //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll));
                //HormoneCon(SDOfRoll);
                
                //////////// pitch ///////////////
                SDOfPitch = calculateSD(ArrayOfPitch);
                //pc.printf("%.3f\t\t", SDOfPitch);
                //pc.printf("%.3f\t\t", pitch);
                
                //////////// yaw ///////////////
                SDOfYaw = calculateSD(ArrayOfYaw);
                //pc.printf("%.3f\t\t", SDOfYaw);
                //pc.printf("%.3f\n\r", yaw);
                
                if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw))    // only one time for comming 
                {
                    FirstOfRoll = calculateMean(ArrayOfRoll);
                    FirstOfPitch = calculateMean(ArrayOfPitch);
                    FirstOfYaw = calculateMean(ArrayOfYaw);
                    pc.printf("FirstOfRoll: %.3f, FirstOfPitch: %.3f, FirstOfYaw: %.3f\n\r", FirstOfRoll, FirstOfPitch, FirstOfYaw);
                    thread2.start(servo);                 // Servo Thread
                    pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r");
                }
              
                //pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
                // func find diff beween directOfRobot and MeanOfYaw
                //pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw));
               
                /*pc.printf("Cg_down: %f  Cg_up: %f \t", Cg_down, Cg_up); 
                pc.printf("down: \t"); 
                pc.printf("%f \t",down_degree);
                pc.printf("up: \t"); 
                pc.printf("%f \n\r",up_degree);*/       
               
                // reset iteration
                i = 0;
            }
            
            timer1.reset();                                 // reset timer 
        }
    }
}

/////////////////////////    servo     ///////////////////////////
////////////////////////////////////////////////////////////////// 
void servo()
{
    /*Servo1.Enable(1000,20000);
    Servo2.Enable(1000,20000);
    Servo3.Enable(1000,20000);
    Servo4.Enable(1000,20000);*/
    
    pc.printf("\n\r Servo Start Ja!!! \n\r"); 
    timerwalk.start();                                  // start timer counting   

    while(1) 
    {
        receive_hormone();
        cal_step_down();            // return "step_down_right" and "step_down_left"
        cal_step_up();              // return "step_up_right" and "step_up_left"
        servo_Left();               // control left lag
        servo_Right();              // control right leg
        
        // fin for walking 
        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! \t");
            walking_time = timerwalk.read_ms(); 
            pc.printf("Walking time = %d  ms\n\r", walking_time);   
            break;      
        }
    }
}                                                                                                                                                                                                                                                                                                

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

/////////////////////////    cal_step_down     ///////////////////
//////////////////////////////////////////////////////////////////
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)));     // get degree for hormone receiver about down_degree ~ 90*,  
    pos_down_end_right = (1060.00 + ((700.00/90.00)*(down_degree)));    // so both pos_down_end_left and pos_down_end_right are around 1700
    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);   //stepmin = 1, pos_down_start = 1400.00
        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                // pos_down_end_right == pos_down_end_left
    {
        step_down_right = stepmin;
        step_down_left = stepmin;
    }
    /*pc.printf("pos_down_right: ");
    pc.printf("%f\t\t",pos_down_end_right);
    pc.printf("pos_down_left: ");
    pc.printf("%f\t\t",pos_down_end_left);
    pc.printf("step_down_right: ");
    pc.printf("%f\t\t",step_down_right);
    pc.printf("step_down_left: ");
    pc.printf("%f\n\r",step_down_left);*/
}

/////////////////////////    cal_step_up     /////////////////////
////////////////////////////////////////////////////////////////// 
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));                   // get degree for hormone receiver about up_degree ~ 45*,
    pos_up_end_right = 1000.00 + ((700.00/90.00)*(up_degree));                  // so both pos_up_end_left and pos_up_end_right are around 1350
    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);         //stepmin = 1, pos_up_start = 1000.00
        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 == step_up_left
    {
        step_up_right = stepmin;
        step_up_left = stepmin;
    }
    /*pc.printf("pos_up_right: ");
    pc.printf("%f\t\t",pos_up_end_right);
    pc.printf("pos_up_left: ");
    pc.printf("%f\t\t",pos_up_end_left);
    pc.printf("step_up_right: ");
    pc.printf("%f\t\t",step_up_right);;
    pc.printf("step_up_left: ");
    pc.printf("%f\n\r",step_up_left);*/
}

/////////////////////   Print State Gait    //////////////////////
//////////////////////////////////////////////////////////////////
void printStateGait()
{
    if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0)
    {
        pc.printf("\n\r State Gait 1 \n\r");
        stateGaitOne = 1; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 0; 
    }
    else if(state_count_left == 2 and state_count_right == 2 and stateGaitTwo == 0)
    {
        pc.printf("\n\r State Gait 2 \n\r");
        stateGaitOne = 0; stateGaitTwo = 1; stateGaitThree = 0; stateGaitFour = 0;
    }
    else if(state_count_left == 3 and state_count_right == 3 and stateGaitThree == 0)
    {
        pc.printf("\n\r State Gait 3 \n\r");
        stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 1; stateGaitFour = 0;
    }
    else if(state_count_left == 4 and state_count_right == 4 and stateGaitFour == 0)
    {
        pc.printf("\n\r State Gait 4 \n\r");
        stateGaitOne = 0; stateGaitTwo = 0; stateGaitThree = 0; stateGaitFour = 1;
    }
} 

/////////////////////////    servo_Left     //////////////////////
//////////////////////////////////////////////////////////////////  
void servo_Left()
{
        if(state_count_left == 1)   
        {
            Servo1.SetPosition(pos_down_left);      // pos_down_left = 1400.00
            wait(waittime);         // 0.001 ms
            pos_down_left += step_down_left;
            if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start)     // pos_down_end_left ~ 1700
            {
                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);        // pos_up_left = 1000.00
            wait(waittime);
            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 -= 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 -= 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++;
            state_count_left = 1;
            pos_down_left = pos_down_start;
            pos_up_left = pos_up_start;
        } 
}

/////////////////////////    servo_Right     /////////////////////
//////////////////////////////////////////////////////////////////  
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;
    }
}

///////////////// Hormone Concentration      /////////////////////
//////////////////////////////////////////////////////////////////
float HormoneCon(float SiPreProcess)
{
    float CgTemp;
    
    //pc.printf("SiPreProcess: %.3f\t", SiPreProcess);
    //pc.printf("CgPrevious: %.3f\t", CgPrevious);
    
    ////// hormone gland //////
    CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious);
    //pc.printf("CgTemp: %.3f\t\t", CgTemp);
    Cg = 1/( 1+exp(-CgTemp) );           // used sigmoid func for calculating Cg which much have value between 0 - 1 
    //pc.printf("Cg: %.3f\n\r", Cg);
    ///////////////////////////
    
    //pc.printf("First Term: %.3f\t", (float)(0.8f*SiPreProcess));
    //pc.printf("Secound Term: %.3f\t", (float)(0.5f*CgPrevious));
    CgPrevious = Cg;
    //*********//
    Cg_down = Cg;
    Cg_up   = Cg;
    
    return Cg;
}

/////////////////////////    calculateSD     /////////////////////
//////////////////////////////////////////////////////////////////
float calculateSD(float sdData[])
{
    float sum = 0.0, mean, standardDeviation = 0.0;
    int i;
    
    for(i = 0; i < 10 ; ++i)
    {
        sum += sdData[i];
        //pc.printf("sum = %.2f \n\r",sum);    
    }    
    mean = sum/10;
    //pc.printf("mean = %.2f \n\r",mean); 
    
    for(i = 0; i < 10; ++i)
    {
        standardDeviation += pow(sdData[i] - mean, 2);
        //pc.printf("standardDeviation = %.2f \n\r",standardDeviation);
    }
    return sqrt(standardDeviation / 10);
}

/////////////////////////    calculateMean     ///////////////////
//////////////////////////////////////////////////////////////////
float calculateMean(float meanData[])
{
    float sum = 0.0, mean;
    int i;
    
    for(i = 0; i < 10 ; ++i)
    {
        sum += meanData[i];
        //pc.printf("sum = %.2f \n\r",sum);    
    }    
    mean = sum/10;
    //pc.printf("mean = %.2f \n\r",mean); 
    return mean;
}

/////////////////////////    check IMU first    //////////////////
//////////////////////////////////////////////////////////////////
bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw)
{    
    if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 0)
    {
        servoFirstState();
        initCheck++;
    }
    else if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 1)
    {
        initCheck++;
        return 1;   
    }
    return 0;
}

/////////////////////  servo First State     /////////////////////
////////////////////////////////////////////////////////////////// 
void servoFirstState()
{
    Servo1.Enable(1000,20000);
    Servo2.Enable(1000,20000);
    Servo3.Enable(1000,20000);
    Servo4.Enable(1000,20000);
    pc.printf("Servo First State\n\r");
    pc.printf("pos_down_left: %.3f\t  pos_up_left: %.3f\t  pos_down_right: %.3f\t  pos_up_right: %.3f\n\r", pos_down_left, pos_up_left, pos_down_right, pos_up_right);
    Servo1.SetPosition(pos_down_left);
    Servo2.SetPosition(pos_up_left);
    Servo3.SetPosition(pos_down_right);
    Servo4.SetPosition(pos_up_right);
       
}