Jesses_Main.cpp

00001 // <- remove this if you want to code this and comment Georges
00002 #include "mbed.h"
00003 #include "LSM9DS1.h"
00004 #define PI 3.14159
00005 // --------------------------------------------------------------------------------
00006 
00007 //// AnalogOut sine wave complete
00008 //AnalogOut aout(p18);
00009 //
00010 //int main(){
00011 //    const double pi = 3.141592653589793238462;
00012 //    const double amplitude = 1;
00013 //    const double offset = 65535/2;
00014 //    double rads = 0.0;
00015 //    uint16_t sample = 0;
00016 //
00017 //    while(1){
00018 //        // sinewave output
00019 //        for (int i = 0; i < 360; i++) {
00020 //            rads = (pi * i) / 180.0f;
00021 //            sample = (uint16_t)(amplitude * (offset * (cos(rads + pi))) + offset);
00022 //            aout.write_u16(sample);
00023 //        }
00024 //    }
00025 //}
00026 // --------------------------------------------------------------------------------
00027 
00028 //// Sharp IR sensor complete
00029 //
00030 //Serial pc(USBTX, USBRX);
00031 //
00032 //AnalogIn sensor(p15);
00033 //DigitalOut led1(LED1);
00034 //DigitalOut led2(LED2);
00035 //DigitalOut led3(LED3);
00036 //DigitalOut led4(LED4);
00037 //
00038 //int main(){
00039 //  while(1){
00040 //
00041 //     if(sensor > 0.61){
00042 //       led1 = 1;
00043 //       led2 = 1;
00044 //       led3 = 1;
00045 //       led4 = 1;
00046 //     }
00047 //     else if(sensor > 0.46){
00048 //       led1 = 1;
00049 //       led2 = 1;
00050 //       led3 = 1;
00051 //       led4 = 0;
00052 //     }
00053 //     else if(sensor > 0.30){
00054 //       led1 = 1;
00055 //       led2 = 1;
00056 //       led3 = 0;
00057 //       led4 = 0;
00058 //     }
00059 //     else if(sensor > 0.15){
00060 //       led1 = 1;
00061 //       led2 = 0;
00062 //       led3 = 0;
00063 //       led4 = 0;
00064 //     }
00065 //     else{
00066 //       led1 = 0;
00067 //       led2 = 0;
00068 //       led3 = 0;
00069 //       led4 = 0;
00070 //     }
00071 //
00072 ////    pc.printf("%f\n", sensor.read());
00073 ////    wait(.5);
00074 //  }
00075 //}
00076 // --------------------------------------------------------------------------------
00077 
00078 // I2C Bus and USB Virtual Com Port
00079 
00080 // Earth's magnetic field varies by location. Add or subtract
00081 // a declination to get a more accurate heading. Calculate
00082 // your's here:
00083 // http://www.ngdc.noaa.gov/geomag-web/#declination
00084 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00085 
00086 DigitalOut myled(LED1);
00087 Serial pc(USBTX, USBRX);
00088 // Calculate pitch, roll, and heading.
00089 // Pitch/roll calculations taken from this app note:
00090 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
00091 // Heading calculations taken from this app note:
00092 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
00093 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00094 {
00095     float roll = atan2(ay, az);
00096     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00097 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00098     mx = -mx;
00099     float heading;
00100     if (my == 0.0)
00101         heading = (mx < 0.0) ? 180.0 : 0.0;
00102     else
00103         heading = atan2(mx, my)*360.0/(2.0*PI);
00104     //pc.printf("heading atan=%f \n\r",heading);
00105     heading -= DECLINATION; //correct for geo location
00106     if(heading>180.0) heading = heading - 360.0;
00107     else if(heading<-180.0) heading = 360.0 + heading;
00108     else if(heading<0.0) heading = 360.0  + heading;
00109 
00110 
00111     // Convert everything from radians to degrees:
00112     //heading *= 180.0 / PI;
00113     pitch *= 180.0 / PI;
00114     roll  *= 180.0 / PI;
00115 
00116     pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00117     pc.printf("Magnetic Heading: %f degress\n\r",heading);
00118 }
00119 
00120 
00121 
00122 
00123 int main()
00124 {
00125     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
00126     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00127     IMU.begin();
00128     if (!IMU.begin()) {
00129         pc.printf("Failed to communicate with LSM9DS1.\n");
00130     }
00131     IMU.calibrate(1);
00132     IMU.calibrateMag(0);
00133     while(1) {
00134         while(!IMU.tempAvailable());
00135         IMU.readTemp();
00136         while(!IMU.magAvailable(X_AXIS));
00137         IMU.readMag();
00138         while(!IMU.accelAvailable());
00139         IMU.readAccel();
00140         while(!IMU.gyroAvailable());
00141         IMU.readGyro();
00142         pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
00143         pc.printf("        X axis    Y axis    Z axis\n\r");
00144         pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
00145         pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00146         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00147         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00148                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00149         myled = 1;
00150         wait(0.5);
00151         myled = 0;
00152         wait(0.5);
00153     }
00154 }