20/6/18 experiment wt hormone
Dependencies: Servo TurtleBot_V6 mbed-rtos mbed
Fork of TurtleBot_v00 by
Diff: main.cpp
- Revision:
- 0:43d21d5145d3
- Child:
- 1:13164a15fbf6
diff -r 000000000000 -r 43d21d5145d3 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Apr 10 13:42:04 2018 +0000 @@ -0,0 +1,338 @@ +#include "mbed.h" +#include "Servo.h" +#include "rtos.h" +#include "attitude.h" + +Serial pc(USBTX, USBRX); + +Timer timer1; +Timer timerwalk; + +Thread thread1; +Thread thread2; + +Servo Servo1(D6); +Servo Servo2(D8); +Servo Servo3(D9); +Servo Servo4(D10); + +void IMU(); +void Cal_si(); +void Avg(); +void Cal_Cg(); +void move(); +void receive_hormone(); +void cal_step_down(); //calculate step +void cal_step_up(); +void servo(); +void servo_Right(); + +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 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 waittime = 0.001 ; + +float pos_down_left = 1400.00; +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; + +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 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() { + pc.printf("start\n"); + attitude_setup(); + thread1.start(IMU); + //wait(0.01); + timerwalk.start(); // start timer counting + move(); + //pc.printf("%f\n",roll_min); + //pc.printf("%f\n",roll_max); +} + +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; + if(up_degree > 75){ + up_degree = 75; + } + } +} + +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); + 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{ + step_down_right = stepmin; + step_down_left = stepmin; + } + /*pc.printf("pos_down_right"); + pc.printf("%f\n",pos_down_end_right); + pc.printf("pos_down_left"); + pc.printf("%f\n",pos_down_end_left); + pc.printf("step_down_right"); + pc.printf("%f\n",step_down_right); + pc.printf("step_down_left"); + pc.printf("%f\n",step_down_left);*/ +} + +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); + 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 = stepmin; + step_up_left = stepmin; + } + /*pc.printf("pos_up_right"); + pc.printf("%f\n",pos_up_end_right); + pc.printf("pos_up_left"); + pc.printf("%f\n",pos_up_end_left); + pc.printf("step_up_right"); + pc.printf("%f\n",step_up_right);; + pc.printf("step_up_left"); + pc.printf("%f\n",step_up_left); */ +} + +void move(){ + Servo1.Enable(1000,2000); + Servo2.Enable(1000,2000); + Servo3.Enable(1000,2000); + Servo4.Enable(1000,2000); + while(1) { + cal_step_down(); + cal_step_up(); + servo_Right(); + if(state_count_left == 1) { + Servo1.SetPosition(pos_down_left); + wait(waittime); + 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) { + 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); + 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) { + 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 = 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 = 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; + } + /*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) { + round_count_left = round_count_left+1; + 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"); + walking_time = timerwalk.read_ms(); + pc.printf("Walking time = %d \n", walking_time); + break; + } + } +} + +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 = 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) { + round_count_right = round_count_right+1; + state_count_right = 1; + pos_down_right = pos_down_start; + pos_up_right = pos_up_start; + receive_hormone(); + } +} + +///////////////////////////Control IMU//////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// +void IMU(){ + while(1) { + if (timer1.read_us() >=10000)// read time in 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 %.0f \t %.0f \n\r", roll, pitch, yaw); + + timer1.reset(); // reset timer + Cal_si(); + } + } +} + +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); + 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); + if(i == 9){ + i = 0; + }else{ + i = i+1; + } + } +} + +void Avg(){ + sum = sum + roll; + avg = sum/10; + if(avg < 0){ + avg = avg*(-1); + } +} + +void Cal_Cg(){ + if(Si > 0){ + Cg = (0.9*Si)+(0.3*Cg); + if(Cg > 1){ + Cg = 1; + } + }else{ + Cg = 0.00; + } +} \ No newline at end of file