turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 5:08334c6a42ca
- Parent:
- 4:ec7e68b84f2b
- Child:
- 6:8ae55e1f7e76
diff -r ec7e68b84f2b -r 08334c6a42ca main.cpp --- a/main.cpp Thu Jun 21 08:12:44 2018 +0000 +++ b/main.cpp Wed Jul 11 05:38:01 2018 +0000 @@ -4,7 +4,7 @@ // board : NUCLEO-F303KB // // date : 19/6/2018 // // code by: Worasuchad Haomachai // -// detail : Modify IMU thread // +// detail : // ////////////////////////////////////////////////////////////////// ///////////////////////// init //////////////////////////////// @@ -31,24 +31,28 @@ ////////////////////////////////////////////////////////////////// void IMU(); void servo(); -/////// In 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 SiPreProcess); -bool directionRobot(float directionMean, float directionSD); +float HormoneCon(float ); +bool checkIMUFirst(float , float , float ); + +////// debug func ////// +void printStateGait(); ///////////////////////// variable /////////////////////////////// ////////////////////////////////////////////////////////////////// // home param int walking_time; -int initDirection = 0; +int initCheck = 0; +float waittime = 0.001 ; +float round = 6; // hormone param float Cg_down = 0.00; @@ -60,10 +64,9 @@ 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 stepmin = 1.5; +// servo left side float pos_down_left = 1400.00; float pos_up_left = 1000.00; float pos_down_end_left; @@ -72,7 +75,7 @@ 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; @@ -82,6 +85,9 @@ 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() @@ -103,8 +109,9 @@ void IMU() { int i; - float ArrayOfRoll[10], ArrayOfYaw[10]; - float SDOfRoll, SDOfYaw, MeanOfYaw, directOfRobot = 0.00; + 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"); @@ -112,7 +119,7 @@ pc.printf("up\n"); */ while(1) { - if (timer1.read_us() >= 1000) // read time in 1 ms + if (timer1.read_us() >= 500) // read time in 0.5 ms { attitude_get(); @@ -121,7 +128,6 @@ //pc.printf(" %f \t", az*10-10); //cm/s*s //pc.printf("%f\t %f\t %f\n\r", roll, pitch, yaw); - pc.printf("%f\n\r",yaw); //pc.printf("up\t"); //pc.printf("%f \t",up_degree); //pc.printf("%f\t",Cg); @@ -134,44 +140,72 @@ 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 + 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 ////////////// - //pc.printf("Roll - Standard Deviation = %.2f \n\r", calculateSD(ArrayOfRoll)); // every 10 ms 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); + //HormoneCon(SDOfRoll); + + //////////// pitch /////////////// + SDOfPitch = calculateSD(ArrayOfPitch); + //pc.printf("%.3f\t\t", SDOfPitch); + //pc.printf("%.3f\t\t", pitch); //////////// yaw /////////////// - MeanOfYaw = calculateMean(ArrayOfYaw); - pc.printf("MeanOfYaw: %.3f\n\r", MeanOfYaw); SDOfYaw = calculateSD(ArrayOfYaw); - pc.printf("SDOfYaw: %.3f\n\r", SDOfYaw); + //pc.printf("%.3f\t\t", SDOfYaw); + //pc.printf("%.3f\n\r", yaw); - if(directionRobot(MeanOfYaw, SDOfYaw)) // only one time for comming + if(checkIMUFirst(SDOfRoll, SDOfPitch, SDOfYaw)) // only one time for comming { - directOfRobot = MeanOfYaw; // direction of robot + 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("::::::::::::::::::::::::::: Init Direction OK :::::::::::::::::::::::::::\n\r"); + pc.printf("\n\r:::::::::: IMU was stable ::::::::::\n\r"); } - pc.printf("directOfRobot: %.3f\n\r", directOfRobot); - - if(directOfRobot > SDOfYaw) - { - pc.printf("Turn Left: %.3f\n\r", (directOfRobot - MeanOfYaw)); - } - else - { - pc.printf("Turn Right: %.3f\n\r", (MeanOfYaw - directOfRobot)); - } - + + //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; } @@ -185,21 +219,14 @@ ////////////////////////////////////////////////////////////////// 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("Servo Start Ja!!!\n"); + pc.printf("\n\r Servo Start Ja!!! \n\r"); timerwalk.start(); // start timer counting - /*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) { receive_hormone(); @@ -225,15 +252,16 @@ void receive_hormone() { //down_degree = 90.00f*(1.00f-(0.06f*Cg_down)); - down_degree = 83.50; - /*pc.printf("down\t"); - pc.printf("%f \t",down_degree); - pc.printf("%f\t",Cg);*/ + 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 \t",up_degree); - pc.printf("%f\n",Cg); + /*pc.printf("up: \t"); + pc.printf("%f \n\r",up_degree);*/ + //pc.printf("%f\n",Cg); if(down_degree < 85) { down_degree = 85; @@ -241,7 +269,7 @@ { up_degree = 75; } - }*/ + } } ///////////////////////// cal_step_down /////////////////// @@ -251,7 +279,7 @@ //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 = (1070.00 + ((700.00/90.00)*(down_degree))); // so both pos_down_end_left and pos_down_end_right are around 1700 + 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 @@ -310,6 +338,32 @@ 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() @@ -459,8 +513,9 @@ ////// hormone gland ////// CgTemp = (0.8f*SiPreProcess) + (0.5f*CgPrevious); - //pc.printf("CgTemp: %.3f\t", CgTemp); + //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)); @@ -513,14 +568,36 @@ return mean; } -///////////////////////// directionRobot ////////////////// +///////////////////////// check IMU first ////////////////// ////////////////////////////////////////////////////////////////// -bool directionRobot(float directionMean, float directionSD) +bool checkIMUFirst(float SDOfRoll, float SDOfPitch, float SDOfYaw) { - if( directionSD < 0.06f and initDirection == 0 ) + 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) { - initDirection++; - return 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); + } \ No newline at end of file