Robot Position Control using IMU and Hall effect sensor
Dependencies: LSM9DS1_Library_cal Motordriver PID mbed
Fork of Lab4 by
main.cpp
00001 #include "mbed.h" 00002 #include "motordriver.h" //Library to drive motors 00003 #include "PID.h" //PID library for distance control 00004 #include "LSM9DS1.h" 00005 #define PI 3.14159 00006 // Earth's magnetic field varies by location. Add or subtract 00007 // a declination to get a more accurate heading. Calculate 00008 // your's here: 00009 // http://www.ngdc.noaa.gov/geomag-web/#declination 00010 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA. 00011 00012 Motor LeftM(p21, p22, p23,1); // pwm, fwd, rev 00013 Motor RightM(p26, p27, p24,1); 00014 00015 InterruptIn rhes(p15); 00016 InterruptIn lhes(p16); 00017 00018 PID leftPid(1.0, 0.0, 0.0,0.01); //Kc, Ti, Td 00019 PID rightPid(1.0, 0.0, 0.0,0.01); //Kc, Ti, Td 00020 00021 Serial pc(USBTX, USBRX); 00022 00023 DigitalOut led1(LED1); 00024 DigitalOut led2(LED2); 00025 DigitalOut led3(LED3); 00026 00027 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); 00028 00029 PwmOut speaker(p25); 00030 00031 int countl = 0, countr = 0; 00032 int xx=0,yy=0; 00033 00034 float note1 = 1568.0, note2 = 1396.9, note3 = 1244.5; 00035 // Calculate pitch, roll, and heading. 00036 // Pitch/roll calculations taken from this app note: 00037 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1 00038 // Heading calculations taken from this app note: 00039 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf 00040 float getHeading(float mx, float my, float mz) 00041 { 00042 // touchy trig stuff to use arctan to get compass heading (scale is 0..360) 00043 mx = -mx; 00044 float heading; 00045 if (my == 0.0) 00046 heading = (mx < 0.0) ? 180.0 : 0.0; 00047 else 00048 heading = atan2(mx, my)*360.0/(2.0*PI); 00049 heading -= DECLINATION; //correct for geo location 00050 if(heading>180.0) heading = heading - 360.0; 00051 else if(heading<-180.0) heading = 360.0 + heading; 00052 else if(heading<0.0) heading = 360.0 + heading; 00053 heading = fabs(heading); 00054 00055 return heading; 00056 } 00057 00058 float getHead(){ 00059 while(!IMU.magAvailable(X_AXIS)); 00060 IMU.readMag(); 00061 return getHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); 00062 } 00063 00064 void leftM_count() { 00065 countl++; 00066 } 00067 void rightM_count() { 00068 countr++; 00069 } 00070 00071 void move(float dist, int dir = 1){ 00072 led1 = 1; 00073 00074 speaker.period(1.0/note1); 00075 speaker =0.25; 00076 wait(0.3); 00077 speaker.period(1.0/note2); 00078 wait(0.3); 00079 speaker=0.0; 00080 00081 leftPid.setInputLimits(0, 1000); 00082 leftPid.setOutputLimits(0.0, 0.9); 00083 leftPid.setMode(AUTO_MODE); 00084 rightPid.setInputLimits(0, 1000); 00085 rightPid.setOutputLimits(0.0, 0.9); 00086 rightPid.setMode(AUTO_MODE); 00087 00088 int leftPulses = 0; 00089 int leftPrevPulses = 0; 00090 float leftVelocity = 0.0; 00091 int rightPulses = 0; 00092 int rightPrevPulses = 0; 00093 float rightVelocity = 0.0; 00094 00095 wait(1); 00096 00097 leftPid.setSetPoint(750); 00098 rightPid.setSetPoint(750); 00099 00100 while ((leftPulses < dist) || (rightPulses < dist)) { 00101 leftPulses = countl; 00102 leftVelocity = (leftPulses - leftPrevPulses) / 0.01; 00103 leftPrevPulses = leftPulses; 00104 leftPid.setProcessValue(leftVelocity); 00105 LeftM.speed(leftPid.compute()*dir); 00106 00107 rightPulses = countr; 00108 rightVelocity = (rightPulses - rightPrevPulses) / 0.01; 00109 rightPrevPulses = rightPulses; 00110 rightPid.setProcessValue(rightVelocity); 00111 RightM.speed(rightPid.compute()*dir); 00112 00113 wait(0.01); 00114 } 00115 countl = 0; 00116 countr = 0; 00117 RightM.speed(0); 00118 LeftM.speed(0); 00119 led1 = 0; 00120 wait(1); 00121 } 00122 00123 void turn(int direction){ 00124 led2 = 1; 00125 00126 speaker.period(1.0/note2); 00127 speaker =0.25; 00128 wait(0.3); 00129 speaker.period(1.0/note1); 00130 wait(0.3); 00131 speaker=0.0; 00132 00133 float head = 0, oldHead = 0,newHead = 0; 00134 oldHead = getHead(); 00135 head = oldHead; 00136 //printf("Old Heading: %f\n\r", oldHead); 00137 if (direction == 1){ 00138 newHead = oldHead + 90; 00139 if (newHead > 360) 00140 newHead -= 360; 00141 } 00142 else{ 00143 newHead = oldHead - 90; 00144 if (newHead < 0) 00145 newHead += 360; 00146 } 00147 //printf("New Heading: %f\n\r", newHead); 00148 00149 while ((head < newHead - 1) || (head > newHead + 1)){ 00150 if ((head < newHead && (newHead - head < 180)) || (head - newHead > 180)){ 00151 LeftM.speed(0.5); 00152 RightM.speed(-0.5); 00153 head = getHead(); 00154 //printf("heading: %f\n\r", head); 00155 wait(0.01); 00156 } 00157 else if ((head > newHead && (head-newHead < 180)) || (newHead - head > 180)){ 00158 LeftM.speed(-0.5); 00159 RightM.speed(0.5); 00160 head = getHead(); 00161 //printf("heading: %f\n\r", head); 00162 wait(0.01); 00163 } 00164 } 00165 LeftM.speed(0); 00166 RightM.speed(0); 00167 led2 = 0; 00168 countl = 0; 00169 countr = 0; 00170 wait(0.5); 00171 } 00172 00173 00174 void coord(int x, int y){ 00175 if (y>yy) 00176 move((y-yy)*250,1); 00177 else if (y<yy) 00178 move((yy-y)*250,-1); 00179 00180 if (x>xx){ 00181 turn(1); 00182 move((x-xx)*250,1); 00183 turn(-1); 00184 } 00185 else if (x<xx){ 00186 turn(-1); 00187 move((xx-x)*250,1); 00188 turn(1); 00189 } 00190 00191 xx=x; 00192 yy=y; 00193 } 00194 00195 int main() 00196 { 00197 lhes.mode(PullUp); 00198 rhes.mode(PullUp); 00199 lhes.rise(&leftM_count); 00200 rhes.rise(&rightM_count); 00201 xx=0; 00202 yy=0; 00203 IMU.begin(); 00204 if (!IMU.begin()) { 00205 led1 = 1; 00206 led2 = 1; 00207 led3 = 1; 00208 } 00209 IMU.calibrate(1); 00210 00211 led3 = 1; 00212 IMU.calibrateMag(0); 00213 led3 = 0; 00214 wait(2); 00215 00216 // while (1){ 00217 // pc.printf("Heading: %f\n\r", getHead()); 00218 // wait (1); 00219 // } 00220 00221 coord(0,2); 00222 coord(2,2); 00223 coord(2,0); 00224 coord(0,0); 00225 00226 speaker.period(1.0/note3); 00227 speaker =0.25; 00228 wait(1); 00229 speaker=0.0; 00230 } 00231
Generated on Fri Aug 12 2022 15:44:55 by 1.7.2