malin
Dependencies: Servo mbed mbed-rtos
Diff: main.cpp
- Revision:
- 2:68dbcd5277e4
- Parent:
- 1:da79761252bc
- Child:
- 3:64bfd330beb4
--- a/main.cpp Tue Jan 30 11:54:34 2018 +0000 +++ b/main.cpp Sun Apr 01 14:12:15 2018 +0000 @@ -1,68 +1,65 @@ #include "mbed.h" #include "Servo.h" #include "rtos.h" + +Serial pc(USBTX, USBRX); -Servo Servo1(D7); -Servo Servo2(D6); +Servo Servo1(D6); +Servo Servo2(D8); Servo Servo3(D9); -Servo Servo4(D8); -Thread thread; +Servo Servo4(D10); + +void myservoLeft(); -void myservoright_thread() { +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){ - Servo3.Enable(1000,20000); - Servo4.Disable(); - for (int pos = 1400; pos <= 1700; pos += 5) { - Servo3.SetPosition(pos); - wait(0.01); + myservoLeft(); + if(pos_down <= pos_down_end and pos_up == 1000){ + Servo1.SetPosition(pos_down); + pos_down = pos_down+2; } - Servo4.Enable(1000,20000); - Servo3.Disable(); - for (int pos = 1000; pos <= 1700; pos += 5) { - Servo4.SetPosition(pos); - wait(0.01); + else if(pos_down == pos_down_end+2 and pos_up <= 1700){ + Servo2.SetPosition(pos_up); + pos_up = pos_up+2; } - Servo3.Enable(1000,20000); - Servo4.Disable(); - for (int pos = 1700; pos >= 1400; pos -= 5) { - Servo3.SetPosition(pos); - wait(0.01); + else if(pos_down >= 1400 and pos_up == pos_up_end+2){ + Servo1.SetPosition(pos_down); + pos_down = pos_down-2; } - Servo4.Enable(1000,20000); - Servo3.Disable(); - for (int pos = 1700; pos >= 1000; pos -= 5) { - Servo4.SetPosition(pos); - wait(0.01); - } + else if(pos_down == 1398 and pos_up > 1000){ + Servo2.SetPosition(pos_up); + pos_up = pos_up-2; + } } } - -int main() { - while(1){ - Servo1.Enable(1000,20000); - Servo2.Disable(); - thread.start(myservoright_thread); - for (int pos = 1400; pos <= 1700; pos += 5) { - Servo1.SetPosition(pos); - wait(0.01); + +void myservoLeft() +{ + if(pos_down <= pos_down_end and pos_up == 1000){ + Servo3.SetPosition(pos_down); + pos_down = pos_down+2; } - Servo2.Enable(1000,20000); - Servo1.Disable(); - for (int pos = 1000; pos <= 1700; pos += 5) { - Servo2.SetPosition(pos); - wait(0.01); + else if(pos_down == pos_down_end+2 and pos_up <= 1700){ + Servo4.SetPosition(pos_up); + pos_up = pos_up+2; } - Servo1.Enable(1000,20000); - Servo2.Disable(); - for (int pos = 1700; pos >= 1400; pos -= 5) { - Servo1.SetPosition(pos); - wait(0.01); + else if(pos_down >= 1400 and pos_up == pos_up_end+2){ + Servo3.SetPosition(pos_down); + pos_down = pos_down-2; } - Servo2.Enable(1000,20000); - Servo1.Disable(); - for (int pos = 1700; pos >= 1000; pos -= 5) { - Servo2.SetPosition(pos); - wait(0.01); - } - } -} \ No newline at end of file + else if(pos_down == 1398 and pos_up > 1000){ + Servo4.SetPosition(pos_up); + pos_up = pos_up-2; + } +}