Hormone V.1
Dependencies: Servo mbed-rtos mbed
main.cpp
- Committer:
- Khanchana
- Date:
- 2018-04-19
- Revision:
- 1:13164a15fbf6
- Parent:
- 0:43d21d5145d3
File content as of revision 1:13164a15fbf6:
#include "mbed.h" #include "Servo.h" #include "rtos.h" #include "attitude.h" #include "math.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_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(); 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 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 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.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; } } } 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,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(); 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; } 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(){ 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 { 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); //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(); } } } 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; } } } 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_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; }