Robot Position Control using IMU and Hall effect sensor
Dependencies: LSM9DS1_Library_cal Motordriver PID mbed
Fork of Lab4 by
Diff: main.cpp
- Revision:
- 1:4a490de4c2a4
- Parent:
- 0:e693d5bf0a25
- Child:
- 3:f41855c51aaf
--- a/main.cpp Wed Feb 03 18:47:07 2016 +0000 +++ b/main.cpp Thu Nov 03 14:07:48 2016 +0000 @@ -1,4 +1,6 @@ #include "mbed.h" +#include "motordriver.h" //Library to drive motors +#include "PID.h" //PID library for distance control #include "LSM9DS1.h" #define PI 3.14159 // Earth's magnetic field varies by location. Add or subtract @@ -7,17 +9,32 @@ // http://www.ngdc.noaa.gov/geomag-web/#declination #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA. -DigitalOut myled(LED1); +Motor LeftM(p21, p22, p23,1); // pwm, fwd, rev +Motor RightM(p26, p25, p24,1); + +InterruptIn rhes(p15); +InterruptIn lhes(p16); + +PID leftPid(1.0, 0.0, 0.0,0.01); //Kc, Ti, Td +PID rightPid(1.0, 0.0, 0.0,0.01); //Kc, Ti, Td + Serial pc(USBTX, USBRX); + +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); + +LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); + +int countl = 0, countr = 0; + // Calculate pitch, roll, and heading. // Pitch/roll calculations taken from this app note: // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1 // Heading calculations taken from this app note: // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf -void printAttitude(float ax, float ay, float az, float mx, float my, float mz) +float getHeading(float mx, float my, float mz) { - float roll = atan2(ay, az); - float pitch = atan2(-ax, sqrt(ay * ay + az * az)); // touchy trig stuff to use arctan to get compass heading (scale is 0..360) mx = -mx; float heading; @@ -25,55 +42,153 @@ heading = (mx < 0.0) ? 180.0 : 0.0; else heading = atan2(mx, my)*360.0/(2.0*PI); - //pc.printf("heading atan=%f \n\r",heading); heading -= DECLINATION; //correct for geo location if(heading>180.0) heading = heading - 360.0; else if(heading<-180.0) heading = 360.0 + heading; else if(heading<0.0) heading = 360.0 + heading; - + heading = fabs(heading); + + return heading; +} - // Convert everything from radians to degrees: - //heading *= 180.0 / PI; - pitch *= 180.0 / PI; - roll *= 180.0 / PI; +float getHead(){ + while(!IMU.magAvailable(X_AXIS)); + IMU.readMag(); + return getHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); +} - pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll); - pc.printf("Magnetic Heading: %f degress\n\r",heading); +void leftM_count() { + countl++; +} +void rightM_count() { + countr++; } +void move(float dist, int dir = 1){ + led1 = 1; + leftPid.setInputLimits(0, 1000); + leftPid.setOutputLimits(0.0, 0.9); + leftPid.setMode(AUTO_MODE); + rightPid.setInputLimits(0, 1000); + rightPid.setOutputLimits(0.0, 0.9); + rightPid.setMode(AUTO_MODE); + + int leftPulses = 0; + int leftPrevPulses = 0; + float leftVelocity = 0.0; + int rightPulses = 0; + int rightPrevPulses = 0; + float rightVelocity = 0.0; + + wait(1); + + leftPid.setSetPoint(750); + rightPid.setSetPoint(750); + + while ((leftPulses < dist) || (rightPulses < dist)) { + leftPulses = countl; + leftVelocity = (leftPulses - leftPrevPulses) / 0.01; + leftPrevPulses = leftPulses; + leftPid.setProcessValue(leftVelocity); + LeftM.speed(leftPid.compute()*dir); + + rightPulses = countr; + rightVelocity = (rightPulses - rightPrevPulses) / 0.01; + rightPrevPulses = rightPulses; + rightPid.setProcessValue(rightVelocity); + RightM.speed(rightPid.compute()*dir); + + wait(0.01); + } + countl = 0; + countr = 0; + RightM.speed(0); + LeftM.speed(0); + led1 = 0; + wait(1); +} - +void turn(int direction){ + led2 = 1; + float head = 0, oldHead = 0,newHead = 0; + oldHead = getHead(); + head = oldHead; + //printf("Old Heading: %f\n\r", oldHead); + if (direction == 1){ + newHead = oldHead + 90; + if (newHead > 360) + newHead -= 360; + } + else{ + newHead = oldHead - 90; + if (newHead < 0) + newHead += 360; + } + //printf("New Heading: %f\n\r", newHead); + + while ((head < newHead - 1) || (head > newHead + 1)){ + if ((head < newHead && (newHead - head < 180)) || (head - newHead > 180)){ + LeftM.speed(0.5); + RightM.speed(-0.5); + head = getHead(); + //printf("heading: %f\n\r", head); + wait(0.01); + } + else if ((head > newHead && (head-newHead < 180)) || (newHead - head > 180)){ + LeftM.speed(-0.5); + RightM.speed(0.5); + head = getHead(); + //printf("heading: %f\n\r", head); + wait(0.01); + } + } + LeftM.speed(0); + RightM.speed(0); + led2 = 0; + countl = 0; + countr = 0; + wait(1); +} int main() { - //LSM9DS1 lol(p9, p10, 0x6B, 0x1E); - LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); + lhes.mode(PullUp); + rhes.mode(PullUp); + lhes.rise(&leftM_count); + rhes.rise(&rightM_count); IMU.begin(); if (!IMU.begin()) { - pc.printf("Failed to communicate with LSM9DS1.\n"); + led1 = 1; + led2 = 1; + led3 = 1; } IMU.calibrate(1); + + led3 = 1; IMU.calibrateMag(0); - while(1) { - while(!IMU.tempAvailable()); - IMU.readTemp(); - while(!IMU.magAvailable(X_AXIS)); - IMU.readMag(); - while(!IMU.accelAvailable()); - IMU.readAccel(); - while(!IMU.gyroAvailable()); - IMU.readGyro(); - pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0); - pc.printf(" X axis Y axis Z axis\n\r"); - pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz)); - pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az)); - pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); - printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), - IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); - myled = 1; - wait(0.5); - myled = 0; - wait(0.5); + led3 = 0; + wait(2); + /* + while (1){ + pc.printf("Heading: %f\n\r", getHead()); + wait (1); } + */ + move(500); + wait(0.5); + turn(1); + wait(0.5); + + move(500); + wait(0.5); + turn(1); + wait(0.5); + + move(500); + wait(0.5); + turn(1); + wait(0.5); + + move(500); }