Craig Raslawski / Mbed 2 deprecated IMURoomba

Roomba that displays room data to a C# GUI

Dependencies:   LSM9DS1_Library_cal mbed

Fork of 4180Lab2Part4 by James Plager

main.cpp@2:516dd8a72972, 2017-04-30 (annotated)

Committer:
CRaslawski
Date:
Sun Apr 30 18:09:43 2017 +0000
Revision:
2:516dd8a72972
Parent:
1:aeb42bbdac27 
started IMU->BT

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
4180_1 0:e693d5bf0a25 4 // Earth's magnetic field varies by location. Add or subtract
4180_1 0:e693d5bf0a25 5 // a declination to get a more accurate heading. Calculate
4180_1 0:e693d5bf0a25 6 // your's here:
4180_1 0:e693d5bf0a25 7 // http://www.ngdc.noaa.gov/geomag-web/#declination
4180_1 0:e693d5bf0a25 8 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
4180_1 0:e693d5bf0a25 9
CRaslawski 2:516dd8a72972 10 //Serial pc(USBTX, USBRX);
CRaslawski 2:516dd8a72972 11 RawSerial pc(USBTX, USBRX);
CRaslawski 2:516dd8a72972 12 RawSerial dev(p28,p27); //tx, rx
4180_1 0:e693d5bf0a25 13 DigitalOut myled(LED1);
CRaslawski 2:516dd8a72972 14 DigitalOut led2(LED2);
CRaslawski 2:516dd8a72972 15 DigitalOut led4(LED4);
4180_1 0:e693d5bf0a25 16 // Calculate pitch, roll, and heading.
4180_1 0:e693d5bf0a25 17 // Pitch/roll calculations taken from this app note:
4180_1 0:e693d5bf0a25 18 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
4180_1 0:e693d5bf0a25 19 // Heading calculations taken from this app note:
4180_1 0:e693d5bf0a25 20 // 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 21 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
4180_1 0:e693d5bf0a25 22 {
4180_1 0:e693d5bf0a25 23 float roll = atan2(ay, az);
4180_1 0:e693d5bf0a25 24 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
4180_1 0:e693d5bf0a25 25 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
4180_1 0:e693d5bf0a25 26 mx = -mx;
4180_1 0:e693d5bf0a25 27 float heading;
4180_1 0:e693d5bf0a25 28 if (my == 0.0)
4180_1 0:e693d5bf0a25 29 heading = (mx < 0.0) ? 180.0 : 0.0;
4180_1 0:e693d5bf0a25 30 else
4180_1 0:e693d5bf0a25 31 heading = atan2(mx, my)*360.0/(2.0*PI);
4180_1 0:e693d5bf0a25 32 //pc.printf("heading atan=%f \n\r",heading);
4180_1 0:e693d5bf0a25 33 heading -= DECLINATION; //correct for geo location
4180_1 0:e693d5bf0a25 34 if(heading>180.0) heading = heading - 360.0;
4180_1 0:e693d5bf0a25 35 else if(heading<-180.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 36 else if(heading<0.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 37
4180_1 0:e693d5bf0a25 38
4180_1 0:e693d5bf0a25 39 // Convert everything from radians to degrees:
4180_1 0:e693d5bf0a25 40 //heading *= 180.0 / PI;
4180_1 0:e693d5bf0a25 41 pitch *= 180.0 / PI;
4180_1 0:e693d5bf0a25 42 roll *= 180.0 / PI;
4180_1 0:e693d5bf0a25 43
CRaslawski 2:516dd8a72972 44 //~pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
CRaslawski 2:516dd8a72972 45 //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
4180_1 0:e693d5bf0a25 46 }
4180_1 0:e693d5bf0a25 47
CRaslawski 2:516dd8a72972 48 /*
CRaslawski 2:516dd8a72972 49 void dev_recv()
CRaslawski 2:516dd8a72972 50 {
CRaslawski 2:516dd8a72972 51 led2 = !led2;
CRaslawski 2:516dd8a72972 52 while(dev.readable()) {
CRaslawski 2:516dd8a72972 53 pc.putc(dev.getc());
CRaslawski 2:516dd8a72972 54 }
CRaslawski 2:516dd8a72972 55 }
CRaslawski 2:516dd8a72972 56
CRaslawski 2:516dd8a72972 57 void pc_recv()
CRaslawski 2:516dd8a72972 58 {
CRaslawski 2:516dd8a72972 59 led4 = !led4;
CRaslawski 2:516dd8a72972 60 while(pc.readable()) {
CRaslawski 2:516dd8a72972 61 dev.putc(pc.getc());
CRaslawski 2:516dd8a72972 62 }
CRaslawski 2:516dd8a72972 63 }*/
4180_1 0:e693d5bf0a25 64
4180_1 0:e693d5bf0a25 65 int main()
4180_1 0:e693d5bf0a25 66 {
CRaslawski 2:516dd8a72972 67 //IMU setup
jplager3 1:aeb42bbdac27 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);
CRaslawski 2:516dd8a72972 75
CRaslawski 2:516dd8a72972 76 //bluetooth setup
CRaslawski 2:516dd8a72972 77 pc.baud(9600);
CRaslawski 2:516dd8a72972 78 dev.baud(9600);
CRaslawski 2:516dd8a72972 79
CRaslawski 2:516dd8a72972 80 /*pc.attach(&pc_recv, Serial::RxIrq);
CRaslawski 2:516dd8a72972 81 dev.attach(&dev_recv, Serial::RxIrq);*/
CRaslawski 2:516dd8a72972 82
4180_1 0:e693d5bf0a25 83 while(1) {
jplager3 1:aeb42bbdac27 84
4180_1 0:e693d5bf0a25 85 while(!IMU.magAvailable(X_AXIS));
4180_1 0:e693d5bf0a25 86 IMU.readMag();
4180_1 0:e693d5bf0a25 87 while(!IMU.accelAvailable());
4180_1 0:e693d5bf0a25 88 IMU.readAccel();
4180_1 0:e693d5bf0a25 89 while(!IMU.gyroAvailable());
4180_1 0:e693d5bf0a25 90 IMU.readGyro();
CRaslawski 2:516dd8a72972 91 pc.puts(" X axis Y axis Z axis\n\r");
CRaslawski 2:516dd8a72972 92 dev.puts(" X axis Y axis Z axis\n\r");
CRaslawski 2:516dd8a72972 93 //pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
CRaslawski 2:516dd8a72972 94 //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
CRaslawski 2:516dd8a72972 95 //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 96 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
4180_1 0:e693d5bf0a25 97 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
4180_1 0:e693d5bf0a25 98 myled = 1;
4180_1 0:e693d5bf0a25 99 wait(0.5);
4180_1 0:e693d5bf0a25 100 myled = 0;
4180_1 0:e693d5bf0a25 101 wait(0.5);
jplager3 1:aeb42bbdac27 102
CRaslawski 2:516dd8a72972 103
CRaslawski 2:516dd8a72972 104 //uLCD.filled_circle(floor(128*oldtempX), floor(128*oldtempY), 6, 0x000000); // erase old bubble
CRaslawski 2:516dd8a72972 105
jplager3 1:aeb42bbdac27 106 //draw filled circle based on info from IMU
jplager3 1:aeb42bbdac27 107 //draw new bubble
CRaslawski 2:516dd8a72972 108 //uLCD.filled_circle(floor(128*tempX), floor(128*tempY), 6, 0xFFFFFF);
jplager3 1:aeb42bbdac27 109
4180_1 0:e693d5bf0a25 110 }
4180_1 0:e693d5bf0a25 111 }
4180_1 0:e693d5bf0a25 112