![](/media/cache/group/GF5017-1-1.jpg.50x50_q85.jpg)
Servo code v.1
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?
User | Revision | Line number | New 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 | } |