Robot Position Control using IMU and Hall effect sensor
Dependencies: LSM9DS1_Library_cal Motordriver PID mbed
Fork of Lab4 by
main.cpp@1:4a490de4c2a4, 2016-11-03 (annotated)
- Committer:
- mmittal8
- Date:
- Thu Nov 03 14:07:48 2016 +0000
- Revision:
- 1:4a490de4c2a4
- Parent:
- 0:e693d5bf0a25
- Child:
- 3:f41855c51aaf
Shadow Robot Position Control utilizing LSM9DS1 IMU chip and Wheel Encoder kit from SparkFun.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
4180_1 | 0:e693d5bf0a25 | 1 | #include "mbed.h" |
mmittal8 | 1:4a490de4c2a4 | 2 | #include "motordriver.h" //Library to drive motors |
mmittal8 | 1:4a490de4c2a4 | 3 | #include "PID.h" //PID library for distance control |
4180_1 | 0:e693d5bf0a25 | 4 | #include "LSM9DS1.h" |
4180_1 | 0:e693d5bf0a25 | 5 | #define PI 3.14159 |
4180_1 | 0:e693d5bf0a25 | 6 | // Earth's magnetic field varies by location. Add or subtract |
4180_1 | 0:e693d5bf0a25 | 7 | // a declination to get a more accurate heading. Calculate |
4180_1 | 0:e693d5bf0a25 | 8 | // your's here: |
4180_1 | 0:e693d5bf0a25 | 9 | // http://www.ngdc.noaa.gov/geomag-web/#declination |
4180_1 | 0:e693d5bf0a25 | 10 | #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA. |
4180_1 | 0:e693d5bf0a25 | 11 | |
mmittal8 | 1:4a490de4c2a4 | 12 | Motor LeftM(p21, p22, p23,1); // pwm, fwd, rev |
mmittal8 | 1:4a490de4c2a4 | 13 | Motor RightM(p26, p25, p24,1); |
mmittal8 | 1:4a490de4c2a4 | 14 | |
mmittal8 | 1:4a490de4c2a4 | 15 | InterruptIn rhes(p15); |
mmittal8 | 1:4a490de4c2a4 | 16 | InterruptIn lhes(p16); |
mmittal8 | 1:4a490de4c2a4 | 17 | |
mmittal8 | 1:4a490de4c2a4 | 18 | PID leftPid(1.0, 0.0, 0.0,0.01); //Kc, Ti, Td |
mmittal8 | 1:4a490de4c2a4 | 19 | PID rightPid(1.0, 0.0, 0.0,0.01); //Kc, Ti, Td |
mmittal8 | 1:4a490de4c2a4 | 20 | |
4180_1 | 0:e693d5bf0a25 | 21 | Serial pc(USBTX, USBRX); |
mmittal8 | 1:4a490de4c2a4 | 22 | |
mmittal8 | 1:4a490de4c2a4 | 23 | DigitalOut led1(LED1); |
mmittal8 | 1:4a490de4c2a4 | 24 | DigitalOut led2(LED2); |
mmittal8 | 1:4a490de4c2a4 | 25 | DigitalOut led3(LED3); |
mmittal8 | 1:4a490de4c2a4 | 26 | |
mmittal8 | 1:4a490de4c2a4 | 27 | LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); |
mmittal8 | 1:4a490de4c2a4 | 28 | |
mmittal8 | 1:4a490de4c2a4 | 29 | int countl = 0, countr = 0; |
mmittal8 | 1:4a490de4c2a4 | 30 | |
4180_1 | 0:e693d5bf0a25 | 31 | // Calculate pitch, roll, and heading. |
4180_1 | 0:e693d5bf0a25 | 32 | // Pitch/roll calculations taken from this app note: |
4180_1 | 0:e693d5bf0a25 | 33 | // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1 |
4180_1 | 0:e693d5bf0a25 | 34 | // Heading calculations taken from this app note: |
4180_1 | 0:e693d5bf0a25 | 35 | // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf |
mmittal8 | 1:4a490de4c2a4 | 36 | float getHeading(float mx, float my, float mz) |
4180_1 | 0:e693d5bf0a25 | 37 | { |
4180_1 | 0:e693d5bf0a25 | 38 | // touchy trig stuff to use arctan to get compass heading (scale is 0..360) |
4180_1 | 0:e693d5bf0a25 | 39 | mx = -mx; |
4180_1 | 0:e693d5bf0a25 | 40 | float heading; |
4180_1 | 0:e693d5bf0a25 | 41 | if (my == 0.0) |
4180_1 | 0:e693d5bf0a25 | 42 | heading = (mx < 0.0) ? 180.0 : 0.0; |
4180_1 | 0:e693d5bf0a25 | 43 | else |
4180_1 | 0:e693d5bf0a25 | 44 | heading = atan2(mx, my)*360.0/(2.0*PI); |
4180_1 | 0:e693d5bf0a25 | 45 | heading -= DECLINATION; //correct for geo location |
4180_1 | 0:e693d5bf0a25 | 46 | if(heading>180.0) heading = heading - 360.0; |
4180_1 | 0:e693d5bf0a25 | 47 | else if(heading<-180.0) heading = 360.0 + heading; |
4180_1 | 0:e693d5bf0a25 | 48 | else if(heading<0.0) heading = 360.0 + heading; |
mmittal8 | 1:4a490de4c2a4 | 49 | heading = fabs(heading); |
mmittal8 | 1:4a490de4c2a4 | 50 | |
mmittal8 | 1:4a490de4c2a4 | 51 | return heading; |
mmittal8 | 1:4a490de4c2a4 | 52 | } |
4180_1 | 0:e693d5bf0a25 | 53 | |
mmittal8 | 1:4a490de4c2a4 | 54 | float getHead(){ |
mmittal8 | 1:4a490de4c2a4 | 55 | while(!IMU.magAvailable(X_AXIS)); |
mmittal8 | 1:4a490de4c2a4 | 56 | IMU.readMag(); |
mmittal8 | 1:4a490de4c2a4 | 57 | return getHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); |
mmittal8 | 1:4a490de4c2a4 | 58 | } |
4180_1 | 0:e693d5bf0a25 | 59 | |
mmittal8 | 1:4a490de4c2a4 | 60 | void leftM_count() { |
mmittal8 | 1:4a490de4c2a4 | 61 | countl++; |
mmittal8 | 1:4a490de4c2a4 | 62 | } |
mmittal8 | 1:4a490de4c2a4 | 63 | void rightM_count() { |
mmittal8 | 1:4a490de4c2a4 | 64 | countr++; |
4180_1 | 0:e693d5bf0a25 | 65 | } |
4180_1 | 0:e693d5bf0a25 | 66 | |
mmittal8 | 1:4a490de4c2a4 | 67 | void move(float dist, int dir = 1){ |
mmittal8 | 1:4a490de4c2a4 | 68 | led1 = 1; |
mmittal8 | 1:4a490de4c2a4 | 69 | leftPid.setInputLimits(0, 1000); |
mmittal8 | 1:4a490de4c2a4 | 70 | leftPid.setOutputLimits(0.0, 0.9); |
mmittal8 | 1:4a490de4c2a4 | 71 | leftPid.setMode(AUTO_MODE); |
mmittal8 | 1:4a490de4c2a4 | 72 | rightPid.setInputLimits(0, 1000); |
mmittal8 | 1:4a490de4c2a4 | 73 | rightPid.setOutputLimits(0.0, 0.9); |
mmittal8 | 1:4a490de4c2a4 | 74 | rightPid.setMode(AUTO_MODE); |
mmittal8 | 1:4a490de4c2a4 | 75 | |
mmittal8 | 1:4a490de4c2a4 | 76 | int leftPulses = 0; |
mmittal8 | 1:4a490de4c2a4 | 77 | int leftPrevPulses = 0; |
mmittal8 | 1:4a490de4c2a4 | 78 | float leftVelocity = 0.0; |
mmittal8 | 1:4a490de4c2a4 | 79 | int rightPulses = 0; |
mmittal8 | 1:4a490de4c2a4 | 80 | int rightPrevPulses = 0; |
mmittal8 | 1:4a490de4c2a4 | 81 | float rightVelocity = 0.0; |
mmittal8 | 1:4a490de4c2a4 | 82 | |
mmittal8 | 1:4a490de4c2a4 | 83 | wait(1); |
mmittal8 | 1:4a490de4c2a4 | 84 | |
mmittal8 | 1:4a490de4c2a4 | 85 | leftPid.setSetPoint(750); |
mmittal8 | 1:4a490de4c2a4 | 86 | rightPid.setSetPoint(750); |
mmittal8 | 1:4a490de4c2a4 | 87 | |
mmittal8 | 1:4a490de4c2a4 | 88 | while ((leftPulses < dist) || (rightPulses < dist)) { |
mmittal8 | 1:4a490de4c2a4 | 89 | leftPulses = countl; |
mmittal8 | 1:4a490de4c2a4 | 90 | leftVelocity = (leftPulses - leftPrevPulses) / 0.01; |
mmittal8 | 1:4a490de4c2a4 | 91 | leftPrevPulses = leftPulses; |
mmittal8 | 1:4a490de4c2a4 | 92 | leftPid.setProcessValue(leftVelocity); |
mmittal8 | 1:4a490de4c2a4 | 93 | LeftM.speed(leftPid.compute()*dir); |
mmittal8 | 1:4a490de4c2a4 | 94 | |
mmittal8 | 1:4a490de4c2a4 | 95 | rightPulses = countr; |
mmittal8 | 1:4a490de4c2a4 | 96 | rightVelocity = (rightPulses - rightPrevPulses) / 0.01; |
mmittal8 | 1:4a490de4c2a4 | 97 | rightPrevPulses = rightPulses; |
mmittal8 | 1:4a490de4c2a4 | 98 | rightPid.setProcessValue(rightVelocity); |
mmittal8 | 1:4a490de4c2a4 | 99 | RightM.speed(rightPid.compute()*dir); |
mmittal8 | 1:4a490de4c2a4 | 100 | |
mmittal8 | 1:4a490de4c2a4 | 101 | wait(0.01); |
mmittal8 | 1:4a490de4c2a4 | 102 | } |
mmittal8 | 1:4a490de4c2a4 | 103 | countl = 0; |
mmittal8 | 1:4a490de4c2a4 | 104 | countr = 0; |
mmittal8 | 1:4a490de4c2a4 | 105 | RightM.speed(0); |
mmittal8 | 1:4a490de4c2a4 | 106 | LeftM.speed(0); |
mmittal8 | 1:4a490de4c2a4 | 107 | led1 = 0; |
mmittal8 | 1:4a490de4c2a4 | 108 | wait(1); |
mmittal8 | 1:4a490de4c2a4 | 109 | } |
4180_1 | 0:e693d5bf0a25 | 110 | |
mmittal8 | 1:4a490de4c2a4 | 111 | void turn(int direction){ |
mmittal8 | 1:4a490de4c2a4 | 112 | led2 = 1; |
mmittal8 | 1:4a490de4c2a4 | 113 | float head = 0, oldHead = 0,newHead = 0; |
mmittal8 | 1:4a490de4c2a4 | 114 | oldHead = getHead(); |
mmittal8 | 1:4a490de4c2a4 | 115 | head = oldHead; |
mmittal8 | 1:4a490de4c2a4 | 116 | //printf("Old Heading: %f\n\r", oldHead); |
mmittal8 | 1:4a490de4c2a4 | 117 | if (direction == 1){ |
mmittal8 | 1:4a490de4c2a4 | 118 | newHead = oldHead + 90; |
mmittal8 | 1:4a490de4c2a4 | 119 | if (newHead > 360) |
mmittal8 | 1:4a490de4c2a4 | 120 | newHead -= 360; |
mmittal8 | 1:4a490de4c2a4 | 121 | } |
mmittal8 | 1:4a490de4c2a4 | 122 | else{ |
mmittal8 | 1:4a490de4c2a4 | 123 | newHead = oldHead - 90; |
mmittal8 | 1:4a490de4c2a4 | 124 | if (newHead < 0) |
mmittal8 | 1:4a490de4c2a4 | 125 | newHead += 360; |
mmittal8 | 1:4a490de4c2a4 | 126 | } |
mmittal8 | 1:4a490de4c2a4 | 127 | //printf("New Heading: %f\n\r", newHead); |
mmittal8 | 1:4a490de4c2a4 | 128 | |
mmittal8 | 1:4a490de4c2a4 | 129 | while ((head < newHead - 1) || (head > newHead + 1)){ |
mmittal8 | 1:4a490de4c2a4 | 130 | if ((head < newHead && (newHead - head < 180)) || (head - newHead > 180)){ |
mmittal8 | 1:4a490de4c2a4 | 131 | LeftM.speed(0.5); |
mmittal8 | 1:4a490de4c2a4 | 132 | RightM.speed(-0.5); |
mmittal8 | 1:4a490de4c2a4 | 133 | head = getHead(); |
mmittal8 | 1:4a490de4c2a4 | 134 | //printf("heading: %f\n\r", head); |
mmittal8 | 1:4a490de4c2a4 | 135 | wait(0.01); |
mmittal8 | 1:4a490de4c2a4 | 136 | } |
mmittal8 | 1:4a490de4c2a4 | 137 | else if ((head > newHead && (head-newHead < 180)) || (newHead - head > 180)){ |
mmittal8 | 1:4a490de4c2a4 | 138 | LeftM.speed(-0.5); |
mmittal8 | 1:4a490de4c2a4 | 139 | RightM.speed(0.5); |
mmittal8 | 1:4a490de4c2a4 | 140 | head = getHead(); |
mmittal8 | 1:4a490de4c2a4 | 141 | //printf("heading: %f\n\r", head); |
mmittal8 | 1:4a490de4c2a4 | 142 | wait(0.01); |
mmittal8 | 1:4a490de4c2a4 | 143 | } |
mmittal8 | 1:4a490de4c2a4 | 144 | } |
mmittal8 | 1:4a490de4c2a4 | 145 | LeftM.speed(0); |
mmittal8 | 1:4a490de4c2a4 | 146 | RightM.speed(0); |
mmittal8 | 1:4a490de4c2a4 | 147 | led2 = 0; |
mmittal8 | 1:4a490de4c2a4 | 148 | countl = 0; |
mmittal8 | 1:4a490de4c2a4 | 149 | countr = 0; |
mmittal8 | 1:4a490de4c2a4 | 150 | wait(1); |
mmittal8 | 1:4a490de4c2a4 | 151 | } |
4180_1 | 0:e693d5bf0a25 | 152 | |
4180_1 | 0:e693d5bf0a25 | 153 | int main() |
4180_1 | 0:e693d5bf0a25 | 154 | { |
mmittal8 | 1:4a490de4c2a4 | 155 | lhes.mode(PullUp); |
mmittal8 | 1:4a490de4c2a4 | 156 | rhes.mode(PullUp); |
mmittal8 | 1:4a490de4c2a4 | 157 | lhes.rise(&leftM_count); |
mmittal8 | 1:4a490de4c2a4 | 158 | rhes.rise(&rightM_count); |
4180_1 | 0:e693d5bf0a25 | 159 | IMU.begin(); |
4180_1 | 0:e693d5bf0a25 | 160 | if (!IMU.begin()) { |
mmittal8 | 1:4a490de4c2a4 | 161 | led1 = 1; |
mmittal8 | 1:4a490de4c2a4 | 162 | led2 = 1; |
mmittal8 | 1:4a490de4c2a4 | 163 | led3 = 1; |
4180_1 | 0:e693d5bf0a25 | 164 | } |
4180_1 | 0:e693d5bf0a25 | 165 | IMU.calibrate(1); |
mmittal8 | 1:4a490de4c2a4 | 166 | |
mmittal8 | 1:4a490de4c2a4 | 167 | led3 = 1; |
4180_1 | 0:e693d5bf0a25 | 168 | IMU.calibrateMag(0); |
mmittal8 | 1:4a490de4c2a4 | 169 | led3 = 0; |
mmittal8 | 1:4a490de4c2a4 | 170 | wait(2); |
mmittal8 | 1:4a490de4c2a4 | 171 | /* |
mmittal8 | 1:4a490de4c2a4 | 172 | while (1){ |
mmittal8 | 1:4a490de4c2a4 | 173 | pc.printf("Heading: %f\n\r", getHead()); |
mmittal8 | 1:4a490de4c2a4 | 174 | wait (1); |
4180_1 | 0:e693d5bf0a25 | 175 | } |
mmittal8 | 1:4a490de4c2a4 | 176 | */ |
mmittal8 | 1:4a490de4c2a4 | 177 | move(500); |
mmittal8 | 1:4a490de4c2a4 | 178 | wait(0.5); |
mmittal8 | 1:4a490de4c2a4 | 179 | turn(1); |
mmittal8 | 1:4a490de4c2a4 | 180 | wait(0.5); |
mmittal8 | 1:4a490de4c2a4 | 181 | |
mmittal8 | 1:4a490de4c2a4 | 182 | move(500); |
mmittal8 | 1:4a490de4c2a4 | 183 | wait(0.5); |
mmittal8 | 1:4a490de4c2a4 | 184 | turn(1); |
mmittal8 | 1:4a490de4c2a4 | 185 | wait(0.5); |
mmittal8 | 1:4a490de4c2a4 | 186 | |
mmittal8 | 1:4a490de4c2a4 | 187 | move(500); |
mmittal8 | 1:4a490de4c2a4 | 188 | wait(0.5); |
mmittal8 | 1:4a490de4c2a4 | 189 | turn(1); |
mmittal8 | 1:4a490de4c2a4 | 190 | wait(0.5); |
mmittal8 | 1:4a490de4c2a4 | 191 | |
mmittal8 | 1:4a490de4c2a4 | 192 | move(500); |
4180_1 | 0:e693d5bf0a25 | 193 | } |
4180_1 | 0:e693d5bf0a25 | 194 |