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