File content as of revision 4:ab08a70e0d7d:

#include "IMU.h"

#define PI 3.14159

IMU::IMU() {
    I = new LSM9DS1(p9, p10, 0xD6, 0x3C);
    I->begin();
    if (!I->begin()) {
        printf("Failed to communicate with LSM9DS1.\n");
    }
    I->calibrate(1);
    //bool state;
    //Do we want to give state a starting value?
    ax01 = I->calcAccel(I->ax);
    ay01 = I->calcAccel(I->ay);
    az01 = I->calcAccel(I->az);
    ax0 = ax01;
    ay0 = ay01;
    az0 = az01;
}

void IMU::trackBed()
{
    if (state != 0) {
        return;
    }
    else {
        while(state == 0) {
            while(!I->accelAvailable());
            I->readAccel();

             // current a = <ax,ay,az>
            ax = I->calcAccel(I->ax);
            ay = I->calcAccel(I->ay);
            az = I->calcAccel(I->az);
            // theta in degrees between a0 and a
            theta = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
        }
    }
}

void IMU::trackHeading()
{
    if (state != 1) {
        return;
    }
    else {
        while (state == 1) {

            while(!I->accelAvailable());

            I->readAccel();
            ax0 = I->calcAccel(I->ax);
            ay0 = I->calcAccel(I->ay);
            az0 = I->calcAccel(I->az);
        }
    }
}