TurtleBot / Mbed 2 deprecated Servo_without_thread

Servo code v.1

Dependencies:   Servo mbed

main.cpp@0:3af6fae90816, 2018-04-01 (annotated)

Committer:
Khanchana
Date:
Sun Apr 01 14:20:53 2018 +0000
Revision:
0:3af6fae90816
Child:
1:852156b5cca1
servo code v.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Khanchana 0:3af6fae90816 1 #include "mbed.h"
Khanchana 0:3af6fae90816 2 #include "Servo.h"
Khanchana 0:3af6fae90816 3 #include "rtos.h"
Khanchana 0:3af6fae90816 4
Khanchana 0:3af6fae90816 5 Serial pc(USBTX, USBRX);
Khanchana 0:3af6fae90816 6
Khanchana 0:3af6fae90816 7 Servo Servo1(D6);
Khanchana 0:3af6fae90816 8 Servo Servo2(D8);
Khanchana 0:3af6fae90816 9 Servo Servo3(D9);
Khanchana 0:3af6fae90816 10 Servo Servo4(D10);
Khanchana 0:3af6fae90816 11
Khanchana 0:3af6fae90816 12 void myservoLeft();
Khanchana 0:3af6fae90816 13
Khanchana 0:3af6fae90816 14 int pos_down = 1400;
Khanchana 0:3af6fae90816 15 int pos_up = 1000;
Khanchana 0:3af6fae90816 16 int pos_down_end = 1700;
Khanchana 0:3af6fae90816 17 int pos_up_end = 1700;
Khanchana 0:3af6fae90816 18
Khanchana 0:3af6fae90816 19 int main() {
Khanchana 0:3af6fae90816 20 pc.baud(9600);
Khanchana 0:3af6fae90816 21 wait(5);
Khanchana 0:3af6fae90816 22 Servo1.Enable(1000,2000);
Khanchana 0:3af6fae90816 23 Servo2.Enable(1000,2000);
Khanchana 0:3af6fae90816 24 Servo3.Enable(1000,2000);
Khanchana 0:3af6fae90816 25 Servo4.Enable(1000,2000);
Khanchana 0:3af6fae90816 26 while(1){
Khanchana 0:3af6fae90816 27 myservoLeft();
Khanchana 0:3af6fae90816 28 if(pos_down <= pos_down_end and pos_up == 1000){
Khanchana 0:3af6fae90816 29 Servo1.SetPosition(pos_down);
Khanchana 0:3af6fae90816 30 pos_down = pos_down+2;
Khanchana 0:3af6fae90816 31 }
Khanchana 0:3af6fae90816 32 else if(pos_down == pos_down_end+2 and pos_up <= 1700){
Khanchana 0:3af6fae90816 33 Servo2.SetPosition(pos_up);
Khanchana 0:3af6fae90816 34 pos_up = pos_up+2;
Khanchana 0:3af6fae90816 35 }
Khanchana 0:3af6fae90816 36 else if(pos_down >= 1400 and pos_up == pos_up_end+2){
Khanchana 0:3af6fae90816 37 Servo1.SetPosition(pos_down);
Khanchana 0:3af6fae90816 38 pos_down = pos_down-2;
Khanchana 0:3af6fae90816 39 }
Khanchana 0:3af6fae90816 40 else if(pos_down == 1398 and pos_up > 1000){
Khanchana 0:3af6fae90816 41 Servo2.SetPosition(pos_up);
Khanchana 0:3af6fae90816 42 pos_up = pos_up-2;
Khanchana 0:3af6fae90816 43 }
Khanchana 0:3af6fae90816 44 }
Khanchana 0:3af6fae90816 45 }
Khanchana 0:3af6fae90816 46
Khanchana 0:3af6fae90816 47 void myservoLeft()
Khanchana 0:3af6fae90816 48 {
Khanchana 0:3af6fae90816 49 if(pos_down <= pos_down_end and pos_up == 1000){
Khanchana 0:3af6fae90816 50 Servo3.SetPosition(pos_down);
Khanchana 0:3af6fae90816 51 pos_down = pos_down+2;
Khanchana 0:3af6fae90816 52 }
Khanchana 0:3af6fae90816 53 else if(pos_down == pos_down_end+2 and pos_up <= 1700){
Khanchana 0:3af6fae90816 54 Servo4.SetPosition(pos_up);
Khanchana 0:3af6fae90816 55 pos_up = pos_up+2;
Khanchana 0:3af6fae90816 56 }
Khanchana 0:3af6fae90816 57 else if(pos_down >= 1400 and pos_up == pos_up_end+2){
Khanchana 0:3af6fae90816 58 Servo3.SetPosition(pos_down);
Khanchana 0:3af6fae90816 59 pos_down = pos_down-2;
Khanchana 0:3af6fae90816 60 }
Khanchana 0:3af6fae90816 61 else if(pos_down == 1398 and pos_up > 1000){
Khanchana 0:3af6fae90816 62 Servo4.SetPosition(pos_up);
Khanchana 0:3af6fae90816 63 pos_up = pos_up-2;
Khanchana 0:3af6fae90816 64 }
Khanchana 0:3af6fae90816 65 }