Yo Shua / Mbed 2 deprecated 4180RobotIMU

Turning Robot with IMU and calibrate the straight movement

Dependencies:   4DGL-uLCD-SE LSM9DS0 Motor PinDetect mbed

main.cpp@1:5bf45482441d, 2015-10-20 (annotated)

Committer:
yoshua0207
Date:
Tue Oct 20 04:23:32 2015 +0000
Revision:
1:5bf45482441d
Parent:
0:31128b41490c 
Version 1.2

Who changed what in which revision?

UserRevisionLine numberNew 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 1:5bf45482441d 54 int counter = 0;
yoshua0207 0:31128b41490c 55 void setup()
yoshua0207 0:31128b41490c 56 {
yoshua0207 0:31128b41490c 57 uLCD.cls();
yoshua0207 0:31128b41490c 58 uint16_t status = imu.begin();
yoshua0207 0:31128b41490c 59 //Make sure communication is working
yoshua0207 0:31128b41490c 60 }
yoshua0207 0:31128b41490c 61
yoshua0207 0:31128b41490c 62 //When pb1 push increment the coordinate
yoshua0207 0:31128b41490c 63 void pb1_hit_callback (void)
yoshua0207 0:31128b41490c 64 {
yoshua0207 0:31128b41490c 65 if (mode1 == SETX)
yoshua0207 0:31128b41490c 66 xCoord++;
yoshua0207 0:31128b41490c 67 else if (mode1 == SETY)
yoshua0207 0:31128b41490c 68 yCoord++;
yoshua0207 0:31128b41490c 69
yoshua0207 0:31128b41490c 70 myLED1 = 1;
yoshua0207 0:31128b41490c 71 myLED2 = 0;
yoshua0207 0:31128b41490c 72 myLED3 = 0;
yoshua0207 0:31128b41490c 73 myLED4 = 0;
yoshua0207 0:31128b41490c 74 }
yoshua0207 0:31128b41490c 75
yoshua0207 0:31128b41490c 76 //When pb2 push decrement the coordinate
yoshua0207 0:31128b41490c 77 void pb2_hit_callback (void)
yoshua0207 0:31128b41490c 78 {
yoshua0207 0:31128b41490c 79 if (mode1 == SETX)
yoshua0207 0:31128b41490c 80 xCoord--;
yoshua0207 0:31128b41490c 81 else if (mode1 == SETY)
yoshua0207 0:31128b41490c 82 yCoord--;
yoshua0207 0:31128b41490c 83
yoshua0207 0:31128b41490c 84 myLED2 = 1;
yoshua0207 0:31128b41490c 85 myLED1 = 0;
yoshua0207 0:31128b41490c 86 myLED3 = 0;
yoshua0207 0:31128b41490c 87 myLED4 = 0;
yoshua0207 0:31128b41490c 88
yoshua0207 0:31128b41490c 89 }
yoshua0207 0:31128b41490c 90
yoshua0207 0:31128b41490c 91 //When pb3 push increment the mode
yoshua0207 0:31128b41490c 92 void pb3_hit_callback (void)
yoshua0207 0:31128b41490c 93 {
yoshua0207 0:31128b41490c 94 mode1 = static_cast<Modetype>((static_cast<int>(mode1) + 1) % 3);
yoshua0207 0:31128b41490c 95 myLED3 = 1;
yoshua0207 0:31128b41490c 96 myLED1 = 0;
yoshua0207 0:31128b41490c 97 myLED2 = 0;
yoshua0207 0:31128b41490c 98 myLED4 = 0;
yoshua0207 0:31128b41490c 99 }
yoshua0207 0:31128b41490c 100
yoshua0207 0:31128b41490c 101 //When pb4 push decrement the mode
yoshua0207 0:31128b41490c 102 void pb4_hit_callback (void)
yoshua0207 0:31128b41490c 103 {
yoshua0207 0:31128b41490c 104 if (mode1 == SETY)
yoshua0207 0:31128b41490c 105 {
yoshua0207 0:31128b41490c 106 mode1 = SETX;
yoshua0207 0:31128b41490c 107 }
yoshua0207 0:31128b41490c 108 myLED4 = 1;
yoshua0207 0:31128b41490c 109 myLED1 = 0;
yoshua0207 0:31128b41490c 110 myLED2 = 0;
yoshua0207 0:31128b41490c 111 myLED3 = 0;
yoshua0207 0:31128b41490c 112 }
yoshua0207 0:31128b41490c 113
yoshua0207 0:31128b41490c 114 void turnRight(void)
yoshua0207 0:31128b41490c 115 {
yoshua0207 0:31128b41490c 116 imu.readMag();
yoshua0207 0:31128b41490c 117 myLED3 = 1;
yoshua0207 1:5bf45482441d 118 // uLCD.locate(1,5);
yoshua0207 1:5bf45482441d 119 // uLCD.printf("Turning Right \n");
yoshua0207 1:5bf45482441d 120 mA.speed(0.25);
yoshua0207 1:5bf45482441d 121 mB.speed(-0.25);
yoshua0207 0:31128b41490c 122 heading = static_cast<int> (imu.calcHeading());
yoshua0207 1:5bf45482441d 123 // uLCD.printf("H : %d \n",heading);
yoshua0207 0:31128b41490c 124 degree = heading - initialHeading;
yoshua0207 0:31128b41490c 125 degree = turningDegree - degree;
yoshua0207 1:5bf45482441d 126 uLCD.locate(1,5);
yoshua0207 1:5bf45482441d 127 uLCD.printf("Degree Left = %d \n",degree);
yoshua0207 0:31128b41490c 128 uLCD.printf("Heading = %d \n",heading);
yoshua0207 0:31128b41490c 129 }
yoshua0207 0:31128b41490c 130
yoshua0207 0:31128b41490c 131 void turnLeft(void)
yoshua0207 0:31128b41490c 132 {
yoshua0207 0:31128b41490c 133 imu.readMag();
yoshua0207 0:31128b41490c 134 myLED3 = 1;
yoshua0207 1:5bf45482441d 135 // uLCD.locate(1,5);
yoshua0207 1:5bf45482441d 136 // uLCD.printf("Turning Left \n");
yoshua0207 1:5bf45482441d 137 mA.speed(-0.25);
yoshua0207 1:5bf45482441d 138 mB.speed(0.25);
yoshua0207 0:31128b41490c 139 heading = static_cast<int> (imu.calcHeading());
yoshua0207 1:5bf45482441d 140 // uLCD.printf("H : %d \n",heading);
yoshua0207 0:31128b41490c 141 degree = heading - initialHeading;
yoshua0207 0:31128b41490c 142 degree = turningDegree - degree;
yoshua0207 1:5bf45482441d 143 uLCD.locate(1,5);
yoshua0207 1:5bf45482441d 144 uLCD.printf("Degree Left = %d \n",degree);
yoshua0207 0:31128b41490c 145 uLCD.printf("Heading = %d \n",heading);
yoshua0207 0:31128b41490c 146 }
yoshua0207 0:31128b41490c 147
yoshua0207 0:31128b41490c 148
yoshua0207 0:31128b41490c 149 int main()
yoshua0207 0:31128b41490c 150 {
yoshua0207 0:31128b41490c 151 uLCD.cls();
yoshua0207 0:31128b41490c 152 // Use internal pullups for the three pushbuttons
yoshua0207 0:31128b41490c 153 pb1.mode(PullUp);
yoshua0207 0:31128b41490c 154 pb2.mode(PullUp);
yoshua0207 0:31128b41490c 155 pb3.mode(PullUp);
yoshua0207 0:31128b41490c 156 pb4.mode(PullUp);
yoshua0207 0:31128b41490c 157 // Delay for initial pullup to take effect
yoshua0207 0:31128b41490c 158 wait(.01);
yoshua0207 0:31128b41490c 159 // Setup Interrupt callback functions for a pb hit
yoshua0207 0:31128b41490c 160 pb1.attach_deasserted(&pb1_hit_callback);
yoshua0207 0:31128b41490c 161 pb2.attach_deasserted(&pb2_hit_callback);
yoshua0207 0:31128b41490c 162 pb3.attach_deasserted(&pb3_hit_callback);
yoshua0207 0:31128b41490c 163 pb4.attach_deasserted(&pb4_hit_callback);
yoshua0207 0:31128b41490c 164 // Start sampling pb inputs using interrupts
yoshua0207 0:31128b41490c 165 pb1.setSampleFrequency();
yoshua0207 0:31128b41490c 166 pb2.setSampleFrequency();
yoshua0207 0:31128b41490c 167 pb3.setSampleFrequency();
yoshua0207 0:31128b41490c 168 pb4.setSampleFrequency();
yoshua0207 0:31128b41490c 169 mode1 = SETX;
yoshua0207 0:31128b41490c 170 setup();
yoshua0207 0:31128b41490c 171
yoshua0207 0:31128b41490c 172 xCoord = 0;
yoshua0207 0:31128b41490c 173 yCoord = 0;
yoshua0207 0:31128b41490c 174 state = 1;
yoshua0207 0:31128b41490c 175 //Setting mode
yoshua0207 0:31128b41490c 176 while(settingMode == 0)
yoshua0207 0:31128b41490c 177 {
yoshua0207 1:5bf45482441d 178 uLCD.locate(1,1);
yoshua0207 0:31128b41490c 179 if (mode1 == SETX)
yoshua0207 0:31128b41490c 180 {
yoshua0207 1:5bf45482441d 181 uLCD.printf("Setting X Coord");
yoshua0207 0:31128b41490c 182 }
yoshua0207 0:31128b41490c 183 else if (mode1 == SETY)
yoshua0207 0:31128b41490c 184 {
yoshua0207 1:5bf45482441d 185 uLCD.printf("Setting Y Coord");
yoshua0207 0:31128b41490c 186 }
yoshua0207 0:31128b41490c 187 else if (mode1 == GO)
yoshua0207 0:31128b41490c 188 {
yoshua0207 0:31128b41490c 189 uLCD.printf("Running Now");
yoshua0207 0:31128b41490c 190 }
yoshua0207 0:31128b41490c 191 uLCD.locate(4,5);
yoshua0207 0:31128b41490c 192 uLCD.printf("X-Coord: %d \n\r", xCoord);
yoshua0207 0:31128b41490c 193 uLCD.locate(4,7);
yoshua0207 0:31128b41490c 194 uLCD.printf("Y-Coord: %d \n\r", yCoord);
yoshua0207 0:31128b41490c 195
yoshua0207 0:31128b41490c 196 if (mode1 == GO)
yoshua0207 0:31128b41490c 197 {
yoshua0207 0:31128b41490c 198 distance = xCoord*xCoord + yCoord*yCoord;
yoshua0207 0:31128b41490c 199 distance = sqrt(distance);
yoshua0207 0:31128b41490c 200 float totalDistance = distance * 30.48;
yoshua0207 0:31128b41490c 201 seconds = totalDistance / 23.18 * 1000;
yoshua0207 0:31128b41490c 202 imu.readMag();
yoshua0207 0:31128b41490c 203 initialHeading = static_cast<int>(imu.calcHeading());
yoshua0207 0:31128b41490c 204 angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI;
yoshua0207 0:31128b41490c 205 finalDegree = initialHeading + angle;
yoshua0207 1:5bf45482441d 206 uLCD.locate(1,9);
yoshua0207 0:31128b41490c 207 uLCD.printf("Distance: %5.2F Feet \n\r", distance);
yoshua0207 1:5bf45482441d 208 uLCD.locate(1,11);
yoshua0207 0:31128b41490c 209 uLCD.printf("Turning: %5.2F Degree \n\r", angle);
yoshua0207 0:31128b41490c 210
yoshua0207 0:31128b41490c 211 settingMode = 1;
yoshua0207 0:31128b41490c 212 }
yoshua0207 0:31128b41490c 213
yoshua0207 0:31128b41490c 214 } //end first while
yoshua0207 0:31128b41490c 215 wait(2);
yoshua0207 0:31128b41490c 216 //Turning mode
yoshua0207 0:31128b41490c 217 while(settingMode == 1)
yoshua0207 0:31128b41490c 218 {
yoshua0207 0:31128b41490c 219 uLCD.cls();
yoshua0207 1:5bf45482441d 220 // uLCD.printf("Setting mode 1");
yoshua0207 0:31128b41490c 221 imu.readMag();
yoshua0207 0:31128b41490c 222 initialHeading = static_cast<int> (imu.calcHeading());
yoshua0207 0:31128b41490c 223 uLCD.locate(1,3);
yoshua0207 0:31128b41490c 224 uLCD.printf("Initial : %d \n",initialHeading);
yoshua0207 0:31128b41490c 225
yoshua0207 0:31128b41490c 226 startDegree = heading;
yoshua0207 0:31128b41490c 227 turningDegree = angle;
yoshua0207 0:31128b41490c 228 degree = static_cast<int> (imu.calcHeading()) - initialHeading;
yoshua0207 0:31128b41490c 229
yoshua0207 0:31128b41490c 230 if (turningDegree > 0)
yoshua0207 0:31128b41490c 231 {
yoshua0207 0:31128b41490c 232 while (degree > -2 && degree < 180)
yoshua0207 0:31128b41490c 233 {
yoshua0207 0:31128b41490c 234 turnRight();
yoshua0207 1:5bf45482441d 235
yoshua0207 0:31128b41490c 236 mA.speed(0);
yoshua0207 0:31128b41490c 237 mB.speed(0);
yoshua0207 1:5bf45482441d 238 wait(0.4);
yoshua0207 0:31128b41490c 239 }
yoshua0207 0:31128b41490c 240 settingMode = 2;
yoshua0207 0:31128b41490c 241
yoshua0207 0:31128b41490c 242 }
yoshua0207 0:31128b41490c 243 else
yoshua0207 0:31128b41490c 244 {
yoshua0207 0:31128b41490c 245 while (degree < 2 && degree > -180 )
yoshua0207 0:31128b41490c 246 {
yoshua0207 0:31128b41490c 247 turnLeft();
yoshua0207 1:5bf45482441d 248
yoshua0207 0:31128b41490c 249 mA.speed(0);
yoshua0207 1:5bf45482441d 250 mB.speed(0);
yoshua0207 1:5bf45482441d 251 wait(0.4);
yoshua0207 0:31128b41490c 252 }
yoshua0207 0:31128b41490c 253 settingMode = 2;
yoshua0207 0:31128b41490c 254 }
yoshua0207 0:31128b41490c 255 }
yoshua0207 0:31128b41490c 256
yoshua0207 0:31128b41490c 257 wait(2);
yoshua0207 0:31128b41490c 258 imu.readMag();
yoshua0207 0:31128b41490c 259 initialHeading = static_cast<int> (imu.calcHeading());
yoshua0207 0:31128b41490c 260 timer.start();
yoshua0207 1:5bf45482441d 261 uLCD.cls();
yoshua0207 0:31128b41490c 262 while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2)
yoshua0207 0:31128b41490c 263 {
yoshua0207 1:5bf45482441d 264
yoshua0207 1:5bf45482441d 265
yoshua0207 1:5bf45482441d 266 // uLCD.printf("Time : %d \n",timer.read_ms());
yoshua0207 0:31128b41490c 267 mA.speed(0.4);
yoshua0207 0:31128b41490c 268 mB.speed(0.4);
yoshua0207 0:31128b41490c 269 imu.readMag();
yoshua0207 0:31128b41490c 270 heading = static_cast<int> (imu.calcHeading());
yoshua0207 1:5bf45482441d 271 difference = heading - finalDegree;
yoshua0207 1:5bf45482441d 272 uLCD.locate(5,6);
yoshua0207 1:5bf45482441d 273 uLCD.printf("Go!");
yoshua0207 1:5bf45482441d 274 while( difference <-2 &&timer.read_ms() < static_cast<int>(seconds) )
yoshua0207 0:31128b41490c 275 {
yoshua0207 1:5bf45482441d 276
yoshua0207 0:31128b41490c 277 mA.speed(0.5);
yoshua0207 0:31128b41490c 278 mB.speed(0.4);
yoshua0207 0:31128b41490c 279 imu.readMag();
yoshua0207 0:31128b41490c 280 heading = static_cast<int> (imu.calcHeading());
yoshua0207 0:31128b41490c 281 difference = heading - finalDegree;
yoshua0207 0:31128b41490c 282 uLCD.locate(1,3);
yoshua0207 1:5bf45482441d 283 uLCD.printf("Difference : %d \n",difference);
yoshua0207 1:5bf45482441d 284 uLCD.locate(1,4);
yoshua0207 1:5bf45482441d 285 uLCD.printf("Turning Right \n");
yoshua0207 1:5bf45482441d 286 uLCD.locate(1,5);
yoshua0207 1:5bf45482441d 287 uLCD.printf("Counter = %d \n",counter);
yoshua0207 1:5bf45482441d 288 counter++;
yoshua0207 1:5bf45482441d 289 // wait(0.2);
yoshua0207 0:31128b41490c 290 }
yoshua0207 1:5bf45482441d 291 while(difference >2 && timer.read_ms() < static_cast<int>(seconds))
yoshua0207 0:31128b41490c 292 {
yoshua0207 0:31128b41490c 293 mA.speed(0.4);
yoshua0207 0:31128b41490c 294 mB.speed(0.5);
yoshua0207 0:31128b41490c 295 imu.readMag();
yoshua0207 0:31128b41490c 296 heading = static_cast<int> (imu.calcHeading());
yoshua0207 0:31128b41490c 297 difference = heading - finalDegree;
yoshua0207 0:31128b41490c 298 uLCD.locate(1,3);
yoshua0207 1:5bf45482441d 299 uLCD.printf("Difference : %d \n",difference);
yoshua0207 1:5bf45482441d 300 uLCD.locate(1,4);
yoshua0207 1:5bf45482441d 301 uLCD.printf("Turning Left \n");
yoshua0207 1:5bf45482441d 302 uLCD.locate(1,5);
yoshua0207 1:5bf45482441d 303 uLCD.printf("Counter = %d \n",counter);
yoshua0207 1:5bf45482441d 304 counter++;
yoshua0207 1:5bf45482441d 305 // wait(0.2);
yoshua0207 0:31128b41490c 306 }
yoshua0207 0:31128b41490c 307 }
yoshua0207 0:31128b41490c 308 mA.speed(0);
yoshua0207 0:31128b41490c 309 mB.speed(0);
yoshua0207 0:31128b41490c 310
yoshua0207 0:31128b41490c 311 }