malin
Dependencies: Servo mbed mbed-rtos
Diff: main.cpp
- Revision:
- 3:64bfd330beb4
- Parent:
- 2:68dbcd5277e4
- Child:
- 4:d00236029c9d
diff -r 68dbcd5277e4 -r 64bfd330beb4 main.cpp --- a/main.cpp Sun Apr 01 14:12:15 2018 +0000 +++ b/main.cpp Mon Apr 02 11:37:43 2018 +0000 @@ -3,63 +3,161 @@ #include "rtos.h" Serial pc(USBTX, USBRX); + +Thread thread; Servo Servo1(D6); Servo Servo2(D8); Servo Servo3(D9); Servo Servo4(D10); -void myservoLeft(); +void servo_Right(); +void move(); -int pos_down = 1400; -int pos_up = 1000; -int pos_down_end = 1700; -int pos_up_end = 1700; +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 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 state_count_right = 1; +float round_count_right = 1; +float step_right = 6.25; -int main() { +int main(){ + while(1){ + wait(5); + move(); + } +} + +void move() { pc.baud(9600); - wait(5); Servo1.Enable(1000,2000); Servo2.Enable(1000,2000); Servo3.Enable(1000,2000); Servo4.Enable(1000,2000); while(1){ - myservoLeft(); - if(pos_down <= pos_down_end and pos_up == 1000){ + servo_Right(); + if(state_count == 1){ Servo1.SetPosition(pos_down); - pos_down = pos_down+2; + //wait(0.001); + pos_down = pos_down + step; + if(pos_down >= pos_down_end + step and pos_up == pos_up_start){ + state_count = 2; + } + pc.printf("LAD"); + pc.printf("%f\n",pos_down); + pc.printf("LAP"); + pc.printf("%f\n",pos_up); } - else if(pos_down == pos_down_end+2 and pos_up <= 1700){ + else if(state_count == 2){ Servo2.SetPosition(pos_up); - pos_up = pos_up+2; + //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("LBD"); + pc.printf("%f\n",pos_down); + pc.printf("LBP"); + pc.printf("%f\n",pos_up); } - else if(pos_down >= 1400 and pos_up == pos_up_end+2){ + else if(state_count == 3){ Servo1.SetPosition(pos_down); - pos_down = pos_down-2; + //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(pos_down == 1398 and pos_up > 1000){ + else if(state_count == 4){ Servo2.SetPosition(pos_up); - pos_up = pos_up-2; - } + //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 myservoLeft() -{ - if(pos_down <= pos_down_end and pos_up == 1000){ - Servo3.SetPosition(pos_down); - pos_down = pos_down+2; +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); } - else if(pos_down == pos_down_end+2 and pos_up <= 1700){ - Servo4.SetPosition(pos_up); - pos_up = pos_up+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); } - else if(pos_down >= 1400 and pos_up == pos_up_end+2){ - Servo3.SetPosition(pos_down); - pos_down = pos_down-2; + 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); } - else if(pos_down == 1398 and pos_up > 1000){ - Servo4.SetPosition(pos_up); - pos_up = pos_up-2; - } -} + 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); + } + 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; + } +} \ No newline at end of file