Changing about IMU
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V1 by
main.cpp@3:5bfa7291c639, 2018-03-23 (annotated)
- Committer:
- worasuchad
- Date:
- Fri Mar 23 18:32:18 2018 +0000
- Revision:
- 3:5bfa7291c639
- Parent:
- 2:d4bd9ff10e8e
edit and test IMU
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
worasuchad | 1:5609c1795245 | 1 | ////////////////////////////////////////////////////////////////// |
worasuchad | 1:5609c1795245 | 2 | // project: TurtleBot Project // |
worasuchad | 1:5609c1795245 | 3 | // code v.: 1.0 // |
worasuchad | 1:5609c1795245 | 4 | // board : NUCLEO-F303KB // |
worasuchad | 1:5609c1795245 | 5 | // date : 20/2/2018 // |
worasuchad | 1:5609c1795245 | 6 | // code by: Coding on Earth by Humans // |
worasuchad | 1:5609c1795245 | 7 | ////////////////////////////////////////////////////////////////// |
worasuchad | 1:5609c1795245 | 8 | |
worasuchad | 1:5609c1795245 | 9 | ///////////////////////// init //////////////////////////////// |
worasuchad | 1:5609c1795245 | 10 | ////////////////////////////////////////////////////////////////// |
Khanchana | 0:812929a5d5ad | 11 | #include "mbed.h" |
Khanchana | 0:812929a5d5ad | 12 | #include "rtos.h" |
Khanchana | 0:812929a5d5ad | 13 | |
Khanchana | 0:812929a5d5ad | 14 | Serial pc(USBTX, USBRX); |
worasuchad | 1:5609c1795245 | 15 | |
worasuchad | 1:5609c1795245 | 16 | Thread thread1; //control servo left |
worasuchad | 1:5609c1795245 | 17 | Thread thread2; //control servo right |
worasuchad | 1:5609c1795245 | 18 | Thread thread3; //read data from IMU |
worasuchad | 1:5609c1795245 | 19 | |
worasuchad | 1:5609c1795245 | 20 | ///////////////////////// IMU //////////////////////////////// |
worasuchad | 1:5609c1795245 | 21 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5bfa7291c639 | 22 | #include "attitude.h" |
worasuchad | 3:5bfa7291c639 | 23 | /* |
worasuchad | 1:5609c1795245 | 24 | #include "MPU9250.h" |
worasuchad | 1:5609c1795245 | 25 | |
worasuchad | 1:5609c1795245 | 26 | float sum = 0; |
worasuchad | 1:5609c1795245 | 27 | uint32_t sumCount = 0; |
worasuchad | 1:5609c1795245 | 28 | char buffer[14]; |
worasuchad | 1:5609c1795245 | 29 | float origin = 0; |
worasuchad | 1:5609c1795245 | 30 | |
worasuchad | 1:5609c1795245 | 31 | MPU9250 mpu9250; |
worasuchad | 1:5609c1795245 | 32 | Timer t; |
worasuchad | 3:5bfa7291c639 | 33 | */ |
worasuchad | 1:5609c1795245 | 34 | |
worasuchad | 1:5609c1795245 | 35 | ///////////////////////// Servo //////////////////////////////// |
worasuchad | 1:5609c1795245 | 36 | ////////////////////////////////////////////////////////////////// |
worasuchad | 1:5609c1795245 | 37 | #include "Servo.h" |
worasuchad | 1:5609c1795245 | 38 | Servo Servo1(D10); |
Khanchana | 0:812929a5d5ad | 39 | Servo Servo2(D6); |
worasuchad | 1:5609c1795245 | 40 | Servo Servo3(D8); |
worasuchad | 1:5609c1795245 | 41 | Servo Servo4(D9); |
worasuchad | 1:5609c1795245 | 42 | /* |
Khanchana | 0:812929a5d5ad | 43 | int pos_up_start; |
Khanchana | 0:812929a5d5ad | 44 | int pos_up_end; |
Khanchana | 0:812929a5d5ad | 45 | int pos_down_start; |
worasuchad | 1:5609c1795245 | 46 | int pos_down_end;*/ |
Khanchana | 0:812929a5d5ad | 47 | |
worasuchad | 1:5609c1795245 | 48 | int pos_down_start = 1400; |
worasuchad | 1:5609c1795245 | 49 | int pos_down_end = 1600; |
worasuchad | 1:5609c1795245 | 50 | int pos_up_start = 1000; |
worasuchad | 1:5609c1795245 | 51 | int pos_up_end = 1600; |
Khanchana | 0:812929a5d5ad | 52 | |
worasuchad | 1:5609c1795245 | 53 | ///////////////////////// prototype func /////////////////////// |
worasuchad | 1:5609c1795245 | 54 | ////////////////////////////////////////////////////////////////// |
worasuchad | 1:5609c1795245 | 55 | void myservoLeft(); |
worasuchad | 1:5609c1795245 | 56 | void myservoRight(); |
worasuchad | 1:5609c1795245 | 57 | void IMU(); |
Khanchana | 0:812929a5d5ad | 58 | |
worasuchad | 1:5609c1795245 | 59 | ///////////////////////// main //////////////////////////// |
worasuchad | 1:5609c1795245 | 60 | ////////////////////////////////////////////////////////////////// |
worasuchad | 1:5609c1795245 | 61 | int main() |
worasuchad | 1:5609c1795245 | 62 | { |
worasuchad | 3:5bfa7291c639 | 63 | |
worasuchad | 3:5bfa7291c639 | 64 | IMU(); |
worasuchad | 1:5609c1795245 | 65 | thread1.start(myservoLeft); |
worasuchad | 1:5609c1795245 | 66 | thread2.start(myservoRight); |
worasuchad | 1:5609c1795245 | 67 | /* while(1) |
worasuchad | 1:5609c1795245 | 68 | { |
Khanchana | 0:812929a5d5ad | 69 | printf("Hello World! Turtlebot is READY\n"); |
Khanchana | 0:812929a5d5ad | 70 | printf("case 1-5\n"); |
worasuchad | 1:5609c1795245 | 71 | switch(pc.getc()) |
worasuchad | 1:5609c1795245 | 72 | { |
Khanchana | 0:812929a5d5ad | 73 | case '1': |
Khanchana | 0:812929a5d5ad | 74 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 75 | pos_down_end = 1700; |
Khanchana | 0:812929a5d5ad | 76 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 77 | pos_up_end = 1700; |
Khanchana | 0:812929a5d5ad | 78 | break; |
Khanchana | 0:812929a5d5ad | 79 | case '2': |
Khanchana | 0:812929a5d5ad | 80 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 81 | pos_down_end = 1600; |
Khanchana | 0:812929a5d5ad | 82 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 83 | pos_up_end = 1600; |
Khanchana | 0:812929a5d5ad | 84 | break; |
Khanchana | 0:812929a5d5ad | 85 | case '3': |
Khanchana | 0:812929a5d5ad | 86 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 87 | pos_down_end = 1650; |
Khanchana | 0:812929a5d5ad | 88 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 89 | pos_up_end = 1500; |
Khanchana | 0:812929a5d5ad | 90 | break; |
Khanchana | 0:812929a5d5ad | 91 | case '4': |
Khanchana | 0:812929a5d5ad | 92 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 93 | pos_down_end = 1700; |
Khanchana | 0:812929a5d5ad | 94 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 95 | pos_up_end = 1650; |
Khanchana | 0:812929a5d5ad | 96 | break; |
Khanchana | 0:812929a5d5ad | 97 | case '5': |
Khanchana | 0:812929a5d5ad | 98 | pos_down_start = 1400; |
Khanchana | 0:812929a5d5ad | 99 | pos_down_end = 1600; |
Khanchana | 0:812929a5d5ad | 100 | pos_up_start = 1000; |
Khanchana | 0:812929a5d5ad | 101 | pos_up_end = 1550; |
Khanchana | 0:812929a5d5ad | 102 | break; |
Khanchana | 0:812929a5d5ad | 103 | } |
Khanchana | 0:812929a5d5ad | 104 | printf("position down motor start = %d\n", pos_down_start); |
Khanchana | 0:812929a5d5ad | 105 | printf("position down motor end = %d\n", pos_down_end); |
Khanchana | 0:812929a5d5ad | 106 | printf("position up motor start = %d\n", pos_up_start); |
Khanchana | 0:812929a5d5ad | 107 | printf("position up motor end = %d\n", pos_up_end); |
worasuchad | 1:5609c1795245 | 108 | thread1.start(myservoLeft); |
worasuchad | 1:5609c1795245 | 109 | thread2.start(myservoRight); |
worasuchad | 1:5609c1795245 | 110 | thread3.start(IMU); |
worasuchad | 1:5609c1795245 | 111 | } */ |
worasuchad | 1:5609c1795245 | 112 | } |
worasuchad | 1:5609c1795245 | 113 | |
worasuchad | 1:5609c1795245 | 114 | |
worasuchad | 1:5609c1795245 | 115 | ///////////////////////// myservoLeft ///////////////////////// |
worasuchad | 1:5609c1795245 | 116 | ////////////////////////////////////////////////////////////////// |
worasuchad | 1:5609c1795245 | 117 | void myservoLeft() |
worasuchad | 1:5609c1795245 | 118 | { |
worasuchad | 1:5609c1795245 | 119 | for(int n = 0; n <= 5; n += 1) |
worasuchad | 1:5609c1795245 | 120 | { |
worasuchad | 1:5609c1795245 | 121 | Servo1.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 122 | Servo2.Disable(); |
worasuchad | 1:5609c1795245 | 123 | |
worasuchad | 1:5609c1795245 | 124 | for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) |
worasuchad | 1:5609c1795245 | 125 | { |
worasuchad | 1:5609c1795245 | 126 | Servo1.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 127 | wait(0.01); |
worasuchad | 1:5609c1795245 | 128 | } |
worasuchad | 1:5609c1795245 | 129 | |
worasuchad | 1:5609c1795245 | 130 | Servo2.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 131 | Servo1.Disable(); |
worasuchad | 1:5609c1795245 | 132 | |
worasuchad | 1:5609c1795245 | 133 | for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) |
worasuchad | 1:5609c1795245 | 134 | { |
worasuchad | 1:5609c1795245 | 135 | Servo2.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 136 | wait(0.01); |
worasuchad | 1:5609c1795245 | 137 | } |
worasuchad | 1:5609c1795245 | 138 | |
worasuchad | 1:5609c1795245 | 139 | Servo1.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 140 | Servo2.Disable(); |
worasuchad | 1:5609c1795245 | 141 | |
worasuchad | 1:5609c1795245 | 142 | for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) |
worasuchad | 1:5609c1795245 | 143 | { |
worasuchad | 1:5609c1795245 | 144 | Servo1.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 145 | wait(0.01); |
worasuchad | 1:5609c1795245 | 146 | } |
worasuchad | 1:5609c1795245 | 147 | |
worasuchad | 1:5609c1795245 | 148 | Servo2.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 149 | Servo1.Disable(); |
worasuchad | 1:5609c1795245 | 150 | |
worasuchad | 1:5609c1795245 | 151 | for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) |
worasuchad | 1:5609c1795245 | 152 | { |
worasuchad | 1:5609c1795245 | 153 | Servo2.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 154 | wait(0.01); |
worasuchad | 1:5609c1795245 | 155 | } |
worasuchad | 1:5609c1795245 | 156 | } |
worasuchad | 1:5609c1795245 | 157 | } |
worasuchad | 1:5609c1795245 | 158 | |
worasuchad | 1:5609c1795245 | 159 | ///////////////////////// myservoRight /////////////////////// |
worasuchad | 1:5609c1795245 | 160 | ////////////////////////////////////////////////////////////////// |
worasuchad | 1:5609c1795245 | 161 | void myservoRight() |
worasuchad | 1:5609c1795245 | 162 | { |
worasuchad | 1:5609c1795245 | 163 | for(int n = 0; n <= 5; n += 1) |
worasuchad | 1:5609c1795245 | 164 | { |
worasuchad | 1:5609c1795245 | 165 | Servo3.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 166 | Servo4.Disable(); |
worasuchad | 1:5609c1795245 | 167 | |
worasuchad | 1:5609c1795245 | 168 | for (int pos = pos_down_start; pos <= pos_down_end; pos += 5) |
worasuchad | 1:5609c1795245 | 169 | { |
worasuchad | 1:5609c1795245 | 170 | Servo3.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 171 | wait(0.01); |
worasuchad | 1:5609c1795245 | 172 | } |
worasuchad | 1:5609c1795245 | 173 | |
worasuchad | 1:5609c1795245 | 174 | Servo4.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 175 | Servo3.Disable(); |
worasuchad | 1:5609c1795245 | 176 | |
worasuchad | 1:5609c1795245 | 177 | for (int pos = pos_up_start; pos <= pos_up_end; pos += 5) |
worasuchad | 1:5609c1795245 | 178 | { |
worasuchad | 1:5609c1795245 | 179 | Servo4.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 180 | wait(0.01); |
worasuchad | 1:5609c1795245 | 181 | } |
worasuchad | 1:5609c1795245 | 182 | |
worasuchad | 1:5609c1795245 | 183 | Servo3.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 184 | Servo4.Disable(); |
worasuchad | 1:5609c1795245 | 185 | |
worasuchad | 1:5609c1795245 | 186 | for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5) |
worasuchad | 1:5609c1795245 | 187 | { |
worasuchad | 1:5609c1795245 | 188 | Servo3.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 189 | wait(0.01); |
worasuchad | 1:5609c1795245 | 190 | } |
worasuchad | 1:5609c1795245 | 191 | |
worasuchad | 1:5609c1795245 | 192 | Servo4.Enable(1000,20000); |
worasuchad | 1:5609c1795245 | 193 | Servo3.Disable(); |
worasuchad | 1:5609c1795245 | 194 | |
worasuchad | 1:5609c1795245 | 195 | for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5) |
worasuchad | 1:5609c1795245 | 196 | { |
worasuchad | 1:5609c1795245 | 197 | Servo4.SetPosition(pos); |
worasuchad | 1:5609c1795245 | 198 | wait(0.01); |
worasuchad | 1:5609c1795245 | 199 | } |
Khanchana | 0:812929a5d5ad | 200 | } |
worasuchad | 1:5609c1795245 | 201 | } |
worasuchad | 1:5609c1795245 | 202 | |
worasuchad | 1:5609c1795245 | 203 | ///////////////////////// IMU /////////////////////// |
worasuchad | 1:5609c1795245 | 204 | ////////////////////////////////////////////////////////////////// |
worasuchad | 3:5bfa7291c639 | 205 | |
worasuchad | 1:5609c1795245 | 206 | void IMU() |
worasuchad | 1:5609c1795245 | 207 | { |
worasuchad | 3:5bfa7291c639 | 208 | attitude_setup(); |
worasuchad | 3:5bfa7291c639 | 209 | while(1) |
worasuchad | 1:5609c1795245 | 210 | { |
worasuchad | 3:5bfa7291c639 | 211 | attitude_get(); |
worasuchad | 3:5bfa7291c639 | 212 | pc.printf("%.0f %.0f %.0f \n\r", roll, pitch, yaw ); |
worasuchad | 3:5bfa7291c639 | 213 | } |
worasuchad | 3:5bfa7291c639 | 214 | } |