Manan Mittal / Mbed 2 deprecated Lab4

Robot Position Control using IMU and Hall effect sensor

Dependencies:   LSM9DS1_Library_cal Motordriver PID mbed

Fork of Lab4 by Manan Mittal

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?

UserRevisionLine numberNew 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