latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V555 by
main.cpp
- Committer:
- Khanchana
- Date:
- 2018-04-01
- Revision:
- 0:3af6fae90816
- Child:
- 1:852156b5cca1
File content as of revision 0:3af6fae90816:
#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; } }