Changing about IMU
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V1 by
Diff: main.cpp
- Revision:
- 0:812929a5d5ad
- Child:
- 1:5609c1795245
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Feb 09 15:59:52 2018 +0000 @@ -0,0 +1,120 @@ +#include "mbed.h" +#include "Servo.h" +#include "rtos.h" + +Serial pc(USBTX, USBRX); + +Servo Servo1(D7); +Servo Servo2(D6); +Servo Servo3(D9); +Servo Servo4(D8); +Thread thread; + +int pos_up_start; +int pos_up_end; +int pos_down_start; +int pos_down_end; + +void myservoright_thread() { + for(int n = 0; n <= 5; n += 1){ + Servo3.Enable(1000,20000); + Servo4.Disable(); + for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) { + Servo3.SetPosition(pos); + wait(0.01); + } + Servo4.Enable(1000,20000); + Servo3.Disable(); + for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) { + Servo4.SetPosition(pos); + wait(0.01); + } + Servo3.Enable(1000,20000); + Servo4.Disable(); + for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) { + Servo3.SetPosition(pos); + wait(0.01); + } + Servo4.Enable(1000,20000); + Servo3.Disable(); + for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) { + Servo4.SetPosition(pos); + wait(0.01); + } + } +} + +void turtlewalk(int pos_down_start,int pos_down_end,int pos_up_start,int pos_up_end){ + for(int n = 0; n <= 5; n += 1){ + Servo1.Enable(1000,20000); + Servo2.Disable(); + thread.start(myservoright_thread); + for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) { + Servo1.SetPosition(pos); + wait(0.01); + } + Servo2.Enable(1000,20000); + Servo1.Disable(); + for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) { + Servo2.SetPosition(pos); + wait(0.01); + } + Servo1.Enable(1000,20000); + Servo2.Disable(); + for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) { + Servo1.SetPosition(pos); + wait(0.01); + } + Servo2.Enable(1000,20000); + Servo1.Disable(); + for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) { + Servo2.SetPosition(pos); + wait(0.01); + } + } +} + +int main(){ + + while(1) { + printf("Hello World! Turtlebot is READY\n"); + printf("case 1-5\n"); + switch(pc.getc()) { + case '1': + pos_down_start = 1400; + pos_down_end = 1700; + pos_up_start = 1000; + pos_up_end = 1700; + break; + case '2': + pos_down_start = 1400; + pos_down_end = 1600; + pos_up_start = 1000; + pos_up_end = 1600; + break; + case '3': + pos_down_start = 1400; + pos_down_end = 1650; + pos_up_start = 1000; + pos_up_end = 1500; + break; + case '4': + pos_down_start = 1400; + pos_down_end = 1700; + pos_up_start = 1000; + pos_up_end = 1650; + break; + case '5': + pos_down_start = 1400; + pos_down_end = 1600; + pos_up_start = 1000; + pos_up_end = 1550; + break; + } + printf("position down motor start = %d\n", pos_down_start); + printf("position down motor end = %d\n", pos_down_end); + printf("position up motor start = %d\n", pos_up_start); + printf("position up motor end = %d\n", pos_up_end); + turtlewalk(pos_down_start,pos_down_end,pos_up_start,pos_up_end); + } +} \ No newline at end of file