![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V55 by
Diff: main.cpp
- Revision:
- 2:436ed0069b61
- Parent:
- 1:852156b5cca1
- Child:
- 3:98ef5105926e
diff -r 852156b5cca1 -r 436ed0069b61 main.cpp --- a/main.cpp Mon Apr 02 10:18:44 2018 +0000 +++ b/main.cpp Mon Apr 02 11:46:08 2018 +0000 @@ -35,76 +35,125 @@ Servo Servo3(D9); Servo Servo4(D10); -void myservoLeft(); +void servoleft(); void move(); int pos_down = 1400; int pos_up = 1000; int pos_down_end = 1700; -int pos_up_end = ; -int final_posdown = 1700 ; -int final_posup = 1466.66; +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; int main() { pc.baud(1000000); - pc.printf("malin"); + //pc.printf("malin"); timer1.start(); // start timer counting if (pc.getc() == '1') { thread2.start(move); - thread1.start(IMU); + //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 move() { attitude_setup(); 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"); + //pc.printf("start"); + //pc.printf("start"); + //pc.printf("start"); pc.printf("start"); while(1){ - + servoleft(); if(state_count == 1){ - Servo1.SetPosition(pos_down); Servo3.SetPosition(pos_down); pos_down = pos_down+5; wait(0.01); - if(pos_down >= pos_down_end+5 and pos_up >= 1000){ + pc.printf("right %d\n",pos_down); + if(pos_down == pos_down_end+5 and pos_up == 1000){ state_count = 2; } } else if(state_count == 2){ - Servo2.SetPosition(pos_up); 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){ + if(pos_down == pos_down_end+5 and pos_up == pos_up_end+5){ state_count = 3; } } else if(state_count == 3){ - Servo1.SetPosition(pos_down); Servo3.SetPosition(pos_down); pos_down = pos_down-5; wait(0.01); - if(pos_down <= 1395 and pos_up >= pos_up_end+5){ + if(pos_down == 1395 and pos_up == pos_up_end+5){ state_count = 4; } } else if(state_count == 4){ - Servo2.SetPosition(pos_up); Servo4.SetPosition(pos_up); pos_up = pos_up-5; wait(0.01); - if(pos_down <= 1395 and pos_up <= 995){ + if(pos_down == 1395 and pos_up == 995){ state_count = 0; } }