turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 1:13164a15fbf6
- Parent:
- 0:43d21d5145d3
- Child:
- 2:18835f8732ad
diff -r 43d21d5145d3 -r 13164a15fbf6 main.cpp --- a/main.cpp Tue Apr 10 13:42:04 2018 +0000 +++ b/main.cpp Thu Apr 19 01:54:55 2018 +0000 @@ -2,6 +2,7 @@ #include "Servo.h" #include "rtos.h" #include "attitude.h" +#include "math.h" Serial pc(USBTX, USBRX); @@ -19,7 +20,8 @@ void IMU(); void Cal_si(); void Avg(); -void Cal_Cg(); +void Cal_Cg_linear(); +void Cal_Cg_log(); void move(); void receive_hormone(); void cal_step_down(); //calculate step @@ -35,13 +37,17 @@ 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 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 = 200; +float round = 5; float waittime = 0.001 ; float pos_down_left = 1400.00; @@ -92,10 +98,17 @@ } void receive_hormone(){ - down_degree = 90.00*(1.00-(0.5*Cg)); - up_degree = 45.00*(1.00+(0.7*Cg)); - if(down_degree < 80){ - down_degree = 80; + down_degree = 90.00*(1.00-(0.06*Cg_down)); + /*pc.printf("%f \t",Si); + 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)); + /*pc.printf("up\t"); + pc.printf("%f \t",up_degree); + pc.printf("%f\n",Cg);*/ + if(down_degree < 85){ + down_degree = 85; if(up_degree > 75){ up_degree = 75; } @@ -153,10 +166,19 @@ } void move(){ - Servo1.Enable(1000,2000); - Servo2.Enable(1000,2000); - Servo3.Enable(1000,2000); - Servo4.Enable(1000,2000); + Servo1.Enable(1000,20000); + Servo2.Enable(1000,20000); + Servo3.Enable(1000,20000); + Servo4.Enable(1000,20000); + + /*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) { cal_step_down(); cal_step_up(); @@ -210,7 +232,6 @@ state_count_left = 1; pos_down_left = pos_down_start; pos_up_left = pos_up_start; - //receive_hormone(); } 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"); @@ -279,6 +300,12 @@ ///////////////////////////Control IMU//////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// void IMU(){ + 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 { @@ -288,7 +315,14 @@ //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 %.0f \t %.0f \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(); @@ -299,17 +333,21 @@ void Cal_si(){ if(state_count_left == 4 or state_count_right == 4){ roll_data[i] = roll; - pc.printf("%f\n",roll_data[i]); + //pc.printf("%f\n",roll_data[i]); Avg(); //pc.printf("Avg "); //pc.printf("%f\n",avg); - Si = avg/4; - sum = sum - roll_data[i]; - //pc.printf("Si "); - //pc.printf("%f\n",Si); - Cal_Cg(); - //pc.printf("Cg "); - //pc.printf("%f\n",Cg); + 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{ @@ -321,18 +359,26 @@ void Avg(){ sum = sum + roll; avg = sum/10; + standardDeviation = (roll - avg)*(roll-avg); + sd = sqrt(standardDeviation/10); if(avg < 0){ avg = avg*(-1); } } -void Cal_Cg(){ + + +void Cal_Cg_linear(){ + Si = sd/0.6; + sum = sum - roll_data[i]; if(Si > 0){ - Cg = (0.9*Si)+(0.3*Cg); + 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; } \ No newline at end of file