Jeremy Cai / Mbed 2 deprecated 4180_Lab2_Bubble

CodeShare

Dependencies:   4DGL-uLCD-SE LSM9DS1_Library_cal mbed

Fork of LSM9DS1_Demo_wCal by jim hamblen

main.cpp@1:736628381fb9, 2016-09-25 (annotated)

Committer:
jeremycai3721
Date:
Sun Sep 25 19:00:05 2016 +0000
Revision:
1:736628381fb9
Parent:
0:e693d5bf0a25 
CodeShare

Who changed what in which revision?

UserRevisionLine numberNew contents of line
4180_1 0:e693d5bf0a25 1 #include "mbed.h"
4180_1 0:e693d5bf0a25 2 #include "LSM9DS1.h"
4180_1 0:e693d5bf0a25 3 #define PI 3.14159
jeremycai3721 1:736628381fb9 4
jeremycai3721 1:736628381fb9 5 #include "uLCD_4DGL.h"
jeremycai3721 1:736628381fb9 6
jeremycai3721 1:736628381fb9 7 uLCD_4DGL uLCD(p28,p27,p30); // serial tx, serial rx, reset pin;
jeremycai3721 1:736628381fb9 8
4180_1 0:e693d5bf0a25 9 // Earth's magnetic field varies by location. Add or subtract
4180_1 0:e693d5bf0a25 10 // a declination to get a more accurate heading. Calculate
4180_1 0:e693d5bf0a25 11 // your's here:
4180_1 0:e693d5bf0a25 12 // http://www.ngdc.noaa.gov/geomag-web/#declination
4180_1 0:e693d5bf0a25 13 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
4180_1 0:e693d5bf0a25 14
4180_1 0:e693d5bf0a25 15 DigitalOut myled(LED1);
4180_1 0:e693d5bf0a25 16 Serial pc(USBTX, USBRX);
jeremycai3721 1:736628381fb9 17
jeremycai3721 1:736628381fb9 18
jeremycai3721 1:736628381fb9 19 float accel_x, accel_y, accel_z;
jeremycai3721 1:736628381fb9 20 int outer_radius = 30;
jeremycai3721 1:736628381fb9 21 int radius = 3;
jeremycai3721 1:736628381fb9 22 int x,y;
jeremycai3721 1:736628381fb9 23 //Serial pc(p13, p14);
4180_1 0:e693d5bf0a25 24 // Calculate pitch, roll, and heading.
4180_1 0:e693d5bf0a25 25 // Pitch/roll calculations taken from this app note:
4180_1 0:e693d5bf0a25 26 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
4180_1 0:e693d5bf0a25 27 // Heading calculations taken from this app note:
4180_1 0:e693d5bf0a25 28 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
4180_1 0:e693d5bf0a25 29 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
4180_1 0:e693d5bf0a25 30 {
4180_1 0:e693d5bf0a25 31 float roll = atan2(ay, az);
4180_1 0:e693d5bf0a25 32 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
4180_1 0:e693d5bf0a25 33 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
4180_1 0:e693d5bf0a25 34 mx = -mx;
4180_1 0:e693d5bf0a25 35 float heading;
4180_1 0:e693d5bf0a25 36 if (my == 0.0)
4180_1 0:e693d5bf0a25 37 heading = (mx < 0.0) ? 180.0 : 0.0;
4180_1 0:e693d5bf0a25 38 else
4180_1 0:e693d5bf0a25 39 heading = atan2(mx, my)*360.0/(2.0*PI);
4180_1 0:e693d5bf0a25 40 //pc.printf("heading atan=%f \n\r",heading);
4180_1 0:e693d5bf0a25 41 heading -= DECLINATION; //correct for geo location
4180_1 0:e693d5bf0a25 42 if(heading>180.0) heading = heading - 360.0;
4180_1 0:e693d5bf0a25 43 else if(heading<-180.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 44 else if(heading<0.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 45
4180_1 0:e693d5bf0a25 46
4180_1 0:e693d5bf0a25 47 // Convert everything from radians to degrees:
4180_1 0:e693d5bf0a25 48 //heading *= 180.0 / PI;
4180_1 0:e693d5bf0a25 49 pitch *= 180.0 / PI;
4180_1 0:e693d5bf0a25 50 roll *= 180.0 / PI;
4180_1 0:e693d5bf0a25 51
4180_1 0:e693d5bf0a25 52 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
4180_1 0:e693d5bf0a25 53 pc.printf("Magnetic Heading: %f degress\n\r",heading);
4180_1 0:e693d5bf0a25 54 }
4180_1 0:e693d5bf0a25 55
4180_1 0:e693d5bf0a25 56
4180_1 0:e693d5bf0a25 57
4180_1 0:e693d5bf0a25 58
4180_1 0:e693d5bf0a25 59 int main()
4180_1 0:e693d5bf0a25 60 {
jeremycai3721 1:736628381fb9 61 set_time(1474451138); // Set RTC time to Wed, 21 Sep 2016 9:46am
jeremycai3721 1:736628381fb9 62 //time_t seconds = time(NULL);
jeremycai3721 1:736628381fb9 63 //printf("Time as seconds since January 1, 1970 = %d\n\r", seconds);
jeremycai3721 1:736628381fb9 64
jeremycai3721 1:736628381fb9 65
jeremycai3721 1:736628381fb9 66
4180_1 0:e693d5bf0a25 67 //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
jeremycai3721 1:736628381fb9 68 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
4180_1 0:e693d5bf0a25 69 IMU.begin();
4180_1 0:e693d5bf0a25 70 if (!IMU.begin()) {
4180_1 0:e693d5bf0a25 71 pc.printf("Failed to communicate with LSM9DS1.\n");
4180_1 0:e693d5bf0a25 72 }
4180_1 0:e693d5bf0a25 73 IMU.calibrate(1);
4180_1 0:e693d5bf0a25 74 IMU.calibrateMag(0);
jeremycai3721 1:736628381fb9 75
4180_1 0:e693d5bf0a25 76 while(1) {
4180_1 0:e693d5bf0a25 77 while(!IMU.tempAvailable());
4180_1 0:e693d5bf0a25 78 IMU.readTemp();
4180_1 0:e693d5bf0a25 79 while(!IMU.magAvailable(X_AXIS));
4180_1 0:e693d5bf0a25 80 IMU.readMag();
4180_1 0:e693d5bf0a25 81 while(!IMU.accelAvailable());
4180_1 0:e693d5bf0a25 82 IMU.readAccel();
4180_1 0:e693d5bf0a25 83 while(!IMU.gyroAvailable());
4180_1 0:e693d5bf0a25 84 IMU.readGyro();
4180_1 0:e693d5bf0a25 85 pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
4180_1 0:e693d5bf0a25 86 pc.printf(" X axis Y axis Z axis\n\r");
4180_1 0:e693d5bf0a25 87 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
4180_1 0:e693d5bf0a25 88 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
4180_1 0:e693d5bf0a25 89 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
4180_1 0:e693d5bf0a25 90 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
4180_1 0:e693d5bf0a25 91 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
4180_1 0:e693d5bf0a25 92 myled = 1;
4180_1 0:e693d5bf0a25 93 wait(0.5);
4180_1 0:e693d5bf0a25 94 myled = 0;
4180_1 0:e693d5bf0a25 95 wait(0.5);
jeremycai3721 1:736628381fb9 96
jeremycai3721 1:736628381fb9 97 while(1) {
jeremycai3721 1:736628381fb9 98
jeremycai3721 1:736628381fb9 99 // 4180 Bubble Level
jeremycai3721 1:736628381fb9 100 while(!IMU.accelAvailable());
jeremycai3721 1:736628381fb9 101 IMU.readAccel();
jeremycai3721 1:736628381fb9 102 accel_x = IMU.calcAccel(IMU.ax);
jeremycai3721 1:736628381fb9 103 accel_y = IMU.calcAccel(IMU.ay);
jeremycai3721 1:736628381fb9 104 accel_z = IMU.calcAccel(IMU.az);
jeremycai3721 1:736628381fb9 105 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
jeremycai3721 1:736628381fb9 106 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jeremycai3721 1:736628381fb9 107 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
jeremycai3721 1:736628381fb9 108
jeremycai3721 1:736628381fb9 109 //draw ball
jeremycai3721 1:736628381fb9 110 x = 64 + 64*accel_x;
jeremycai3721 1:736628381fb9 111 y = 64 + 64*accel_y;
jeremycai3721 1:736628381fb9 112 uLCD.circle(64, 64, outer_radius, RED);
jeremycai3721 1:736628381fb9 113 uLCD.filled_circle(x,y, radius, BLUE);
jeremycai3721 1:736628381fb9 114
jeremycai3721 1:736628381fb9 115 time_t seconds = time(NULL);
jeremycai3721 1:736628381fb9 116
jeremycai3721 1:736628381fb9 117 printf("Time as a basic string = %s", ctime(&seconds));
jeremycai3721 1:736628381fb9 118
jeremycai3721 1:736628381fb9 119 char buffer[32];
jeremycai3721 1:736628381fb9 120 strftime(buffer, 32, "%I:%M %p", localtime(&seconds));
jeremycai3721 1:736628381fb9 121 printf("\rTime as a custom formatted string = %s\n\r", buffer);
jeremycai3721 1:736628381fb9 122
jeremycai3721 1:736628381fb9 123
jeremycai3721 1:736628381fb9 124 uLCD.locate(0,0);
jeremycai3721 1:736628381fb9 125 uLCD.color(RED);
jeremycai3721 1:736628381fb9 126 uLCD.printf("%s", ctime(&seconds));
jeremycai3721 1:736628381fb9 127 wait(0.2);
jeremycai3721 1:736628381fb9 128 uLCD.color(BLACK);
jeremycai3721 1:736628381fb9 129 uLCD.printf("%s", ctime(&seconds));
jeremycai3721 1:736628381fb9 130 //wait(0.1);
jeremycai3721 1:736628381fb9 131
jeremycai3721 1:736628381fb9 132 uLCD.filled_circle(x,y, radius, BLACK);
jeremycai3721 1:736628381fb9 133
jeremycai3721 1:736628381fb9 134 }
jeremycai3721 1:736628381fb9 135
4180_1 0:e693d5bf0a25 136 }
4180_1 0:e693d5bf0a25 137 }
4180_1 0:e693d5bf0a25 138