malin
Dependencies: Servo mbed mbed-rtos
Diff: main.cpp
- Revision:
- 1:da79761252bc
- Parent:
- 0:596de9a5c573
- Child:
- 2:68dbcd5277e4
diff -r 596de9a5c573 -r da79761252bc main.cpp --- a/main.cpp Tue Jan 30 11:01:49 2018 +0000 +++ b/main.cpp Tue Jan 30 11:54:34 2018 +0000 @@ -1,29 +1,68 @@ #include "mbed.h" #include "Servo.h" +#include "rtos.h" -Servo Servo1(D6); -Servo Servo2(D7); +Servo Servo1(D7); +Servo Servo2(D6); +Servo Servo3(D9); +Servo Servo4(D8); +Thread thread; + +void myservoright_thread() { + while(1){ + Servo3.Enable(1000,20000); + Servo4.Disable(); + for (int pos = 1400; pos <= 1700; pos += 5) { + Servo3.SetPosition(pos); + wait(0.01); + } + Servo4.Enable(1000,20000); + Servo3.Disable(); + for (int pos = 1000; pos <= 1700; pos += 5) { + Servo4.SetPosition(pos); + wait(0.01); + } + Servo3.Enable(1000,20000); + Servo4.Disable(); + for (int pos = 1700; pos >= 1400; pos -= 5) { + Servo3.SetPosition(pos); + wait(0.01); + } + Servo4.Enable(1000,20000); + Servo3.Disable(); + for (int pos = 1700; pos >= 1000; pos -= 5) { + Servo4.SetPosition(pos); + wait(0.01); + } + } +} int main() { + while(1){ Servo1.Enable(1000,20000); - Servo2.Enable(1000,20000); Servo2.Disable(); - for (int pos = 1000; pos < 1500; pos += 25) { + thread.start(myservoright_thread); + for (int pos = 1400; pos <= 1700; pos += 5) { Servo1.SetPosition(pos); - wait(0.05); - } - for (int pos = 1500; pos > 1000; pos -= 25) { - Servo1.SetPosition(pos); - wait(0.05); + wait(0.01); } Servo2.Enable(1000,20000); Servo1.Disable(); - for (int pos = 1000; pos < 1500; pos += 25) { + for (int pos = 1000; pos <= 1700; pos += 5) { Servo2.SetPosition(pos); - wait(0.05); + wait(0.01); } - for (int pos = 1500; pos > 1000; pos -= 25) { + Servo1.Enable(1000,20000); + Servo2.Disable(); + for (int pos = 1700; pos >= 1400; pos -= 5) { + Servo1.SetPosition(pos); + wait(0.01); + } + Servo2.Enable(1000,20000); + Servo1.Disable(); + for (int pos = 1700; pos >= 1000; pos -= 5) { Servo2.SetPosition(pos); - wait(0.05); + wait(0.01); } + } } \ No newline at end of file