latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V555 by
Diff: main.cpp
- Revision:
- 3:98ef5105926e
- Parent:
- 2:436ed0069b61
- Child:
- 4:6c8b844d291f
diff -r 436ed0069b61 -r 98ef5105926e main.cpp --- a/main.cpp Mon Apr 02 11:46:08 2018 +0000 +++ b/main.cpp Wed Apr 04 14:47:57 2018 +0000 @@ -7,7 +7,6 @@ //Serial bt(A7,A2); Timer timer1; Thread thread1; - Thread thread2; void IMU() @@ -16,14 +15,15 @@ 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", 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 \n\r", roll, pitch ); + pc.printf("%.0f\t %.0f \t %.0f \n\r", roll, pitch, yaw); timer1.reset(); // reset timer } @@ -35,136 +35,278 @@ Servo Servo3(D9); Servo Servo4(D10); -void servoleft(); -void move(); +int value; -int pos_down = 1400; -int pos_up = 1000; -int pos_down_end = 1700; -int pos_up_end = 1500; -int edit_up = 1400; - -int state_count = 1; -int state_count2 = 1; -int round_count = 1; -int round_count2 = 1; +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') { - thread2.start(move); - //thread1.start(IMU); - } - + pc.printf("ma"); + thread2.start(servo); + thread1.start(IMU); + } } -void servoleft(){ - - if(state_count2 == 1){ - Servo1.SetPosition(pos_down); - //pos_down = pos_down+5; - wait(0.01); - pc.printf("left %d\n",pos_down); - if(pos_down == pos_down_end+5 and pos_up == 1000){ - state_count2 = 2; - } - } - else if(state_count2 == 2){ - Servo2.SetPosition(pos_up); - //pos_up = pos_up+5; - wait(0.01); - pc.printf("stage2"); - if(pos_down == pos_down_end+5 and pos_up == edit_up+5){ - state_count2 = 3; - } - } - else if(state_count2 == 3){ - Servo1.SetPosition(pos_down); - //pos_down = pos_down-5; - wait(0.01); - pc.printf("stage3"); - if(pos_down == 1395 and pos_up == edit_up+5){ - state_count2 = 4; - } - } - else if(state_count2 == 4){ - Servo2.SetPosition(pos_up); - //pos_up = pos_up-5; - wait(0.01); - pc.printf("stage4"); - if(pos_down == 1395 and pos_up == 995){ - state_count2 = 0; - } - } - else if (state_count2 == 0 and round_count2 < 10){ - round_count2 = round_count2+1; - state_count2 = 1; - pos_down = 1400; - pos_up = 1000; - } - else { - pc.printf("1stop"); - //thread1.stop(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; } -void move() { - attitude_setup(); + /*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); - //pc.printf("start"); - //pc.printf("start"); - //pc.printf("start"); - //pc.printf("start"); - pc.printf("start"); - while(1){ - servoleft(); - if(state_count == 1){ - Servo3.SetPosition(pos_down); - pos_down = pos_down+5; - wait(0.01); - pc.printf("right %d\n",pos_down); - if(pos_down == pos_down_end+5 and pos_up == 1000){ - state_count = 2; + 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; } - else if(state_count == 2){ - Servo4.SetPosition(pos_up); - pos_up = pos_up+5; - wait(0.01); - if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){ - state_count = 3; - } + } +} + +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; } - else if(state_count == 3){ - Servo3.SetPosition(pos_down); - pos_down = pos_down-5; - wait(0.01); - if(pos_down == 1395 and pos_up == pos_up_end+5){ - state_count = 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; } - else if(state_count == 4){ - Servo4.SetPosition(pos_up); - pos_up = pos_up-5; - wait(0.01); - if(pos_down == 1395 and pos_up == 995){ - state_count = 0; - } - } - else if (state_count == 0 and round_count < 10){ - round_count = round_count+1; - state_count = 1; - pos_down = 1400; - pos_up = 1000; - } - else { - pc.printf("stop"); - } + /*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; } -} \ No newline at end of file +} + + +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 ); + + } \ No newline at end of file