turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 2:18835f8732ad
- Parent:
- 1:13164a15fbf6
- Child:
- 3:5e867483469e
diff -r 13164a15fbf6 -r 18835f8732ad main.cpp --- a/main.cpp Thu Apr 19 01:54:55 2018 +0000 +++ b/main.cpp Wed Jun 20 04:59:45 2018 +0000 @@ -1,14 +1,24 @@ +////////////////////////////////////////////////////////////////// +// project: TurtleBot Project // +// code v.: 0.0 // +// board : NUCLEO-F303KB // +// date : 19/6/2018 // +// code by: Worasuchad Haomachai // +// detail : Modify IMU thread // +////////////////////////////////////////////////////////////////// + +///////////////////////// init //////////////////////////////// +////////////////////////////////////////////////////////////////// #include "mbed.h" #include "Servo.h" #include "rtos.h" #include "attitude.h" #include "math.h" -Serial pc(USBTX, USBRX); +Serial pc(USBTX, USBRX); //Serial Port Timer timer1; Timer timerwalk; - Thread thread1; Thread thread2; @@ -17,37 +27,40 @@ Servo Servo3(D9); Servo Servo4(D10); +///////////////////////// prototype func /////////////////////// +////////////////////////////////////////////////////////////////// +void servo(); void IMU(); +void move(); +/////// In Move ////// +void cal_step_down(); +void cal_step_up(); +void servo_Right(); +////////////////////// 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(); +float calculateSD(float data[]); +float HormoneCon(float SiPreProcess); + +///////////////////////// variable /////////////////////////////// +////////////////////////////////////////////////////////////////// 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 Cg = 0.00, CgPrevious = 0.00; 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 round = 10; float waittime = 0.001 ; float pos_down_left = 1400.00; @@ -68,65 +81,78 @@ 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() { +///////////////////////// main //////////////////////////// +////////////////////////////////////////////////////////////////// +int main() +{ + pc.baud(115200); + timer1.start(); // start timer counting + pc.printf("PRESS '1'\n"); + if (pc.getc() == '1') + { + thread2.start(servo); // servo thread start + } + //thread2.start(servo); +} + +///////////////////////// Servo /////////////////////////// +////////////////////////////////////////////////////////////////// +void servo() +{ pc.printf("start\n"); - attitude_setup(); - thread1.start(IMU); + attitude_setup(); // IMU setup + thread1.start(IMU); // IMU thread start //wait(0.01); - timerwalk.start(); // start timer counting - move(); + timerwalk.start(); // start timer counting + move(); // func of driving servo //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"); +///////////////////////// receive_hormone ///////////////// +////////////////////////////////////////////////////////////////// +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);*/ - up_degree = 45.00*(1.00+(0.7*Cg_up)); + //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);*/ - if(down_degree < 85){ + pc.printf("%f\n",Cg); + if(down_degree < 85) + { down_degree = 85; - if(up_degree > 75){ - up_degree = 75; + if(up_degree > 75) + { + up_degree = 75; } - } + }*/ } -void cal_step_down(){ +///////////////////////// 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))); - 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); + 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 + 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){ + } + 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{ + } + else // pos_down_end_right == pos_down_end_left + { step_down_right = stepmin; step_down_left = stepmin; } @@ -139,19 +165,27 @@ pc.printf("step_down_left"); pc.printf("%f\n",step_down_left);*/ } - -void cal_step_up(){ + +///////////////////////// 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)); - 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); + 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){ + } + 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{ + } + else // step_up_right == step_up_left + { step_up_right = stepmin; step_up_left = stepmin; } @@ -162,15 +196,17 @@ pc.printf("step_up_right"); pc.printf("%f\n",step_up_right);; pc.printf("step_up_left"); - pc.printf("%f\n",step_up_left); */ + pc.printf("%f\n",step_up_left);*/ } - -void move(){ + +///////////////////////// move //////////////////////////// +////////////////////////////////////////////////////////////////// +void move() +{ Servo1.Enable(1000,20000); Servo2.Enable(1000,20000); Servo3.Enable(1000,20000); - Servo4.Enable(1000,20000); - + Servo4.Enable(1000,20000); /*pc.printf("Si\t"); pc.printf("%f \t",Si); pc.printf("down\t"); @@ -179,116 +215,148 @@ 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); + while(1) + { + cal_step_down(); // return "step_down_right" and "step_down_left" + cal_step_up(); // return "step_up_right" and "step_up_left" + servo_Right(); // control right leg + + if(state_count_left == 1) // control left lag + { + Servo1.SetPosition(pos_down_left); // pos_down_left = 1400.00 + wait(waittime); // 0.001 ms 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) { + 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); + } + else if(state_count_left == 2) + { + Servo2.SetPosition(pos_up_left); // pos_up_left = 1000.00 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) { + 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) { + } + 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) { + 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) { + } + 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) { + 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) { + } + 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){ + } + 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"); + //pc.printf("Finish \n"); walking_time = timerwalk.read_ms(); - pc.printf("Walking time = %d \n", walking_time); + //pc.printf("Walking time = %d \n", walking_time); break; } } } - + +///////////////////////// servo_Right ///////////////////// +////////////////////////////////////////////////////////////////// void servo_Right() { - if(state_count_right == 1) { + 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) { + 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) { + } + 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) { + 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) { + } + 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) { + 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) { + } + else if(state_count_right == 4) + { Servo4.SetPosition(pos_up_right); - wait(waittime); + 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) { + 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) { + } + 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; @@ -297,17 +365,22 @@ } } -///////////////////////////Control IMU//////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////// -void IMU(){ - pc.printf("roll\t"); + +///////////////////////// IMU ///////////////////////////// +////////////////////////////////////////////////////////////////// +void IMU() +{ + int i; + float SiInput[10]; + float SDOfRoll; +/* 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 + pc.printf("up\n"); */ + while(1) + { + if (timer1.read_us() >= 1000) // read time in 1 ms { attitude_get(); @@ -315,70 +388,80 @@ //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("%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); - - timer1.reset(); // reset timer - Cal_si(); + + ////////////////////////// Signal Pre-Process every 10 ms ////////////////////// + //////////////////////////////////////////////////////////////////////////////// + if(i < 10) + { + SiInput[i] = roll; + //pc.printf("i = %i ,SiInput = %.2f, roll= %.2f\n\r",i, SiInput[i], roll); + i++; + } + else + { + //pc.printf("Roll - Standard Deviation = %.2f \n\r", calculateSD(SiInput)); // every 10 ms + SDOfRoll = calculateSD(SiInput); + //pc.printf("HC: %.3f\n\r", HormoneCon(SDOfRoll)); + + i = 0; + } + + timer1.reset(); // reset timer } } } -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; - } - } +///////////////// 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", CgTemp); + Cg = 1/( 1+exp(-CgTemp) ); // used sigmoid func for calculating Cg which much have value between 0 - 1 + /////////////////////////// + + //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; } -void Avg(){ - sum = sum + roll; - avg = sum/10; - standardDeviation = (roll - avg)*(roll-avg); - sd = sqrt(standardDeviation/10); - if(avg < 0){ - avg = avg*(-1); +///////////////////////// 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); } -} - - - -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; + return sqrt(standardDeviation / 10); } \ No newline at end of file