Changing about IMU
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V1 by
main.cpp@0:812929a5d5ad, 2018-02-09 (annotated)
- Committer:
- Khanchana
- Date:
- Fri Feb 09 15:59:52 2018 +0000
- Revision:
- 0:812929a5d5ad
- Child:
- 1:5609c1795245
???????????? 5 ???
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Khanchana | 0:812929a5d5ad | 1 | #include "mbed.h" |
Khanchana | 0:812929a5d5ad | 2 | #include "Servo.h" |
Khanchana | 0:812929a5d5ad | 3 | #include "rtos.h" |
Khanchana | 0:812929a5d5ad | 4 | |
Khanchana | 0:812929a5d5ad | 5 | Serial pc(USBTX, USBRX); |
Khanchana | 0:812929a5d5ad | 6 | |
Khanchana | 0:812929a5d5ad | 7 | Servo Servo1(D7); |
Khanchana | 0:812929a5d5ad | 8 | Servo Servo2(D6); |
Khanchana | 0:812929a5d5ad | 9 | Servo Servo3(D9); |
Khanchana | 0:812929a5d5ad | 10 | Servo Servo4(D8); |
Khanchana | 0:812929a5d5ad | 11 | Thread thread; |
Khanchana | 0:812929a5d5ad | 12 | |
Khanchana | 0:812929a5d5ad | 13 | int pos_up_start; |
Khanchana | 0:812929a5d5ad | 14 | int pos_up_end; |
Khanchana | 0:812929a5d5ad | 15 | int pos_down_start; |
Khanchana | 0:812929a5d5ad | 16 | int pos_down_end; |
Khanchana | 0:812929a5d5ad | 17 | |
Khanchana | 0:812929a5d5ad | 18 | void myservoright_thread() { |
Khanchana | 0:812929a5d5ad | 19 | for(int n = 0; n <= 5; n += 1){ |
Khanchana | 0:812929a5d5ad | 20 | Servo3.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 21 | Servo4.Disable(); |
Khanchana | 0:812929a5d5ad | 22 | for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) { |
Khanchana | 0:812929a5d5ad | 23 | Servo3.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 24 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 25 | } |
Khanchana | 0:812929a5d5ad | 26 | Servo4.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 27 | Servo3.Disable(); |
Khanchana | 0:812929a5d5ad | 28 | for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) { |
Khanchana | 0:812929a5d5ad | 29 | Servo4.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 30 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 31 | } |
Khanchana | 0:812929a5d5ad | 32 | Servo3.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 33 | Servo4.Disable(); |
Khanchana | 0:812929a5d5ad | 34 | for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) { |
Khanchana | 0:812929a5d5ad | 35 | Servo3.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 36 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 37 | } |
Khanchana | 0:812929a5d5ad | 38 | Servo4.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 39 | Servo3.Disable(); |
Khanchana | 0:812929a5d5ad | 40 | for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) { |
Khanchana | 0:812929a5d5ad | 41 | Servo4.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 42 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 43 | } |
Khanchana | 0:812929a5d5ad | 44 | } |
Khanchana | 0:812929a5d5ad | 45 | } |
Khanchana | 0:812929a5d5ad | 46 | |
Khanchana | 0:812929a5d5ad | 47 | void turtlewalk(int pos_down_start,int pos_down_end,int pos_up_start,int pos_up_end){ |
Khanchana | 0:812929a5d5ad | 48 | for(int n = 0; n <= 5; n += 1){ |
Khanchana | 0:812929a5d5ad | 49 | Servo1.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 50 | Servo2.Disable(); |
Khanchana | 0:812929a5d5ad | 51 | thread.start(myservoright_thread); |
Khanchana | 0:812929a5d5ad | 52 | for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) { |
Khanchana | 0:812929a5d5ad | 53 | Servo1.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 54 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 55 | } |
Khanchana | 0:812929a5d5ad | 56 | Servo2.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 57 | Servo1.Disable(); |
Khanchana | 0:812929a5d5ad | 58 | for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) { |
Khanchana | 0:812929a5d5ad | 59 | Servo2.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 60 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 61 | } |
Khanchana | 0:812929a5d5ad | 62 | Servo1.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 63 | Servo2.Disable(); |
Khanchana | 0:812929a5d5ad | 64 | for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) { |
Khanchana | 0:812929a5d5ad | 65 | Servo1.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 66 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 67 | } |
Khanchana | 0:812929a5d5ad | 68 | Servo2.Enable(1000,20000); |
Khanchana | 0:812929a5d5ad | 69 | Servo1.Disable(); |
Khanchana | 0:812929a5d5ad | 70 | for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) { |
Khanchana | 0:812929a5d5ad | 71 | Servo2.SetPosition(pos); |
Khanchana | 0:812929a5d5ad | 72 | wait(0.01); |
Khanchana | 0:812929a5d5ad | 73 | } |
Khanchana | 0:812929a5d5ad | 74 | } |
Khanchana | 0:812929a5d5ad | 75 | } |
Khanchana | 0:812929a5d5ad | 76 | |
Khanchana | 0:812929a5d5ad | 77 | int main(){ |
Khanchana | 0:812929a5d5ad | 78 | |
Khanchana | 0:812929a5d5ad | 79 | while(1) { |
Khanchana | 0:812929a5d5ad | 80 | printf("Hello World! Turtlebot is READY\n"); |
Khanchana | 0:812929a5d5ad | 81 | printf("case 1-5\n"); |
Khanchana | 0:812929a5d5ad | 82 | switch(pc.getc()) { |
Khanchana | 0:812929a5d5ad | 83 | case '1': |
Khanchana | 0:812929a5d5ad | 84 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 85 | pos_down_end = 1700; |
Khanchana | 0:812929a5d5ad | 86 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 87 | pos_up_end = 1700; |
Khanchana | 0:812929a5d5ad | 88 | break; |
Khanchana | 0:812929a5d5ad | 89 | case '2': |
Khanchana | 0:812929a5d5ad | 90 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 91 | pos_down_end = 1600; |
Khanchana | 0:812929a5d5ad | 92 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 93 | pos_up_end = 1600; |
Khanchana | 0:812929a5d5ad | 94 | break; |
Khanchana | 0:812929a5d5ad | 95 | case '3': |
Khanchana | 0:812929a5d5ad | 96 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 97 | pos_down_end = 1650; |
Khanchana | 0:812929a5d5ad | 98 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 99 | pos_up_end = 1500; |
Khanchana | 0:812929a5d5ad | 100 | break; |
Khanchana | 0:812929a5d5ad | 101 | case '4': |
Khanchana | 0:812929a5d5ad | 102 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 103 | pos_down_end = 1700; |
Khanchana | 0:812929a5d5ad | 104 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 105 | pos_up_end = 1650; |
Khanchana | 0:812929a5d5ad | 106 | break; |
Khanchana | 0:812929a5d5ad | 107 | case '5': |
Khanchana | 0:812929a5d5ad | 108 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 109 | pos_down_end = 1600; |
Khanchana | 0:812929a5d5ad | 110 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 111 | pos_up_end = 1550; |
Khanchana | 0:812929a5d5ad | 112 | break; |
Khanchana | 0:812929a5d5ad | 113 | } |
Khanchana | 0:812929a5d5ad | 114 | printf("position down motor start = %d\n", pos_down_start); |
Khanchana | 0:812929a5d5ad | 115 | printf("position down motor end = %d\n", pos_down_end); |
Khanchana | 0:812929a5d5ad | 116 | printf("position up motor start = %d\n", pos_up_start); |
Khanchana | 0:812929a5d5ad | 117 | printf("position up motor end = %d\n", pos_up_end); |
Khanchana | 0:812929a5d5ad | 118 | turtlewalk(pos_down_start,pos_down_end,pos_up_start,pos_up_end); |
Khanchana | 0:812929a5d5ad | 119 | } |
Khanchana | 0:812929a5d5ad | 120 | } |