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@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?

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 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 }