main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "uLCD_4DGL.h"
00004 #define PI 3.14159
00005 // Earth's magnetic field varies by location. Add or subtract
00006 // a declination to get a more accurate heading. Calculate
00007 // your's here:
00008 // http://www.ngdc.noaa.gov/geomag-web/#declination
00009 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00010 
00011 DigitalOut myled(LED1);
00012 Serial pc(USBTX, USBRX);
00013 float accel_x, accel_y, accel_z;
00014 int outer_radius = 9;
00015 int radius = 3;
00016 uLCD_4DGL uLCD(p9,p10,p11); // serial tx, serial rx, reset pin;
00017 int x,y;
00018 
00019 // Calculate pitch, roll, and heading.
00020 // Pitch/roll calculations taken from this app note:
00021 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
00022 // Heading calculations taken from this app note:
00023 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
00024 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00025 {
00026     float roll = atan2(ay, az);
00027     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00028 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00029     mx = -mx;
00030     float heading;
00031     if (my == 0.0)
00032         heading = (mx < 0.0) ? 180.0 : 0.0;
00033     else
00034         heading = atan2(mx, my)*360.0/(2.0*PI);
00035     //pc.printf("heading atan=%f \n\r",heading);
00036     heading -= DECLINATION; //correct for geo location
00037     if(heading>180.0) heading = heading - 360.0;
00038     else if(heading<-180.0) heading = 360.0 + heading;
00039     else if(heading<0.0) heading = 360.0  + heading;
00040 
00041 
00042     // Convert everything from radians to degrees:
00043     //heading *= 180.0 / PI;
00044     pitch *= 180.0 / PI;
00045     roll  *= 180.0 / PI;
00046 
00047     pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00048     pc.printf("Magnetic Heading: %f degress\n\r",heading);
00049 }
00050 
00051 
00052 
00053 
00054 int main()
00055 {
00056     // basic printf demo = 16 by 18 characters on screen
00057     
00058     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
00059     LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
00060     IMU.begin();
00061     if (!IMU.begin()) {
00062         pc.printf("Failed to communicate with LSM9DS1.\n");
00063     }
00064     IMU.calibrate(1);
00065     IMU.calibrateMag(0);
00066     while(1) {
00067         while(!IMU.tempAvailable());
00068         IMU.readTemp();
00069         while(!IMU.magAvailable(X_AXIS));
00070         IMU.readMag();
00071         while(!IMU.accelAvailable());
00072         IMU.readAccel();
00073         while(!IMU.gyroAvailable());
00074         IMU.readGyro();
00075         pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
00076         pc.printf("        X axis    Y axis    Z axis\n\r");
00077         pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
00078         pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00079         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00080         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00081                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00082         myled = 1;
00083         wait(2.5);
00084         myled = 0;
00085         wait(0.5);
00086 
00087         while(1) {
00088             while(!IMU.accelAvailable());
00089             IMU.readAccel();
00090             accel_x = IMU.calcAccel(IMU.ax);
00091             accel_y = IMU.calcAccel(IMU.ay);
00092             accel_z = IMU.calcAccel(IMU.az);
00093             pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00094             //draw ball
00095             x = 64 + 64*accel_x;
00096             y = 64 + 64*accel_y;
00097             uLCD.circle(64, 64, outer_radius, RED);
00098             uLCD.filled_circle(x,y, radius, BLUE);
00099             //bounce off edge walls and slow down a bit?
00100            // if ((x<=radius+1) || (x>=126-radius)) vx = -.90*vx;
00101            // if ((y<=radius+1) || (y>=126-radius)) vy = -.90*vy;
00102             //erase old ball location
00103           //  uLCD.filled_circle(x, y, radius, BLACK);
00104             //move ball
00105           //  fx=fx+vx;
00106            // fy=fy+vy;
00107          //   x=(int)fx;
00108          //   y=(int)fy;
00109             wait(0.1);
00110            uLCD.filled_circle(x,y, radius, BLACK);
00111 
00112         }
00113     }
00114 }
00115