TurtleBot
/
Servo_without_thread
Servo code v.1
Diff: main.cpp
- Revision:
- 0:3af6fae90816
- Child:
- 1:852156b5cca1
diff -r 000000000000 -r 3af6fae90816 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Apr 01 14:20:53 2018 +0000 @@ -0,0 +1,65 @@ +#include "mbed.h" +#include "Servo.h" +#include "rtos.h" + +Serial pc(USBTX, USBRX); + +Servo Servo1(D6); +Servo Servo2(D8); +Servo Servo3(D9); +Servo Servo4(D10); + +void myservoLeft(); + +int pos_down = 1400; +int pos_up = 1000; +int pos_down_end = 1700; +int pos_up_end = 1700; + +int main() { + 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){ + Servo1.SetPosition(pos_down); + pos_down = pos_down+2; + } + else if(pos_down == pos_down_end+2 and pos_up <= 1700){ + Servo2.SetPosition(pos_up); + pos_up = pos_up+2; + } + else if(pos_down >= 1400 and pos_up == pos_up_end+2){ + Servo1.SetPosition(pos_down); + pos_down = pos_down-2; + } + else if(pos_down == 1398 and pos_up > 1000){ + Servo2.SetPosition(pos_up); + pos_up = pos_up-2; + } + } +} + +void myservoLeft() +{ + if(pos_down <= pos_down_end and pos_up == 1000){ + Servo3.SetPosition(pos_down); + pos_down = pos_down+2; + } + else if(pos_down == pos_down_end+2 and pos_up <= 1700){ + Servo4.SetPosition(pos_up); + pos_up = pos_up+2; + } + else if(pos_down >= 1400 and pos_up == pos_up_end+2){ + Servo3.SetPosition(pos_down); + pos_down = pos_down-2; + } + else if(pos_down == 1398 and pos_up > 1000){ + Servo4.SetPosition(pos_up); + pos_up = pos_up-2; + } +}