Turning Robot with IMU and calibrate the straight movement
Dependencies: 4DGL-uLCD-SE LSM9DS0 Motor PinDetect mbed
main.cpp
- Committer:
- yoshua0207
- Date:
- 2015-10-20
- Revision:
- 1:5bf45482441d
- Parent:
- 0:31128b41490c
File content as of revision 1:5bf45482441d:
#include "mbed.h" #include "PinDetect.h" #include "uLCD_4DGL.h" #include "math.h" #include "LSM9DS0.h" #include "Motor.h" #define PI 3.14159265 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW // refresh time. set to 500 for part 2 and 50 for part 4 #define REFRESH_TIME_MS 500 Serial pc(USBTX, USBRX); // Verify that the pin assignments below match your breadboard LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); Motor mA(p21, p23, p24); // pwm, fwd, rev LEFT Motor mB(p22, p25, p26); // pwm, fwd, rev RIGHT //speed: 10.44 cm/s // use class to setup pushbuttons pins PinDetect pb1(p16); PinDetect pb2(p17); PinDetect pb3(p18); PinDetect pb4(p19); DigitalOut myLED1(LED1); DigitalOut myLED2(LED2); DigitalOut myLED3(LED3); DigitalOut myLED4(LED4); // use class to setup the Color LCD uLCD_4DGL uLCD(p28, p27, p29); // create a global uLCD object //Timer class Timer timer; //Mode settting enum Modetype { SETX = 0, SETY, GO }; Modetype mode1 = SETX; int settingMode = 0; //Variable int xCoord = 0; int yCoord = 0; float distance = 0; float angle = 0; float seconds = 0; int startDegree = 0; int turningDegree = 0; int finalDegree = 0; int heading = 0; int degree = 0; int initialHeading = 0; int state = 0; int difference = 0; int counter = 0; void setup() { uLCD.cls(); uint16_t status = imu.begin(); //Make sure communication is working } //When pb1 push increment the coordinate void pb1_hit_callback (void) { if (mode1 == SETX) xCoord++; else if (mode1 == SETY) yCoord++; myLED1 = 1; myLED2 = 0; myLED3 = 0; myLED4 = 0; } //When pb2 push decrement the coordinate void pb2_hit_callback (void) { if (mode1 == SETX) xCoord--; else if (mode1 == SETY) yCoord--; myLED2 = 1; myLED1 = 0; myLED3 = 0; myLED4 = 0; } //When pb3 push increment the mode void pb3_hit_callback (void) { mode1 = static_cast<Modetype>((static_cast<int>(mode1) + 1) % 3); myLED3 = 1; myLED1 = 0; myLED2 = 0; myLED4 = 0; } //When pb4 push decrement the mode void pb4_hit_callback (void) { if (mode1 == SETY) { mode1 = SETX; } myLED4 = 1; myLED1 = 0; myLED2 = 0; myLED3 = 0; } void turnRight(void) { imu.readMag(); myLED3 = 1; // uLCD.locate(1,5); // uLCD.printf("Turning Right \n"); mA.speed(0.25); mB.speed(-0.25); heading = static_cast<int> (imu.calcHeading()); // uLCD.printf("H : %d \n",heading); degree = heading - initialHeading; degree = turningDegree - degree; uLCD.locate(1,5); uLCD.printf("Degree Left = %d \n",degree); uLCD.printf("Heading = %d \n",heading); } void turnLeft(void) { imu.readMag(); myLED3 = 1; // uLCD.locate(1,5); // uLCD.printf("Turning Left \n"); mA.speed(-0.25); mB.speed(0.25); heading = static_cast<int> (imu.calcHeading()); // uLCD.printf("H : %d \n",heading); degree = heading - initialHeading; degree = turningDegree - degree; uLCD.locate(1,5); uLCD.printf("Degree Left = %d \n",degree); uLCD.printf("Heading = %d \n",heading); } int main() { uLCD.cls(); // Use internal pullups for the three pushbuttons pb1.mode(PullUp); pb2.mode(PullUp); pb3.mode(PullUp); pb4.mode(PullUp); // Delay for initial pullup to take effect wait(.01); // Setup Interrupt callback functions for a pb hit pb1.attach_deasserted(&pb1_hit_callback); pb2.attach_deasserted(&pb2_hit_callback); pb3.attach_deasserted(&pb3_hit_callback); pb4.attach_deasserted(&pb4_hit_callback); // Start sampling pb inputs using interrupts pb1.setSampleFrequency(); pb2.setSampleFrequency(); pb3.setSampleFrequency(); pb4.setSampleFrequency(); mode1 = SETX; setup(); xCoord = 0; yCoord = 0; state = 1; //Setting mode while(settingMode == 0) { uLCD.locate(1,1); if (mode1 == SETX) { uLCD.printf("Setting X Coord"); } else if (mode1 == SETY) { uLCD.printf("Setting Y Coord"); } else if (mode1 == GO) { uLCD.printf("Running Now"); } uLCD.locate(4,5); uLCD.printf("X-Coord: %d \n\r", xCoord); uLCD.locate(4,7); uLCD.printf("Y-Coord: %d \n\r", yCoord); if (mode1 == GO) { distance = xCoord*xCoord + yCoord*yCoord; distance = sqrt(distance); float totalDistance = distance * 30.48; seconds = totalDistance / 23.18 * 1000; imu.readMag(); initialHeading = static_cast<int>(imu.calcHeading()); angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI; finalDegree = initialHeading + angle; uLCD.locate(1,9); uLCD.printf("Distance: %5.2F Feet \n\r", distance); uLCD.locate(1,11); uLCD.printf("Turning: %5.2F Degree \n\r", angle); settingMode = 1; } } //end first while wait(2); //Turning mode while(settingMode == 1) { uLCD.cls(); // uLCD.printf("Setting mode 1"); imu.readMag(); initialHeading = static_cast<int> (imu.calcHeading()); uLCD.locate(1,3); uLCD.printf("Initial : %d \n",initialHeading); startDegree = heading; turningDegree = angle; degree = static_cast<int> (imu.calcHeading()) - initialHeading; if (turningDegree > 0) { while (degree > -2 && degree < 180) { turnRight(); mA.speed(0); mB.speed(0); wait(0.4); } settingMode = 2; } else { while (degree < 2 && degree > -180 ) { turnLeft(); mA.speed(0); mB.speed(0); wait(0.4); } settingMode = 2; } } wait(2); imu.readMag(); initialHeading = static_cast<int> (imu.calcHeading()); timer.start(); uLCD.cls(); while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2) { // uLCD.printf("Time : %d \n",timer.read_ms()); mA.speed(0.4); mB.speed(0.4); imu.readMag(); heading = static_cast<int> (imu.calcHeading()); difference = heading - finalDegree; uLCD.locate(5,6); uLCD.printf("Go!"); while( difference <-2 &&timer.read_ms() < static_cast<int>(seconds) ) { mA.speed(0.5); mB.speed(0.4); imu.readMag(); heading = static_cast<int> (imu.calcHeading()); difference = heading - finalDegree; uLCD.locate(1,3); uLCD.printf("Difference : %d \n",difference); uLCD.locate(1,4); uLCD.printf("Turning Right \n"); uLCD.locate(1,5); uLCD.printf("Counter = %d \n",counter); counter++; // wait(0.2); } while(difference >2 && timer.read_ms() < static_cast<int>(seconds)) { mA.speed(0.4); mB.speed(0.5); imu.readMag(); heading = static_cast<int> (imu.calcHeading()); difference = heading - finalDegree; uLCD.locate(1,3); uLCD.printf("Difference : %d \n",difference); uLCD.locate(1,4); uLCD.printf("Turning Left \n"); uLCD.locate(1,5); uLCD.printf("Counter = %d \n",counter); counter++; // wait(0.2); } } mA.speed(0); mB.speed(0); }