main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "Wireless.h"
00004 //#include "USBKeyboard.h"
00005 //#include "MahonyAHRS.h"
00006 #define PI 3.14159
00007 // Earth's magnetic field varies by location. Add or subtract
00008 // a declination to get a more accurate heading. Calculate
00009 // your's here:
00010 // http://www.ngdc.noaa.gov/geomag-web/#declination
00011 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00012 
00013 DigitalOut led1(LED1);
00014 DigitalOut led2(LED2);
00015 DigitalOut led3(LED3);
00016 DigitalOut led4(LED4);
00017 Serial pc(USBTX, USBRX);
00018 DigitalIn pb1(p17);
00019 
00020 
00021 
00022 
00023 
00024 
00025 float correctHeading(float currHeading, float forward)
00026 {
00027     float newHeading = currHeading - forward;
00028     if(newHeading < 0) newHeading = 360 + newHeading;
00029     return newHeading;
00030 }
00031 
00032 float printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00033 {
00034     float roll = atan2(ay, az);
00035     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00036 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00037     mx = -mx;
00038     float heading;
00039     if (my == 0.0)
00040         heading = (mx < 0.0) ? 180.0 : 0.0;
00041     else
00042         heading = atan2(mx, my)*360.0/(2.0*PI);
00043     //pc.printf("heading atan=%f \n\r",heading);
00044     heading -= DECLINATION; //correct for geo location
00045     if(heading>180.0) heading = heading - 360.0;
00046     else if(heading<-180.0) heading = 360.0 + heading;
00047     else if(heading<0.0) heading = 360.0  + heading;
00048 
00049 
00050     // Convert everything from radians to degrees:
00051     //heading *= 180.0 / PI;
00052     pitch *= 180.0 / PI;
00053     roll  *= 180.0 / PI;
00054 
00055     //pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00056     //pc.printf("Magnetic Heading: %f degress\n\r",heading);
00057     return abs(heading);
00058 }
00059 
00060 bool isWalking = false;
00061 
00062 Ticker walkingTimer;
00063 Ticker resetStart;
00064 WirelessModule wireless(p9, p10, FOOT_STEP);
00065 float ax ;
00066 float ay ;
00067 float az ;
00068 float gx ;
00069 float gy ;
00070 float gz ;
00071 float mx ;
00072 float my ;
00073 float mz ;
00074 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
00075 
00076 void printStop()
00077 {
00078     // pc.printf("stop\n\r");
00079     wireless.sendDirection(DIR_NONE);
00080     isWalking = false;
00081 }
00082 
00083 
00084 
00085 
00086 int main()
00087 {
00088 
00089     pb1.mode(PullUp);
00090     IMU.begin();
00091     float forward;
00092     if (!IMU.begin()) {
00093         pc.printf("Failed to communicate with LSM9DS1.\n");
00094     }
00095     led4 = 1;
00096     IMU.calibrate(1);
00097     led4 = 0;
00098     wait(0.5);
00099     led1 = 1;
00100     led4 = 1;
00101     IMU.calibrateMag(0);
00102     led4 = 0;
00103     led2 = 1;
00104     pc.printf("Press button to set forward direction");
00105     while(pb1 == 1) {
00106         IMU.readMag();
00107         IMU.readAccel();
00108         ax = IMU.calcAccel(IMU.ax);
00109         ay = IMU.calcAccel(IMU.ay);
00110         az = IMU.calcAccel(IMU.az);
00111         gx = IMU.calcGyro(IMU.gx);
00112         gy = IMU.calcGyro(IMU.gy);
00113         gz = IMU.calcGyro(IMU.gz);
00114         mx = IMU.calcMag(IMU.mx);
00115         my = IMU.calcMag(IMU.my);
00116         mz = IMU.calcMag(IMU.mz);
00117         forward = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00118                                 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));;
00119     }
00120     led3 = 1;
00121     while(1) {
00122         while(!IMU.tempAvailable());
00123         IMU.readTemp();
00124         while(!IMU.magAvailable(X_AXIS));
00125         IMU.readMag();
00126         while(!IMU.accelAvailable());
00127         IMU.readAccel();
00128         while(!IMU.gyroAvailable());
00129         IMU.readGyro();
00130 
00131         if(abs(IMU.calcGyro(IMU.gy)) > 100) {
00132 
00133             //Calculate heading relative to forward direction
00134             float currHeading = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00135             currHeading = correctHeading(currHeading, forward);
00136             pc.printf("heading: %f\n\r", currHeading);
00137       
00138             //Start timeout to detect when stopped walking
00139             walkingTimer.attach(printStop, 0.3);
00140 
00141             //Detect direction and send command to main mbed
00142             if((currHeading > 225 && currHeading < 315) && !isWalking) {
00143                 pc.printf("left\n\r");
00144                 wireless.sendDirection(DIR_LEFT);
00145 
00146                 isWalking = true;
00147             } else if((currHeading > 45 && currHeading < 135) && !isWalking) {
00148                 pc.printf("right\n\r");
00149                 wireless.sendDirection(DIR_RIGHT);
00150                 isWalking = true;
00151             } else if((currHeading > 135 && currHeading < 225) && !isWalking) {
00152                 pc.printf("down\n\r");
00153                 wireless.sendDirection(DIR_DOWN);
00154                 isWalking = true;
00155             } else if((currHeading > 315 || currHeading < 45) && !isWalking) {
00156                 pc.printf("up\n\r");
00157                 wireless.sendDirection(DIR_UP);
00158 
00159                 isWalking = true;
00160             }
00161         }
00162     }
00163 }
00164