turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 6:8ae55e1f7e76
- Parent:
- 5:08334c6a42ca
- Child:
- 7:ca887fdc5388
diff -r 08334c6a42ca -r 8ae55e1f7e76 main.cpp --- a/main.cpp Wed Jul 11 05:38:01 2018 +0000 +++ b/main.cpp Fri Jul 20 17:44:07 2018 +0000 @@ -1,8 +1,8 @@ ////////////////////////////////////////////////////////////////// // project: TurtleBot Project // -// code v.: 0.0 // +// code v.: 0.3 // // board : NUCLEO-F303KB // -// date : 19/6/2018 // +// date : 19/7/2018 // // code by: Worasuchad Haomachai // // detail : // ////////////////////////////////////////////////////////////////// @@ -15,17 +15,16 @@ #include "attitude.h" #include "math.h" -Serial pc(USBTX, USBRX); //Serial Port +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); +Servo Servo1(D4); // Servo Left Up (L1) +Servo Servo2(D5); // Servo Left Down (L2) +Servo Servo3(D8); // Servo Right Up (R1) +Servo Servo4(D9); // Servo Right Down (R2) ///////////////////////// prototype func /////////////////////// ////////////////////////////////////////////////////////////////// @@ -38,28 +37,29 @@ void servo_Right(); void receive_hormone(); -float calculateSD(float data[]); -float calculateMean(float meanData[]); +float calculateSD(float [], int ); +float calcSDGait(float [], int ); +float calculateMean(float [], int ); float HormoneCon(float ); -bool checkIMUFirst(float , float , float ); +bool checkIMUFirst(float , float, float ); ////// debug func ////// void printStateGait(); ///////////////////////// variable /////////////////////////////// ////////////////////////////////////////////////////////////////// -// home param -int walking_time; +// global variable int initCheck = 0; float waittime = 0.001 ; float round = 6; +float ArrRoll[10], ArrPitch[10], ArrYaw[10]; -// hormone param +// hormone variable float Cg_down = 0.00; float Cg_up = 0; float Cg = 0.00, CgPrevious = 0.00; -// servo motor param +// servo motor variable float pos_down_start = 1400.00; float pos_up_start = 1000.00; float down_degree = 90.00; @@ -71,47 +71,61 @@ 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; +int state_count_left = 0; +int round_count_left = 0; + // 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; +int state_count_right = 0; +int round_count_right = 0; -////// debug param ////// +// other variable +uint32_t getIMUTimer; + +////// debug variable ////// // print state gait int stateGaitOne = 0, stateGaitTwo = 0, stateGaitThree = 0, stateGaitFour = 0; +int showIMUData = 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"); + pc.baud(115200); + attitude_setup(); // IMU setup + //thread1.start(IMU); // IMU thread start + //thread2.start(servo); // servo 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 - } + thread1.start(IMU); // IMU thread start + thread2.start(servo); // servo thread start + } } ///////////////////////// IMU ///////////////////////////// ////////////////////////////////////////////////////////////////// void IMU() { - int i; - float ArrayOfRoll[10], ArrayOfPitch[10], ArrayOfYaw[10]; + int iterIMU = 0, iterArr = 0; + bool IMUWasStable = false; + float ArrayOfRoll[10] = {0.000f}, ArrayOfPitch[10] = {0.000f}, ArrayOfYaw[10] = {0.000f}; float SDOfRoll, SDOfPitch, SDOfYaw, FirstOfRoll, FirstOfPitch, FirstOfYaw; float DiffOfRP = 0.0f; + ////// new //////// + int iterAxG2 = 0; + float ArrayOfAx_G2[80] = {0}, SDOfAx_G2; + int state_count_left_old, state_count_right_old; + + timer1.start(); // start timer counting + getIMUTimer = timer1.read_ms(); /* pc.printf("roll\t"); pc.printf("Si\t"); pc.printf("Cg\t"); @@ -119,98 +133,131 @@ pc.printf("up\n"); */ while(1) { - if (timer1.read_us() >= 500) // read time in 0.5 ms - { + if ((timer1.read_ms() - getIMUTimer) >= 1) // read time in 1 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("down\t"); //pc.printf("%f \t",down_degree); //pc.printf("%f\n",Cg); ////////////////////////// Signal Pre-Process every 10 ms ////////////////////// //////////////////////////////////////////////////////////////////////////////// - if(i < 10) + if(iterIMU < 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++; + ArrayOfRoll[iterIMU] = roll; + ArrayOfPitch[iterIMU] = pitch; + ArrayOfYaw[iterIMU] = yaw; + //pc.printf("%i\t %.3f\t %.3f\t %.3f\n\r",i, roll, pitch, yaw); + iterIMU++; } - else // every 5 ms + else // every 10 ms { ////// print state of gait ///// - //printStateGait(); + 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); + //pc.printf("%.3f\t\t %.3f\t\t %.3f\t\t", roll, pitch, yaw ); + pc.printf("%.3f\t\t", roll); + pc.printf("%.3f\t\t", pitch); + pc.printf("%.3f\t\t", yaw); + + for(iterArr = 0; iterArr < 10 ; iterArr++) + { + ArrRoll[iterArr] = ArrayOfRoll[iterArr]; + ArrPitch[iterArr] = ArrayOfPitch[iterArr]; + ArrYaw[iterArr] = ArrayOfYaw[iterArr]; + } + + // the accleration value + 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 + 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 + + //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll)); + //HormoneCon(SDOfRoll); + + if(!IMUWasStable) + { + //////////// roll ////////////// + SDOfRoll = calculateSD(ArrRoll, 10); + //pc.printf("%.3f\t\t", SDOfRoll); + //pc.printf("%.3f\t\t", ArrayOfRoll[9]); + + //////////// pitch /////////////// + SDOfPitch = calculateSD(ArrPitch, 10 ); + //pc.printf("%.3f\t\t", SDOfPitch); + //pc.printf("%.3f\t\t", ArrayOfPitch[9]); + + //////////// yaw /////////////// + SDOfYaw = calculateSD(ArrYaw, 10 ); + //pc.printf("%.3f\n\r", SDOfYaw); + //pc.printf("%.3f\t\t", ArrayOfYaw[9]); + } - // diff roll pitch + if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming + { + FirstOfRoll = calculateMean(ArrRoll, 10); + FirstOfPitch = calculateMean(ArrPitch, 10); + FirstOfYaw = calculateMean(ArrYaw, 10); + pc.printf("FirstOfRoll: %.3f, FirstOfPitch: %.3f, FirstOfYaw: %.3f\n\r", FirstOfRoll, FirstOfPitch, FirstOfYaw); + + state_count_left = 1; + round_count_left = 1; + state_count_right = 1; + round_count_right = 1; + IMUWasStable = true; + pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r"); + } + + ///////// SD of Ax in G2 /////// +/* if(state_count_left == 2 and state_count_right == 2 ) + { + //pc.printf("%d\t %.3f\n\r", iterAxG2, ax * 9.81f ); // convert g to m/s^2 + ArrayOfAx_G2[iterAxG2++] = ax * 9.81f; + state_count_left_old = state_count_left; + state_count_right_old = state_count_right; + } + else if(state_count_left_old == 2 and state_count_right_old == 2 and state_count_left == 3 and state_count_right == 3) + { + SDOfAx_G2 = calcSDGait(ArrayOfAx_G2, iterAxG2); + //pc.printf("SD of AxG2 %.3f\n\r", SDOfAx_G2); + iterAxG2 = 0; + memset(ArrayOfAx_G2, 0, sizeof(ArrayOfAx_G2)); + state_count_left_old = 0; + state_count_right_old = 0; + } + + ///// distance form origin ////// if(state_count_left == 4 and state_count_right == 4 ) { DiffOfRP = sqrt( pow(roll,2) + pow(pitch,2) ); - pc.printf("%.3f\n\r", DiffOfRP); + //pc.printf("DiffOfRP %.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("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);*/ - + pc.printf("%f \n\r",up_degree); +*/ // reset iteration - i = 0; + iterIMU = 0; } - - timer1.reset(); // reset timer + getIMUTimer = timer1.read_ms(); } } } @@ -219,13 +266,17 @@ ////////////////////////////////////////////////////////////////// void servo() { - /*Servo1.Enable(1000,20000); + Servo1.Enable(1000,20000); Servo2.Enable(1000,20000); Servo3.Enable(1000,20000); - Servo4.Enable(1000,20000);*/ + Servo4.Enable(1000,20000); - pc.printf("\n\r Servo Start Ja!!! \n\r"); - timerwalk.start(); // start timer counting + Servo1.SetPosition(pos_down_left); + Servo2.SetPosition(pos_up_left); + Servo3.SetPosition(pos_down_right); + Servo4.SetPosition(pos_up_right); + + //pc.printf("\n\r Servo Start Ja!!! \n\r"); while(1) { @@ -235,43 +286,16 @@ 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) + // FIN for walking + if (state_count_left == 5 and round_count_left == round and state_count_right == 5 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; + pc.printf("FIN! \n\r"); + 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() @@ -336,32 +360,6 @@ 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 ////////////////////// @@ -417,14 +415,14 @@ 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; + state_count_left = 5; } /*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) + else if (state_count_left == 5 and round_count_left < round) { round_count_left++; state_count_left = 1; @@ -486,14 +484,14 @@ 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; + state_count_right = 5; } /*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) + else if (state_count_right == 5 and round_count_right < round) { round_count_right = round_count_right+1; state_count_right = 1; @@ -502,6 +500,84 @@ } } +///////////////////////// calcSDGait /////////////////////// +////////////////////////////////////////////////////////////////// +float calcSDGait(float sdDataGait[], int iterGait) +{ + float sum = 0.0, mean, standardDeviation = 0.0; + int i; + + //pc.printf("iterGait = %d \n\r", iterGait); + for(i = 5; i < (iterGait + 1) - 5 ; i++) + { + sum += sdDataGait[i]; + //pc.printf("sum = %.2f \n\r",sum); + } + mean = sum/iterGait; + //pc.printf("sum = %.2f \n\r",sum); + //pc.printf("mean = %.2f \n\r",mean); + + for(i = 5; i < (iterGait + 1) - 5; i++) + { + standardDeviation += pow(sdDataGait[i] - mean, 2); + //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); + } + //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); + return sqrt(standardDeviation / iterGait); +} + +///////////////////////// calculateSD ///////////////////// +////////////////////////////////////////////////////////////////// +float calculateSD(float sdData[], int size) +{ + float sum = 0.0, mean, standardDeviation = 0.0; + int i; + + for(i = 0; i < size ; i++) + { + sum += sdData[i]; + //pc.printf("sum = %.2f \n\r",sum); + } + mean = sum/size; + //pc.printf("mean = %.2f \n\r",mean); + + for(i = 0; i < size; i++) + { + standardDeviation += pow(sdData[i] - mean, 2); + //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); + } + return sqrt(standardDeviation / size); +} + +///////////////////////// calculateMean /////////////////// +////////////////////////////////////////////////////////////////// +float calculateMean(float meanData[], int size) +{ + float sum = 0.0, mean; + int i; + + for(i = 0; i < size ; ++i) + { + sum += meanData[i]; + //pc.printf("sum = %.2f \n\r",sum); + } + mean = sum/size; + //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) + { + initCheck = 1; + return true; + } + return false; +} + ///////////////// Hormone Concentration ///////////////////// ////////////////////////////////////////////////////////////////// float HormoneCon(float SiPreProcess) @@ -528,76 +604,53 @@ return Cg; } -///////////////////////// calculateSD ///////////////////// +///////////////////////// receive_hormone ///////////////// ////////////////////////////////////////////////////////////////// -float calculateSD(float sdData[]) +void receive_hormone() { - float sum = 0.0, mean, standardDeviation = 0.0; - int i; - - for(i = 0; i < 10 ; ++i) + //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) { - 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); + down_degree = 85; + if(up_degree > 75) + { + up_degree = 75; + } } - return sqrt(standardDeviation / 10); } -///////////////////////// calculateMean /////////////////// +///////////////////// Print State Gait ////////////////////// ////////////////////////////////////////////////////////////////// -float calculateMean(float meanData[]) +void printStateGait() { - float sum = 0.0, mean; - int i; - - for(i = 0; i < 10 ; ++i) + if(state_count_left == 1 and state_count_right == 1 and stateGaitOne == 0) { - 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) + 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) { - servoFirstState(); - initCheck++; - } - else if( SDOfRoll < 0.03f and SDOfPitch < 0.03f and SDOfYaw < 0.03f and initCheck == 1) - { - initCheck++; - return 1; + pc.printf("\n\r State Gait 2 \n\r"); + stateGaitOne = 0; stateGaitTwo = 1; stateGaitThree = 0; stateGaitFour = 0; } - 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); - + 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; + } } \ No newline at end of file