IMU.cpp

00001 #include "IMU.h"
00002 
00003 #define PI 3.14159
00004 
00005 IMU::IMU() {
00006     I = new LSM9DS1(p9, p10, 0xD6, 0x3C);
00007     I->begin();
00008     if (!I->begin()) {
00009         printf("Failed to communicate with LSM9DS1.\n");
00010     }
00011     I->calibrate(1);
00012     //bool state;
00013     //Do we want to give state a starting value?
00014     ax01 = I->calcAccel(I->ax);
00015     ay01 = I->calcAccel(I->ay);
00016     az01 = I->calcAccel(I->az);
00017     ax0 = ax01;
00018     ay0 = ay01;
00019     az0 = az01;
00020 }
00021 
00022 void IMU::trackBed()
00023 {
00024     if (state != 0) {
00025         return;
00026     }
00027     else {
00028         while(state == 0) {
00029             while(!I->accelAvailable());
00030             I->readAccel();
00031 
00032              // current a = <ax,ay,az>
00033             ax = I->calcAccel(I->ax);
00034             ay = I->calcAccel(I->ay);
00035             az = I->calcAccel(I->az);
00036             // theta in degrees between a0 and a
00037             theta = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
00038         }
00039     }
00040 }
00041 
00042 void IMU::trackHeading()
00043 {
00044     if (state != 1) {
00045         return;
00046     }
00047     else {
00048         while (state == 1) {
00049 
00050             while(!I->accelAvailable());
00051 
00052             I->readAccel();
00053             ax0 = I->calcAccel(I->ax);
00054             ay0 = I->calcAccel(I->ay);
00055             az0 = I->calcAccel(I->az);
00056         }
00057     }
00058 }