![](/media/cache/group/GF5017-1-1.jpg.50x50_q85.jpg)
turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
main.cpp
- Committer:
- worasuchad
- Date:
- 2018-07-20
- Revision:
- 6:8ae55e1f7e76
- Parent:
- 5:08334c6a42ca
- Child:
- 7:ca887fdc5388
File content as of revision 6:8ae55e1f7e76:
////////////////////////////////////////////////////////////////// // project: TurtleBot Project // // code v.: 0.3 // // board : NUCLEO-F303KB // // date : 19/7/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; Thread thread1; Thread thread2; 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 /////////////////////// ////////////////////////////////////////////////////////////////// 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 [], int ); float calcSDGait(float [], int ); float calculateMean(float [], int ); float HormoneCon(float ); bool checkIMUFirst(float , float, float ); ////// debug func ////// void printStateGait(); ///////////////////////// variable /////////////////////////////// ////////////////////////////////////////////////////////////////// // global variable int initCheck = 0; float waittime = 0.001 ; float round = 6; float ArrRoll[10], ArrPitch[10], ArrYaw[10]; // hormone variable float Cg_down = 0.00; float Cg_up = 0; float Cg = 0.00, CgPrevious = 0.00; // servo motor variable 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 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 step_up_right; float step_down_right; int state_count_right = 0; int round_count_right = 0; // 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); 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 } } ///////////////////////// IMU ///////////////////////////// ////////////////////////////////////////////////////////////////// void IMU() { 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"); pc.printf("down\t"); pc.printf("up\n"); */ while(1) { if ((timer1.read_ms() - getIMUTimer) >= 1) // read time in 1 ms { attitude_get(); //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(iterIMU < 10) { 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 10 ms { ////// print state of gait ///// printStateGait(); //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("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]); } 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("DiffOfRP %.3f\n\r", DiffOfRP); DiffOfRP = 0.0f; } */ //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 iterIMU = 0; } getIMUTimer = timer1.read_ms(); } } } ///////////////////////// servo /////////////////////////// ////////////////////////////////////////////////////////////////// void servo() { Servo1.Enable(1000,20000); Servo2.Enable(1000,20000); Servo3.Enable(1000,20000); Servo4.Enable(1000,20000); 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) { 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 == 5 and round_count_left == round and state_count_right == 5 and round_count_right == round) { thread1.terminate(); pc.printf("FIN! \n\r"); break; } } } ///////////////////////// 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);*/ } ///////////////////////// 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 = 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 == 5 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 = 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 == 5 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; } } ///////////////////////// 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) { 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; } ///////////////////////// 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; } } } ///////////////////// 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; } }