latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V555 by
main.cpp
- Committer:
- 59010050
- Date:
- 2018-04-04
- Revision:
- 3:98ef5105926e
- Parent:
- 2:436ed0069b61
- Child:
- 4:6c8b844d291f
File content as of revision 3:98ef5105926e:
#include "mbed.h" #include "Servo.h" #include "rtos.h" #include "attitude.h" Serial pc(USBTX, USBRX); //Serial bt(A7,A2); Timer timer1; Thread thread1; Thread thread2; void IMU() { while(1) { if (timer1.read_us() >=1000)// read time in ms { attitude_get(); //pc.printf("IMU \n"); 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", gx ); pc.printf(" %f \t", gy ); pc.printf(" %f \t", gz ); //deg/s */ pc.printf("%.0f\t %.0f \t %.0f \n\r", roll, pitch, yaw); timer1.reset(); // reset timer } } } Servo Servo1(D6); Servo Servo2(D8); Servo Servo3(D9); Servo Servo4(D10); int value; void servo_Right(); void move(); void cal_step_down(); void cal_step_up(); void servo(); void getvalue(); float pos_down_start = 1400.00; float pos_up_start = 1000.00; float down_degree = 80.00000000; float up_degree = 15.00000000 ; float stepmin = 1; float round = 10; float waittime = 0.001 ; float pos_down_left = 1400.00; float pos_up_left = 1000.00; float pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); //left down //90 ใน80 นอก(45)+7 ใน85 นอก+5 float pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); //left up// 15 , 30+10 , 45-1.75 , 60-5 , 75-5 , 45+5 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 = (1070.00 + ((700.00/90.00)* (down_degree))) ; //right down //99 float pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); //right up// 15 , 30-10 , 45+1.75 , 60+5 , 75+5 , 45-5 float state_count_right = 1; float round_count_right = 1; float step_up_right; float step_down_right; int main() { pc.baud(1000000); //pc.printf("malin"); //getvalue(); timer1.start(); // start timer counting if (pc.getc() == '1') { pc.printf("ma"); thread2.start(servo); thread1.start(IMU); } } void cal_step_down(){ 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(){ 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) { 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){ pc.printf("Finish"); thread1.terminate(); 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; } } void servo() { attitude_setup(); pc.printf("start\n"); cal_step_down(); cal_step_up(); move(); } void getvalue() { pc.printf("case 1 = 90-15 \n"); pc.printf("case 2 = 90-30 \n"); pc.printf("case 3 = 90-45 \n"); pc.printf("case 4 = 90-60 \n"); pc.printf("case 5 = 80-45 \n"); pc.printf("case 6 = 85-45 \n"); pc.printf("case 7 = 95-45 \n"); pc.printf("case 8 = 100-45 \n"); value = pc.getc() ; switch (value) { case '1': { down_degree = 90.00 ; up_degree = 15.00 ; break; } case '2': { down_degree = 90.00 ; up_degree = 30.00 ; break; } case '3': { down_degree = 90.00 ; up_degree = 45.00 ; break; } case '4': { down_degree = 90.00 ; up_degree = 60.00 ; break; } case '5': { down_degree = 80.00 ; up_degree = 45.00 ; break; } case '6': { down_degree = 85.00 ; up_degree = 45.00 ; break; } case '7': { down_degree = 95.00 ; up_degree = 45.00 ; break; } case '8': { down_degree = 100.00 ; up_degree = 45.00 ; break; } break; } pc.printf("%f \n",down_degree ); pc.printf("%f \n",up_degree ); }