![](/media/cache/group/GF5017-1-1.jpg.50x50_q85.jpg)
malin
Dependencies: Servo mbed mbed-rtos
Revision 4:d00236029c9d, committed 2018-04-02
- Comitter:
- Khanchana
- Date:
- Mon Apr 02 16:41:17 2018 +0000
- Parent:
- 3:64bfd330beb4
- Commit message:
- SERVOOOO
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Apr 02 11:37:43 2018 +0000 +++ b/main.cpp Mon Apr 02 16:41:17 2018 +0000 @@ -5,7 +5,7 @@ Serial pc(USBTX, USBRX); Thread thread; - + Servo Servo1(D6); Servo Servo2(D8); Servo Servo3(D9); @@ -13,151 +13,197 @@ void servo_Right(); void move(); +void cal_step_down(); +void cal_step_up(); -float pos_down = 1400; -float pos_up = 1000; float pos_down_start = 1400; float pos_up_start = 1000; -float pos_down_end = 1544.44; -float pos_up_end = 1400; -float state_count = 1; -float round_count = 1; -float step = 5; +float stepmin = 5; +float round = 5; + +float pos_down_left = 1400; +float pos_up_left = 1000; +float pos_down_end_left = 1700; //left down +float pos_up_end_left = 1350; //left up +float state_count_left = 1; +float round_count_left = 1; +float step_down_left; +float step_up_left; + float pos_down_right = 1400; float pos_up_right = 1000; -float pos_down_start_right = 1400; -float pos_up_start_right = 1000; -float pos_down_end_right = 1544.44; -float pos_up_end_right = 1500; +float pos_down_end_right = 1650; //right down +float pos_up_end_right = 1700; //right up float state_count_right = 1; float round_count_right = 1; -float step_right = 6.25; +float step_up_right; +float step_down_right; -int main(){ - while(1){ - wait(5); - move(); - } +int main() +{ + wait(5); + cal_step_down(); + cal_step_up(); + move(); } -void move() { +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(){ pc.baud(9600); Servo1.Enable(1000,2000); Servo2.Enable(1000,2000); Servo3.Enable(1000,2000); Servo4.Enable(1000,2000); - while(1){ + pc.printf("Start\n"); + while(1) { servo_Right(); - if(state_count == 1){ - Servo1.SetPosition(pos_down); - //wait(0.001); - pos_down = pos_down + step; - if(pos_down >= pos_down_end + step and pos_up == pos_up_start){ - state_count = 2; + if(state_count_left == 1) { + Servo1.SetPosition(pos_down_left); + wait(0.005); + 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); + /*pc.printf("LAD"); + pc.printf("%f\n",pos_down_left); pc.printf("LAP"); - pc.printf("%f\n",pos_up); - } - else if(state_count == 2){ - Servo2.SetPosition(pos_up); - //wait(0.001); - pos_up = pos_up + step; - if(pos_down >= pos_down_end + step and pos_up >= pos_up_end + step){ - state_count = 3; + pc.printf("%f\n",pos_up_left);*/ + } else if(state_count_left == 2) { + Servo2.SetPosition(pos_up_left); + wait(0.005); + 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(0.005); + 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("LBD"); - pc.printf("%f\n",pos_down); - pc.printf("LBP"); - pc.printf("%f\n",pos_up); + /*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(0.005); + 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"); + break; } - else if(state_count == 3){ - Servo1.SetPosition(pos_down); - //wait(0.001); - pos_down = pos_down - step; - if(pos_down <= pos_down_start - step and pos_up >= pos_up_end + step){ - state_count = 4; - } - pc.printf("LCD"); - pc.printf("%f\n",pos_down); - pc.printf("LCP"); - pc.printf("%f\n",pos_up); - } - else if(state_count == 4){ - Servo2.SetPosition(pos_up); - //wait(0.001); - pos_up = pos_up - step; - if(pos_down <= pos_down_start - step and pos_up <= pos_up_start - step){ - state_count = 0; - } - pc.printf("LDD"); - pc.printf("%f\n",pos_down); - pc.printf("LDP"); - pc.printf("%f\n",pos_up); - } - else if (state_count == 0 and round_count < 1){ - round_count = round_count+1; - state_count = 1; - pos_down = 1400; - pos_up = 1000; - } } } -void servo_Right(){ - if(state_count_right == 1){ - Servo3.SetPosition(pos_down_right); - //wait(0.001); - pos_down_right = pos_down_right + step; - if(pos_down_right >= pos_down_end_right + step 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); +void servo_Right() +{ + if(state_count_right == 1) { + Servo3.SetPosition(pos_down_right); + wait(0.005); + 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; } - else if(state_count_right == 2){ - Servo4.SetPosition(pos_up_right); - //wait(0.001); - pos_up_right = pos_up_right + step_right; - if(pos_down_right >= pos_down_end_right + step and pos_up_right >= pos_up_end_right + step_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); + /*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(0.005); + 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; } - else if(state_count_right == 3){ - Servo3.SetPosition(pos_down_right); - //wait(0.001); - pos_down_right = pos_down_right - step; - if(pos_down_right <= pos_down_start_right - step and pos_up_right >= pos_up_end_right + step_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); + /*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(0.005); + 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_right == 4){ - Servo4.SetPosition(pos_up_right); - //wait(0.001); - pos_up_right = pos_up_right - step_right; - if(pos_down_right <= pos_down_start_right - step and pos_up_right <= pos_up_start_right - step_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); + /*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(0.005); + 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_right == 0 and round_count_right < 1){ - round_count_right = round_count_right+1; - state_count_right = 1; - pos_down_right = 1400; - pos_up_right = 1000; - } + /*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