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@3:f41855c51aaf, 2016-11-04 (annotated)

Committer:
mmittal8
Date:
Fri Nov 04 00:11:18 2016 +0000
Revision:
3:f41855c51aaf
Parent:
1:4a490de4c2a4 
Robot Position Control

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 3:f41855c51aaf 13 Motor RightM(p26, p27, 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 3:f41855c51aaf 29 PwmOut speaker(p25);
mmittal8 3:f41855c51aaf 30
mmittal8 1:4a490de4c2a4 31 int countl = 0, countr = 0;
mmittal8 3:f41855c51aaf 32 int xx=0,yy=0;
mmittal8 3:f41855c51aaf 33
mmittal8 3:f41855c51aaf 34 float note1 = 1568.0, note2 = 1396.9, note3 = 1244.5;
4180_1 0:e693d5bf0a25 35 // Calculate pitch, roll, and heading.
4180_1 0:e693d5bf0a25 36 // Pitch/roll calculations taken from this app note:
4180_1 0:e693d5bf0a25 37 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
4180_1 0:e693d5bf0a25 38 // Heading calculations taken from this app note:
4180_1 0:e693d5bf0a25 39 // 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 40 float getHeading(float mx, float my, float mz)
4180_1 0:e693d5bf0a25 41 {
4180_1 0:e693d5bf0a25 42 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
4180_1 0:e693d5bf0a25 43 mx = -mx;
4180_1 0:e693d5bf0a25 44 float heading;
4180_1 0:e693d5bf0a25 45 if (my == 0.0)
4180_1 0:e693d5bf0a25 46 heading = (mx < 0.0) ? 180.0 : 0.0;
4180_1 0:e693d5bf0a25 47 else
4180_1 0:e693d5bf0a25 48 heading = atan2(mx, my)*360.0/(2.0*PI);
4180_1 0:e693d5bf0a25 49 heading -= DECLINATION; //correct for geo location
4180_1 0:e693d5bf0a25 50 if(heading>180.0) heading = heading - 360.0;
4180_1 0:e693d5bf0a25 51 else if(heading<-180.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 52 else if(heading<0.0) heading = 360.0 + heading;
mmittal8 1:4a490de4c2a4 53 heading = fabs(heading);
mmittal8 1:4a490de4c2a4 54
mmittal8 1:4a490de4c2a4 55 return heading;
mmittal8 1:4a490de4c2a4 56 }
4180_1 0:e693d5bf0a25 57
mmittal8 1:4a490de4c2a4 58 float getHead(){
mmittal8 1:4a490de4c2a4 59 while(!IMU.magAvailable(X_AXIS));
mmittal8 1:4a490de4c2a4 60 IMU.readMag();
mmittal8 1:4a490de4c2a4 61 return getHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
mmittal8 1:4a490de4c2a4 62 }
4180_1 0:e693d5bf0a25 63
mmittal8 1:4a490de4c2a4 64 void leftM_count() {
mmittal8 1:4a490de4c2a4 65 countl++;
mmittal8 1:4a490de4c2a4 66 }
mmittal8 1:4a490de4c2a4 67 void rightM_count() {
mmittal8 1:4a490de4c2a4 68 countr++;
4180_1 0:e693d5bf0a25 69 }
4180_1 0:e693d5bf0a25 70
mmittal8 1:4a490de4c2a4 71 void move(float dist, int dir = 1){
mmittal8 1:4a490de4c2a4 72 led1 = 1;
mmittal8 3:f41855c51aaf 73
mmittal8 3:f41855c51aaf 74 speaker.period(1.0/note1);
mmittal8 3:f41855c51aaf 75 speaker =0.25;
mmittal8 3:f41855c51aaf 76 wait(0.3);
mmittal8 3:f41855c51aaf 77 speaker.period(1.0/note2);
mmittal8 3:f41855c51aaf 78 wait(0.3);
mmittal8 3:f41855c51aaf 79 speaker=0.0;
mmittal8 3:f41855c51aaf 80
mmittal8 1:4a490de4c2a4 81 leftPid.setInputLimits(0, 1000);
mmittal8 1:4a490de4c2a4 82 leftPid.setOutputLimits(0.0, 0.9);
mmittal8 1:4a490de4c2a4 83 leftPid.setMode(AUTO_MODE);
mmittal8 1:4a490de4c2a4 84 rightPid.setInputLimits(0, 1000);
mmittal8 1:4a490de4c2a4 85 rightPid.setOutputLimits(0.0, 0.9);
mmittal8 1:4a490de4c2a4 86 rightPid.setMode(AUTO_MODE);
mmittal8 1:4a490de4c2a4 87
mmittal8 1:4a490de4c2a4 88 int leftPulses = 0;
mmittal8 1:4a490de4c2a4 89 int leftPrevPulses = 0;
mmittal8 1:4a490de4c2a4 90 float leftVelocity = 0.0;
mmittal8 1:4a490de4c2a4 91 int rightPulses = 0;
mmittal8 1:4a490de4c2a4 92 int rightPrevPulses = 0;
mmittal8 1:4a490de4c2a4 93 float rightVelocity = 0.0;
mmittal8 1:4a490de4c2a4 94
mmittal8 1:4a490de4c2a4 95 wait(1);
mmittal8 1:4a490de4c2a4 96
mmittal8 1:4a490de4c2a4 97 leftPid.setSetPoint(750);
mmittal8 1:4a490de4c2a4 98 rightPid.setSetPoint(750);
mmittal8 1:4a490de4c2a4 99
mmittal8 1:4a490de4c2a4 100 while ((leftPulses < dist) || (rightPulses < dist)) {
mmittal8 1:4a490de4c2a4 101 leftPulses = countl;
mmittal8 1:4a490de4c2a4 102 leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
mmittal8 1:4a490de4c2a4 103 leftPrevPulses = leftPulses;
mmittal8 1:4a490de4c2a4 104 leftPid.setProcessValue(leftVelocity);
mmittal8 1:4a490de4c2a4 105 LeftM.speed(leftPid.compute()*dir);
mmittal8 1:4a490de4c2a4 106
mmittal8 1:4a490de4c2a4 107 rightPulses = countr;
mmittal8 1:4a490de4c2a4 108 rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
mmittal8 1:4a490de4c2a4 109 rightPrevPulses = rightPulses;
mmittal8 1:4a490de4c2a4 110 rightPid.setProcessValue(rightVelocity);
mmittal8 1:4a490de4c2a4 111 RightM.speed(rightPid.compute()*dir);
mmittal8 1:4a490de4c2a4 112
mmittal8 1:4a490de4c2a4 113 wait(0.01);
mmittal8 1:4a490de4c2a4 114 }
mmittal8 1:4a490de4c2a4 115 countl = 0;
mmittal8 1:4a490de4c2a4 116 countr = 0;
mmittal8 1:4a490de4c2a4 117 RightM.speed(0);
mmittal8 1:4a490de4c2a4 118 LeftM.speed(0);
mmittal8 1:4a490de4c2a4 119 led1 = 0;
mmittal8 1:4a490de4c2a4 120 wait(1);
mmittal8 1:4a490de4c2a4 121 }
4180_1 0:e693d5bf0a25 122
mmittal8 1:4a490de4c2a4 123 void turn(int direction){
mmittal8 1:4a490de4c2a4 124 led2 = 1;
mmittal8 3:f41855c51aaf 125
mmittal8 3:f41855c51aaf 126 speaker.period(1.0/note2);
mmittal8 3:f41855c51aaf 127 speaker =0.25;
mmittal8 3:f41855c51aaf 128 wait(0.3);
mmittal8 3:f41855c51aaf 129 speaker.period(1.0/note1);
mmittal8 3:f41855c51aaf 130 wait(0.3);
mmittal8 3:f41855c51aaf 131 speaker=0.0;
mmittal8 3:f41855c51aaf 132
mmittal8 1:4a490de4c2a4 133 float head = 0, oldHead = 0,newHead = 0;
mmittal8 1:4a490de4c2a4 134 oldHead = getHead();
mmittal8 1:4a490de4c2a4 135 head = oldHead;
mmittal8 1:4a490de4c2a4 136 //printf("Old Heading: %f\n\r", oldHead);
mmittal8 1:4a490de4c2a4 137 if (direction == 1){
mmittal8 1:4a490de4c2a4 138 newHead = oldHead + 90;
mmittal8 1:4a490de4c2a4 139 if (newHead > 360)
mmittal8 1:4a490de4c2a4 140 newHead -= 360;
mmittal8 1:4a490de4c2a4 141 }
mmittal8 1:4a490de4c2a4 142 else{
mmittal8 1:4a490de4c2a4 143 newHead = oldHead - 90;
mmittal8 1:4a490de4c2a4 144 if (newHead < 0)
mmittal8 1:4a490de4c2a4 145 newHead += 360;
mmittal8 1:4a490de4c2a4 146 }
mmittal8 1:4a490de4c2a4 147 //printf("New Heading: %f\n\r", newHead);
mmittal8 1:4a490de4c2a4 148
mmittal8 1:4a490de4c2a4 149 while ((head < newHead - 1) || (head > newHead + 1)){
mmittal8 1:4a490de4c2a4 150 if ((head < newHead && (newHead - head < 180)) || (head - newHead > 180)){
mmittal8 1:4a490de4c2a4 151 LeftM.speed(0.5);
mmittal8 1:4a490de4c2a4 152 RightM.speed(-0.5);
mmittal8 1:4a490de4c2a4 153 head = getHead();
mmittal8 1:4a490de4c2a4 154 //printf("heading: %f\n\r", head);
mmittal8 1:4a490de4c2a4 155 wait(0.01);
mmittal8 1:4a490de4c2a4 156 }
mmittal8 1:4a490de4c2a4 157 else if ((head > newHead && (head-newHead < 180)) || (newHead - head > 180)){
mmittal8 1:4a490de4c2a4 158 LeftM.speed(-0.5);
mmittal8 1:4a490de4c2a4 159 RightM.speed(0.5);
mmittal8 1:4a490de4c2a4 160 head = getHead();
mmittal8 1:4a490de4c2a4 161 //printf("heading: %f\n\r", head);
mmittal8 1:4a490de4c2a4 162 wait(0.01);
mmittal8 1:4a490de4c2a4 163 }
mmittal8 1:4a490de4c2a4 164 }
mmittal8 1:4a490de4c2a4 165 LeftM.speed(0);
mmittal8 1:4a490de4c2a4 166 RightM.speed(0);
mmittal8 1:4a490de4c2a4 167 led2 = 0;
mmittal8 1:4a490de4c2a4 168 countl = 0;
mmittal8 1:4a490de4c2a4 169 countr = 0;
mmittal8 3:f41855c51aaf 170 wait(0.5);
mmittal8 3:f41855c51aaf 171 }
mmittal8 3:f41855c51aaf 172
mmittal8 3:f41855c51aaf 173
mmittal8 3:f41855c51aaf 174 void coord(int x, int y){
mmittal8 3:f41855c51aaf 175 if (y>yy)
mmittal8 3:f41855c51aaf 176 move((y-yy)*250,1);
mmittal8 3:f41855c51aaf 177 else if (y<yy)
mmittal8 3:f41855c51aaf 178 move((yy-y)*250,-1);
mmittal8 3:f41855c51aaf 179
mmittal8 3:f41855c51aaf 180 if (x>xx){
mmittal8 3:f41855c51aaf 181 turn(1);
mmittal8 3:f41855c51aaf 182 move((x-xx)*250,1);
mmittal8 3:f41855c51aaf 183 turn(-1);
mmittal8 3:f41855c51aaf 184 }
mmittal8 3:f41855c51aaf 185 else if (x<xx){
mmittal8 3:f41855c51aaf 186 turn(-1);
mmittal8 3:f41855c51aaf 187 move((xx-x)*250,1);
mmittal8 3:f41855c51aaf 188 turn(1);
mmittal8 3:f41855c51aaf 189 }
mmittal8 3:f41855c51aaf 190
mmittal8 3:f41855c51aaf 191 xx=x;
mmittal8 3:f41855c51aaf 192 yy=y;
mmittal8 1:4a490de4c2a4 193 }
4180_1 0:e693d5bf0a25 194
4180_1 0:e693d5bf0a25 195 int main()
4180_1 0:e693d5bf0a25 196 {
mmittal8 1:4a490de4c2a4 197 lhes.mode(PullUp);
mmittal8 1:4a490de4c2a4 198 rhes.mode(PullUp);
mmittal8 1:4a490de4c2a4 199 lhes.rise(&leftM_count);
mmittal8 1:4a490de4c2a4 200 rhes.rise(&rightM_count);
mmittal8 3:f41855c51aaf 201 xx=0;
mmittal8 3:f41855c51aaf 202 yy=0;
4180_1 0:e693d5bf0a25 203 IMU.begin();
4180_1 0:e693d5bf0a25 204 if (!IMU.begin()) {
mmittal8 1:4a490de4c2a4 205 led1 = 1;
mmittal8 1:4a490de4c2a4 206 led2 = 1;
mmittal8 1:4a490de4c2a4 207 led3 = 1;
4180_1 0:e693d5bf0a25 208 }
4180_1 0:e693d5bf0a25 209 IMU.calibrate(1);
mmittal8 1:4a490de4c2a4 210
mmittal8 1:4a490de4c2a4 211 led3 = 1;
4180_1 0:e693d5bf0a25 212 IMU.calibrateMag(0);
mmittal8 1:4a490de4c2a4 213 led3 = 0;
mmittal8 1:4a490de4c2a4 214 wait(2);
mmittal8 3:f41855c51aaf 215
mmittal8 3:f41855c51aaf 216 // while (1){
mmittal8 3:f41855c51aaf 217 // pc.printf("Heading: %f\n\r", getHead());
mmittal8 3:f41855c51aaf 218 // wait (1);
mmittal8 3:f41855c51aaf 219 // }
mmittal8 1:4a490de4c2a4 220
mmittal8 3:f41855c51aaf 221 coord(0,2);
mmittal8 3:f41855c51aaf 222 coord(2,2);
mmittal8 3:f41855c51aaf 223 coord(2,0);
mmittal8 3:f41855c51aaf 224 coord(0,0);
mmittal8 1:4a490de4c2a4 225
mmittal8 3:f41855c51aaf 226 speaker.period(1.0/note3);
mmittal8 3:f41855c51aaf 227 speaker =0.25;
mmittal8 3:f41855c51aaf 228 wait(1);
mmittal8 3:f41855c51aaf 229 speaker=0.0;
4180_1 0:e693d5bf0a25 230 }
4180_1 0:e693d5bf0a25 231