![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Turning Robot with IMU and calibrate the straight movement
Dependencies: 4DGL-uLCD-SE LSM9DS0 Motor PinDetect mbed
main.cpp@0:31128b41490c, 2015-10-19 (annotated)
- Committer:
- yoshua0207
- Date:
- Mon Oct 19 04:20:24 2015 +0000
- Revision:
- 0:31128b41490c
- Child:
- 1:5bf45482441d
Version 1;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yoshua0207 | 0:31128b41490c | 1 | #include "mbed.h" |
yoshua0207 | 0:31128b41490c | 2 | #include "PinDetect.h" |
yoshua0207 | 0:31128b41490c | 3 | #include "uLCD_4DGL.h" |
yoshua0207 | 0:31128b41490c | 4 | #include "math.h" |
yoshua0207 | 0:31128b41490c | 5 | #include "LSM9DS0.h" |
yoshua0207 | 0:31128b41490c | 6 | #include "Motor.h" |
yoshua0207 | 0:31128b41490c | 7 | #define PI 3.14159265 |
yoshua0207 | 0:31128b41490c | 8 | #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW |
yoshua0207 | 0:31128b41490c | 9 | #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW |
yoshua0207 | 0:31128b41490c | 10 | |
yoshua0207 | 0:31128b41490c | 11 | // refresh time. set to 500 for part 2 and 50 for part 4 |
yoshua0207 | 0:31128b41490c | 12 | #define REFRESH_TIME_MS 500 |
yoshua0207 | 0:31128b41490c | 13 | Serial pc(USBTX, USBRX); |
yoshua0207 | 0:31128b41490c | 14 | // Verify that the pin assignments below match your breadboard |
yoshua0207 | 0:31128b41490c | 15 | LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); |
yoshua0207 | 0:31128b41490c | 16 | Motor mA(p21, p23, p24); // pwm, fwd, rev LEFT |
yoshua0207 | 0:31128b41490c | 17 | Motor mB(p22, p25, p26); // pwm, fwd, rev RIGHT |
yoshua0207 | 0:31128b41490c | 18 | //speed: 10.44 cm/s |
yoshua0207 | 0:31128b41490c | 19 | // use class to setup pushbuttons pins |
yoshua0207 | 0:31128b41490c | 20 | PinDetect pb1(p16); |
yoshua0207 | 0:31128b41490c | 21 | PinDetect pb2(p17); |
yoshua0207 | 0:31128b41490c | 22 | PinDetect pb3(p18); |
yoshua0207 | 0:31128b41490c | 23 | PinDetect pb4(p19); |
yoshua0207 | 0:31128b41490c | 24 | |
yoshua0207 | 0:31128b41490c | 25 | DigitalOut myLED1(LED1); |
yoshua0207 | 0:31128b41490c | 26 | DigitalOut myLED2(LED2); |
yoshua0207 | 0:31128b41490c | 27 | DigitalOut myLED3(LED3); |
yoshua0207 | 0:31128b41490c | 28 | DigitalOut myLED4(LED4); |
yoshua0207 | 0:31128b41490c | 29 | // use class to setup the Color LCD |
yoshua0207 | 0:31128b41490c | 30 | uLCD_4DGL uLCD(p28, p27, p29); // create a global uLCD object |
yoshua0207 | 0:31128b41490c | 31 | |
yoshua0207 | 0:31128b41490c | 32 | //Timer class |
yoshua0207 | 0:31128b41490c | 33 | Timer timer; |
yoshua0207 | 0:31128b41490c | 34 | |
yoshua0207 | 0:31128b41490c | 35 | //Mode settting |
yoshua0207 | 0:31128b41490c | 36 | |
yoshua0207 | 0:31128b41490c | 37 | enum Modetype { SETX = 0, SETY, GO }; |
yoshua0207 | 0:31128b41490c | 38 | Modetype mode1 = SETX; |
yoshua0207 | 0:31128b41490c | 39 | int settingMode = 0; |
yoshua0207 | 0:31128b41490c | 40 | //Variable |
yoshua0207 | 0:31128b41490c | 41 | int xCoord = 0; |
yoshua0207 | 0:31128b41490c | 42 | int yCoord = 0; |
yoshua0207 | 0:31128b41490c | 43 | float distance = 0; |
yoshua0207 | 0:31128b41490c | 44 | float angle = 0; |
yoshua0207 | 0:31128b41490c | 45 | float seconds = 0; |
yoshua0207 | 0:31128b41490c | 46 | int startDegree = 0; |
yoshua0207 | 0:31128b41490c | 47 | int turningDegree = 0; |
yoshua0207 | 0:31128b41490c | 48 | int finalDegree = 0; |
yoshua0207 | 0:31128b41490c | 49 | int heading = 0; |
yoshua0207 | 0:31128b41490c | 50 | int degree = 0; |
yoshua0207 | 0:31128b41490c | 51 | int initialHeading = 0; |
yoshua0207 | 0:31128b41490c | 52 | int state = 0; |
yoshua0207 | 0:31128b41490c | 53 | int difference = 0; |
yoshua0207 | 0:31128b41490c | 54 | void setup() |
yoshua0207 | 0:31128b41490c | 55 | { |
yoshua0207 | 0:31128b41490c | 56 | uLCD.cls(); |
yoshua0207 | 0:31128b41490c | 57 | uint16_t status = imu.begin(); |
yoshua0207 | 0:31128b41490c | 58 | //Make sure communication is working |
yoshua0207 | 0:31128b41490c | 59 | } |
yoshua0207 | 0:31128b41490c | 60 | |
yoshua0207 | 0:31128b41490c | 61 | //When pb1 push increment the coordinate |
yoshua0207 | 0:31128b41490c | 62 | void pb1_hit_callback (void) |
yoshua0207 | 0:31128b41490c | 63 | { |
yoshua0207 | 0:31128b41490c | 64 | if (mode1 == SETX) |
yoshua0207 | 0:31128b41490c | 65 | xCoord++; |
yoshua0207 | 0:31128b41490c | 66 | else if (mode1 == SETY) |
yoshua0207 | 0:31128b41490c | 67 | yCoord++; |
yoshua0207 | 0:31128b41490c | 68 | |
yoshua0207 | 0:31128b41490c | 69 | myLED1 = 1; |
yoshua0207 | 0:31128b41490c | 70 | myLED2 = 0; |
yoshua0207 | 0:31128b41490c | 71 | myLED3 = 0; |
yoshua0207 | 0:31128b41490c | 72 | myLED4 = 0; |
yoshua0207 | 0:31128b41490c | 73 | } |
yoshua0207 | 0:31128b41490c | 74 | |
yoshua0207 | 0:31128b41490c | 75 | //When pb2 push decrement the coordinate |
yoshua0207 | 0:31128b41490c | 76 | void pb2_hit_callback (void) |
yoshua0207 | 0:31128b41490c | 77 | { |
yoshua0207 | 0:31128b41490c | 78 | if (mode1 == SETX) |
yoshua0207 | 0:31128b41490c | 79 | xCoord--; |
yoshua0207 | 0:31128b41490c | 80 | else if (mode1 == SETY) |
yoshua0207 | 0:31128b41490c | 81 | yCoord--; |
yoshua0207 | 0:31128b41490c | 82 | |
yoshua0207 | 0:31128b41490c | 83 | myLED2 = 1; |
yoshua0207 | 0:31128b41490c | 84 | myLED1 = 0; |
yoshua0207 | 0:31128b41490c | 85 | myLED3 = 0; |
yoshua0207 | 0:31128b41490c | 86 | myLED4 = 0; |
yoshua0207 | 0:31128b41490c | 87 | |
yoshua0207 | 0:31128b41490c | 88 | } |
yoshua0207 | 0:31128b41490c | 89 | |
yoshua0207 | 0:31128b41490c | 90 | //When pb3 push increment the mode |
yoshua0207 | 0:31128b41490c | 91 | void pb3_hit_callback (void) |
yoshua0207 | 0:31128b41490c | 92 | { |
yoshua0207 | 0:31128b41490c | 93 | mode1 = static_cast<Modetype>((static_cast<int>(mode1) + 1) % 3); |
yoshua0207 | 0:31128b41490c | 94 | myLED3 = 1; |
yoshua0207 | 0:31128b41490c | 95 | myLED1 = 0; |
yoshua0207 | 0:31128b41490c | 96 | myLED2 = 0; |
yoshua0207 | 0:31128b41490c | 97 | myLED4 = 0; |
yoshua0207 | 0:31128b41490c | 98 | } |
yoshua0207 | 0:31128b41490c | 99 | |
yoshua0207 | 0:31128b41490c | 100 | //When pb4 push decrement the mode |
yoshua0207 | 0:31128b41490c | 101 | void pb4_hit_callback (void) |
yoshua0207 | 0:31128b41490c | 102 | { |
yoshua0207 | 0:31128b41490c | 103 | if (mode1 == SETY) |
yoshua0207 | 0:31128b41490c | 104 | { |
yoshua0207 | 0:31128b41490c | 105 | mode1 = SETX; |
yoshua0207 | 0:31128b41490c | 106 | } |
yoshua0207 | 0:31128b41490c | 107 | myLED4 = 1; |
yoshua0207 | 0:31128b41490c | 108 | myLED1 = 0; |
yoshua0207 | 0:31128b41490c | 109 | myLED2 = 0; |
yoshua0207 | 0:31128b41490c | 110 | myLED3 = 0; |
yoshua0207 | 0:31128b41490c | 111 | } |
yoshua0207 | 0:31128b41490c | 112 | |
yoshua0207 | 0:31128b41490c | 113 | void turnRight(void) |
yoshua0207 | 0:31128b41490c | 114 | { |
yoshua0207 | 0:31128b41490c | 115 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 116 | myLED3 = 1; |
yoshua0207 | 0:31128b41490c | 117 | uLCD.locate(1,5); |
yoshua0207 | 0:31128b41490c | 118 | uLCD.printf("Turning Right \n"); |
yoshua0207 | 0:31128b41490c | 119 | mA.speed(0.2); |
yoshua0207 | 0:31128b41490c | 120 | mB.speed(-0.2); |
yoshua0207 | 0:31128b41490c | 121 | heading = static_cast<int> (imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 122 | uLCD.printf("H : %d \n",heading); |
yoshua0207 | 0:31128b41490c | 123 | degree = heading - initialHeading; |
yoshua0207 | 0:31128b41490c | 124 | degree = turningDegree - degree; |
yoshua0207 | 0:31128b41490c | 125 | uLCD.printf("Degree = %d \n",degree); |
yoshua0207 | 0:31128b41490c | 126 | uLCD.printf("Heading = %d \n",heading); |
yoshua0207 | 0:31128b41490c | 127 | } |
yoshua0207 | 0:31128b41490c | 128 | |
yoshua0207 | 0:31128b41490c | 129 | void turnLeft(void) |
yoshua0207 | 0:31128b41490c | 130 | { |
yoshua0207 | 0:31128b41490c | 131 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 132 | myLED3 = 1; |
yoshua0207 | 0:31128b41490c | 133 | uLCD.locate(1,5); |
yoshua0207 | 0:31128b41490c | 134 | uLCD.printf("Turning Left \n"); |
yoshua0207 | 0:31128b41490c | 135 | mA.speed(-0.2); |
yoshua0207 | 0:31128b41490c | 136 | mB.speed(0.2); |
yoshua0207 | 0:31128b41490c | 137 | heading = static_cast<int> (imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 138 | uLCD.printf("H : %d \n",heading); |
yoshua0207 | 0:31128b41490c | 139 | degree = heading - initialHeading; |
yoshua0207 | 0:31128b41490c | 140 | degree = turningDegree - degree; |
yoshua0207 | 0:31128b41490c | 141 | uLCD.printf("Degree = %d \n",degree); |
yoshua0207 | 0:31128b41490c | 142 | uLCD.printf("Heading = %d \n",heading); |
yoshua0207 | 0:31128b41490c | 143 | } |
yoshua0207 | 0:31128b41490c | 144 | |
yoshua0207 | 0:31128b41490c | 145 | |
yoshua0207 | 0:31128b41490c | 146 | int main() |
yoshua0207 | 0:31128b41490c | 147 | { |
yoshua0207 | 0:31128b41490c | 148 | uLCD.cls(); |
yoshua0207 | 0:31128b41490c | 149 | // Use internal pullups for the three pushbuttons |
yoshua0207 | 0:31128b41490c | 150 | pb1.mode(PullUp); |
yoshua0207 | 0:31128b41490c | 151 | pb2.mode(PullUp); |
yoshua0207 | 0:31128b41490c | 152 | pb3.mode(PullUp); |
yoshua0207 | 0:31128b41490c | 153 | pb4.mode(PullUp); |
yoshua0207 | 0:31128b41490c | 154 | // Delay for initial pullup to take effect |
yoshua0207 | 0:31128b41490c | 155 | wait(.01); |
yoshua0207 | 0:31128b41490c | 156 | // Setup Interrupt callback functions for a pb hit |
yoshua0207 | 0:31128b41490c | 157 | pb1.attach_deasserted(&pb1_hit_callback); |
yoshua0207 | 0:31128b41490c | 158 | pb2.attach_deasserted(&pb2_hit_callback); |
yoshua0207 | 0:31128b41490c | 159 | pb3.attach_deasserted(&pb3_hit_callback); |
yoshua0207 | 0:31128b41490c | 160 | pb4.attach_deasserted(&pb4_hit_callback); |
yoshua0207 | 0:31128b41490c | 161 | // Start sampling pb inputs using interrupts |
yoshua0207 | 0:31128b41490c | 162 | pb1.setSampleFrequency(); |
yoshua0207 | 0:31128b41490c | 163 | pb2.setSampleFrequency(); |
yoshua0207 | 0:31128b41490c | 164 | pb3.setSampleFrequency(); |
yoshua0207 | 0:31128b41490c | 165 | pb4.setSampleFrequency(); |
yoshua0207 | 0:31128b41490c | 166 | mode1 = SETX; |
yoshua0207 | 0:31128b41490c | 167 | setup(); |
yoshua0207 | 0:31128b41490c | 168 | |
yoshua0207 | 0:31128b41490c | 169 | xCoord = 0; |
yoshua0207 | 0:31128b41490c | 170 | yCoord = 0; |
yoshua0207 | 0:31128b41490c | 171 | state = 1; |
yoshua0207 | 0:31128b41490c | 172 | //Setting mode |
yoshua0207 | 0:31128b41490c | 173 | while(settingMode == 0) |
yoshua0207 | 0:31128b41490c | 174 | { |
yoshua0207 | 0:31128b41490c | 175 | uLCD.locate(1,2); |
yoshua0207 | 0:31128b41490c | 176 | if (mode1 == SETX) |
yoshua0207 | 0:31128b41490c | 177 | { |
yoshua0207 | 0:31128b41490c | 178 | uLCD.printf("Setting X coordinate"); |
yoshua0207 | 0:31128b41490c | 179 | } |
yoshua0207 | 0:31128b41490c | 180 | else if (mode1 == SETY) |
yoshua0207 | 0:31128b41490c | 181 | { |
yoshua0207 | 0:31128b41490c | 182 | uLCD.printf("Setting Y Coordinate"); |
yoshua0207 | 0:31128b41490c | 183 | } |
yoshua0207 | 0:31128b41490c | 184 | else if (mode1 == GO) |
yoshua0207 | 0:31128b41490c | 185 | { |
yoshua0207 | 0:31128b41490c | 186 | uLCD.printf("Running Now"); |
yoshua0207 | 0:31128b41490c | 187 | } |
yoshua0207 | 0:31128b41490c | 188 | uLCD.locate(4,5); |
yoshua0207 | 0:31128b41490c | 189 | uLCD.printf("X-Coord: %d \n\r", xCoord); |
yoshua0207 | 0:31128b41490c | 190 | uLCD.locate(4,7); |
yoshua0207 | 0:31128b41490c | 191 | uLCD.printf("Y-Coord: %d \n\r", yCoord); |
yoshua0207 | 0:31128b41490c | 192 | |
yoshua0207 | 0:31128b41490c | 193 | if (mode1 == GO) |
yoshua0207 | 0:31128b41490c | 194 | { |
yoshua0207 | 0:31128b41490c | 195 | distance = xCoord*xCoord + yCoord*yCoord; |
yoshua0207 | 0:31128b41490c | 196 | distance = sqrt(distance); |
yoshua0207 | 0:31128b41490c | 197 | float totalDistance = distance * 30.48; |
yoshua0207 | 0:31128b41490c | 198 | seconds = totalDistance / 23.18 * 1000; |
yoshua0207 | 0:31128b41490c | 199 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 200 | initialHeading = static_cast<int>(imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 201 | angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI; |
yoshua0207 | 0:31128b41490c | 202 | finalDegree = initialHeading + angle; |
yoshua0207 | 0:31128b41490c | 203 | uLCD.locate(4,9); |
yoshua0207 | 0:31128b41490c | 204 | uLCD.printf("Distance: %5.2F Feet \n\r", distance); |
yoshua0207 | 0:31128b41490c | 205 | uLCD.locate(4,11); |
yoshua0207 | 0:31128b41490c | 206 | uLCD.printf("Turning: %5.2F Degree \n\r", angle); |
yoshua0207 | 0:31128b41490c | 207 | |
yoshua0207 | 0:31128b41490c | 208 | settingMode = 1; |
yoshua0207 | 0:31128b41490c | 209 | } |
yoshua0207 | 0:31128b41490c | 210 | |
yoshua0207 | 0:31128b41490c | 211 | } //end first while |
yoshua0207 | 0:31128b41490c | 212 | wait(2); |
yoshua0207 | 0:31128b41490c | 213 | //Turning mode |
yoshua0207 | 0:31128b41490c | 214 | while(settingMode == 1) |
yoshua0207 | 0:31128b41490c | 215 | { |
yoshua0207 | 0:31128b41490c | 216 | uLCD.cls(); |
yoshua0207 | 0:31128b41490c | 217 | uLCD.printf("Setting mode 1"); |
yoshua0207 | 0:31128b41490c | 218 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 219 | initialHeading = static_cast<int> (imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 220 | uLCD.locate(1,3); |
yoshua0207 | 0:31128b41490c | 221 | uLCD.printf("Initial : %d \n",initialHeading); |
yoshua0207 | 0:31128b41490c | 222 | |
yoshua0207 | 0:31128b41490c | 223 | startDegree = heading; |
yoshua0207 | 0:31128b41490c | 224 | turningDegree = angle; |
yoshua0207 | 0:31128b41490c | 225 | degree = static_cast<int> (imu.calcHeading()) - initialHeading; |
yoshua0207 | 0:31128b41490c | 226 | |
yoshua0207 | 0:31128b41490c | 227 | if (turningDegree > 0) |
yoshua0207 | 0:31128b41490c | 228 | { |
yoshua0207 | 0:31128b41490c | 229 | while (degree > -2 && degree < 180) |
yoshua0207 | 0:31128b41490c | 230 | { |
yoshua0207 | 0:31128b41490c | 231 | turnRight(); |
yoshua0207 | 0:31128b41490c | 232 | wait(0.2); |
yoshua0207 | 0:31128b41490c | 233 | mA.speed(0); |
yoshua0207 | 0:31128b41490c | 234 | mB.speed(0); |
yoshua0207 | 0:31128b41490c | 235 | } |
yoshua0207 | 0:31128b41490c | 236 | settingMode = 2; |
yoshua0207 | 0:31128b41490c | 237 | |
yoshua0207 | 0:31128b41490c | 238 | } |
yoshua0207 | 0:31128b41490c | 239 | else |
yoshua0207 | 0:31128b41490c | 240 | { |
yoshua0207 | 0:31128b41490c | 241 | while (degree < 2 && degree > -180 ) |
yoshua0207 | 0:31128b41490c | 242 | { |
yoshua0207 | 0:31128b41490c | 243 | turnLeft(); |
yoshua0207 | 0:31128b41490c | 244 | wait(0.2); |
yoshua0207 | 0:31128b41490c | 245 | mA.speed(0); |
yoshua0207 | 0:31128b41490c | 246 | mB.speed(0); |
yoshua0207 | 0:31128b41490c | 247 | } |
yoshua0207 | 0:31128b41490c | 248 | settingMode = 2; |
yoshua0207 | 0:31128b41490c | 249 | } |
yoshua0207 | 0:31128b41490c | 250 | } |
yoshua0207 | 0:31128b41490c | 251 | |
yoshua0207 | 0:31128b41490c | 252 | wait(2); |
yoshua0207 | 0:31128b41490c | 253 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 254 | initialHeading = static_cast<int> (imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 255 | timer.start(); |
yoshua0207 | 0:31128b41490c | 256 | while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2) |
yoshua0207 | 0:31128b41490c | 257 | { |
yoshua0207 | 0:31128b41490c | 258 | uLCD.locate(1,7); |
yoshua0207 | 0:31128b41490c | 259 | uLCD.printf("Time : %d \n",timer.read_ms()); |
yoshua0207 | 0:31128b41490c | 260 | mA.speed(0.4); |
yoshua0207 | 0:31128b41490c | 261 | mB.speed(0.4); |
yoshua0207 | 0:31128b41490c | 262 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 263 | heading = static_cast<int> (imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 264 | difference = heading - initialHeading; |
yoshua0207 | 0:31128b41490c | 265 | uLCD.locate(1,3); |
yoshua0207 | 0:31128b41490c | 266 | uLCD.printf("Heading : %d \n",heading); |
yoshua0207 | 0:31128b41490c | 267 | if( difference <-2) |
yoshua0207 | 0:31128b41490c | 268 | { |
yoshua0207 | 0:31128b41490c | 269 | mA.speed(0.5); |
yoshua0207 | 0:31128b41490c | 270 | mB.speed(0.4); |
yoshua0207 | 0:31128b41490c | 271 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 272 | heading = static_cast<int> (imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 273 | difference = heading - finalDegree; |
yoshua0207 | 0:31128b41490c | 274 | uLCD.locate(1,3); |
yoshua0207 | 0:31128b41490c | 275 | uLCD.printf("Heading : %d \n",heading); |
yoshua0207 | 0:31128b41490c | 276 | } |
yoshua0207 | 0:31128b41490c | 277 | if(difference >2) |
yoshua0207 | 0:31128b41490c | 278 | { |
yoshua0207 | 0:31128b41490c | 279 | mA.speed(0.4); |
yoshua0207 | 0:31128b41490c | 280 | mB.speed(0.5); |
yoshua0207 | 0:31128b41490c | 281 | imu.readMag(); |
yoshua0207 | 0:31128b41490c | 282 | heading = static_cast<int> (imu.calcHeading()); |
yoshua0207 | 0:31128b41490c | 283 | difference = heading - finalDegree; |
yoshua0207 | 0:31128b41490c | 284 | uLCD.locate(1,3); |
yoshua0207 | 0:31128b41490c | 285 | uLCD.printf("Heading : %d \n",heading); |
yoshua0207 | 0:31128b41490c | 286 | } |
yoshua0207 | 0:31128b41490c | 287 | } |
yoshua0207 | 0:31128b41490c | 288 | mA.speed(0); |
yoshua0207 | 0:31128b41490c | 289 | mB.speed(0); |
yoshua0207 | 0:31128b41490c | 290 | |
yoshua0207 | 0:31128b41490c | 291 | } |