Balances a robot on two wheels using a PID controller for fine tuning the motor speed and behavior. It uses an IMU to send input to the PID controller and calibrate the robot's desired angle.

Dependencies:   mbed Motor LSM9DS1_Library_cal

Files at this revision

API Documentation at this revision

Comitter:
johnedwardlee
Date:
Fri Dec 06 13:47:56 2019 +0000
Parent:
2:e8c2301f7523
Commit message:
First commit

Changed in this revision

LSM9DS1.cpp Show diff for this revision Revisions of this file
LSM9DS1.h Show diff for this revision Revisions of this file
LSM9DS1_Library_cal.lib Show annotated file Show diff for this revision Revisions of this file
LSM9DS1_Registers.h Show diff for this revision Revisions of this file
LSM9DS1_Types.h Show diff for this revision Revisions of this file
Motor.lib Show annotated file Show diff for this revision Revisions of this file
PinDetect.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/LSM9DS1.cpp	Wed Feb 03 18:24:29 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1197 +0,0 @@
-/******************************************************************************
-SFE_LSM9DS1.cpp
-SFE_LSM9DS1 Library Source File
-Jim Lindblom @ SparkFun Electronics
-Original Creation Date: February 27, 2015
-https://github.com/sparkfun/LSM9DS1_Breakout
-
-This file implements all functions of the LSM9DS1 class. Functions here range
-from higher level stuff, like reading/writing LSM9DS1 registers to low-level,
-hardware reads and writes. Both SPI and I2C handler functions can be found
-towards the bottom of this file.
-
-Development environment specifics:
-    IDE: Arduino 1.6
-    Hardware Platform: Arduino Uno
-    LSM9DS1 Breakout Version: 1.0
-
-This code is beerware; if you see me (or any other SparkFun employee) at the
-local, and you've found our code helpful, please buy us a round!
-
-Distributed as-is; no warranty is given.
-******************************************************************************/
-
-#include "LSM9DS1.h"
-#include "LSM9DS1_Registers.h"
-#include "LSM9DS1_Types.h"
-//#include <Wire.h> // Wire library is used for I2C
-//#include <SPI.h>  // SPI library is used for...SPI.
-
-//#if defined(ARDUINO) && ARDUINO >= 100
-//  #include "Arduino.h"
-//#else
-//  #include "WProgram.h"
-//#endif
-
-#define LSM9DS1_COMMUNICATION_TIMEOUT 1000
-
-float magSensitivity[4] = {0.00014, 0.00029, 0.00043, 0.00058};
-extern Serial pc;
-
-LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr)
-    :i2c(sda, scl)
-{
-    init(IMU_MODE_I2C, xgAddr, mAddr); // dont know about 0xD6 or 0x3B
-}
-/*
-LSM9DS1::LSM9DS1()
-{
-    init(IMU_MODE_I2C, LSM9DS1_AG_ADDR(1), LSM9DS1_M_ADDR(1));
-}
-
-LSM9DS1::LSM9DS1(interface_mode interface, uint8_t xgAddr, uint8_t mAddr)
-{
-    init(interface, xgAddr, mAddr);
-}
-*/
-
-void LSM9DS1::init(interface_mode interface, uint8_t xgAddr, uint8_t mAddr)
-{
-    settings.device.commInterface = interface;
-    settings.device.agAddress = xgAddr;
-    settings.device.mAddress = mAddr;
-
-    settings.gyro.enabled = true;
-    settings.gyro.enableX = true;
-    settings.gyro.enableY = true;
-    settings.gyro.enableZ = true;
-    // gyro scale can be 245, 500, or 2000
-    settings.gyro.scale = 245;
-    // gyro sample rate: value between 1-6
-    // 1 = 14.9    4 = 238
-    // 2 = 59.5    5 = 476
-    // 3 = 119     6 = 952
-    settings.gyro.sampleRate = 6;
-    // gyro cutoff frequency: value between 0-3
-    // Actual value of cutoff frequency depends
-    // on sample rate.
-    settings.gyro.bandwidth = 0;
-    settings.gyro.lowPowerEnable = false;
-    settings.gyro.HPFEnable = false;
-    // Gyro HPF cutoff frequency: value between 0-9
-    // Actual value depends on sample rate. Only applies
-    // if gyroHPFEnable is true.
-    settings.gyro.HPFCutoff = 0;
-    settings.gyro.flipX = false;
-    settings.gyro.flipY = false;
-    settings.gyro.flipZ = false;
-    settings.gyro.orientation = 0;
-    settings.gyro.latchInterrupt = true;
-
-    settings.accel.enabled = true;
-    settings.accel.enableX = true;
-    settings.accel.enableY = true;
-    settings.accel.enableZ = true;
-    // accel scale can be 2, 4, 8, or 16
-    settings.accel.scale = 2;
-    // accel sample rate can be 1-6
-    // 1 = 10 Hz    4 = 238 Hz
-    // 2 = 50 Hz    5 = 476 Hz
-    // 3 = 119 Hz   6 = 952 Hz
-    settings.accel.sampleRate = 6;
-    // Accel cutoff freqeuncy can be any value between -1 - 3. 
-    // -1 = bandwidth determined by sample rate
-    // 0 = 408 Hz   2 = 105 Hz
-    // 1 = 211 Hz   3 = 50 Hz
-    settings.accel.bandwidth = -1;
-    settings.accel.highResEnable = false;
-    // accelHighResBandwidth can be any value between 0-3
-    // LP cutoff is set to a factor of sample rate
-    // 0 = ODR/50    2 = ODR/9
-    // 1 = ODR/100   3 = ODR/400
-    settings.accel.highResBandwidth = 0;
-
-    settings.mag.enabled = true;
-    // mag scale can be 4, 8, 12, or 16
-    settings.mag.scale = 4;
-    // mag data rate can be 0-7
-    // 0 = 0.625 Hz  4 = 10 Hz
-    // 1 = 1.25 Hz   5 = 20 Hz
-    // 2 = 2.5 Hz    6 = 40 Hz
-    // 3 = 5 Hz      7 = 80 Hz
-    settings.mag.sampleRate = 7;
-    settings.mag.tempCompensationEnable = false;
-    // magPerformance can be any value between 0-3
-    // 0 = Low power mode      2 = high performance
-    // 1 = medium performance  3 = ultra-high performance
-    settings.mag.XYPerformance = 3;
-    settings.mag.ZPerformance = 3;
-    settings.mag.lowPowerEnable = false;
-    // magOperatingMode can be 0-2
-    // 0 = continuous conversion
-    // 1 = single-conversion
-    // 2 = power down
-    settings.mag.operatingMode = 0;
-
-    settings.temp.enabled = true;
-    for (int i=0; i<3; i++)
-    {
-        gBias[i] = 0;
-        aBias[i] = 0;
-        mBias[i] = 0;
-        gBiasRaw[i] = 0;
-        aBiasRaw[i] = 0;
-        mBiasRaw[i] = 0;
-    }
-    _autoCalc = false;
-}
-
-
-uint16_t LSM9DS1::begin()
-{
-    //! Todo: don't use _xgAddress or _mAddress, duplicating memory
-    _xgAddress = settings.device.agAddress;
-    _mAddress = settings.device.mAddress;
-    
-    constrainScales();
-    // Once we have the scale values, we can calculate the resolution
-    // of each sensor. That's what these functions are for. One for each sensor
-    calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
-    calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
-    calcaRes(); // Calculate g / ADC tick, stored in aRes variable
-    
-    // Now, initialize our hardware interface.
-    if (settings.device.commInterface == IMU_MODE_I2C)  // If we're using I2C
-        initI2C();  // Initialize I2C
-    else if (settings.device.commInterface == IMU_MODE_SPI)     // else, if we're using SPI
-        initSPI();  // Initialize SPI
-        
-    // To verify communication, we can read from the WHO_AM_I register of
-    // each device. Store those in a variable so we can return them.
-    uint8_t mTest = mReadByte(WHO_AM_I_M);      // Read the gyro WHO_AM_I
-    uint8_t xgTest = xgReadByte(WHO_AM_I_XG);   // Read the accel/mag WHO_AM_I
-    pc.printf("%x, %x, %x, %x\n\r", mTest, xgTest, _xgAddress, _mAddress);
-    uint16_t whoAmICombined = (xgTest << 8) | mTest;
-    
-    if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP))
-        return 0;
-    
-    // Gyro initialization stuff:
-    initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
-    
-    // Accelerometer initialization stuff:
-    initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
-    
-    // Magnetometer initialization stuff:
-    initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
-
-    // Once everything is initialized, return the WHO_AM_I registers we read:
-    return whoAmICombined;
-}
-
-void LSM9DS1::initGyro()
-{
-    uint8_t tempRegValue = 0;
-    
-    // CTRL_REG1_G (Default value: 0x00)
-    // [ODR_G2][ODR_G1][ODR_G0][FS_G1][FS_G0][0][BW_G1][BW_G0]
-    // ODR_G[2:0] - Output data rate selection
-    // FS_G[1:0] - Gyroscope full-scale selection
-    // BW_G[1:0] - Gyroscope bandwidth selection
-    
-    // To disable gyro, set sample rate bits to 0. We'll only set sample
-    // rate if the gyro is enabled.
-    if (settings.gyro.enabled)
-    {
-        tempRegValue = (settings.gyro.sampleRate & 0x07) << 5;
-    }
-    switch (settings.gyro.scale)
-    {
-        case 500:
-            tempRegValue |= (0x1 << 3);
-            break;
-        case 2000:
-            tempRegValue |= (0x3 << 3);
-            break;
-        // Otherwise we'll set it to 245 dps (0x0 << 4)
-    }
-    tempRegValue |= (settings.gyro.bandwidth & 0x3);
-    xgWriteByte(CTRL_REG1_G, tempRegValue);
-    
-    // CTRL_REG2_G (Default value: 0x00)
-    // [0][0][0][0][INT_SEL1][INT_SEL0][OUT_SEL1][OUT_SEL0]
-    // INT_SEL[1:0] - INT selection configuration
-    // OUT_SEL[1:0] - Out selection configuration
-    xgWriteByte(CTRL_REG2_G, 0x00); 
-    
-    // CTRL_REG3_G (Default value: 0x00)
-    // [LP_mode][HP_EN][0][0][HPCF3_G][HPCF2_G][HPCF1_G][HPCF0_G]
-    // LP_mode - Low-power mode enable (0: disabled, 1: enabled)
-    // HP_EN - HPF enable (0:disabled, 1: enabled)
-    // HPCF_G[3:0] - HPF cutoff frequency
-    tempRegValue = settings.gyro.lowPowerEnable ? (1<<7) : 0;
-    if (settings.gyro.HPFEnable)
-    {
-        tempRegValue |= (1<<6) | (settings.gyro.HPFCutoff & 0x0F);
-    }
-    xgWriteByte(CTRL_REG3_G, tempRegValue);
-    
-    // CTRL_REG4 (Default value: 0x38)
-    // [0][0][Zen_G][Yen_G][Xen_G][0][LIR_XL1][4D_XL1]
-    // Zen_G - Z-axis output enable (0:disable, 1:enable)
-    // Yen_G - Y-axis output enable (0:disable, 1:enable)
-    // Xen_G - X-axis output enable (0:disable, 1:enable)
-    // LIR_XL1 - Latched interrupt (0:not latched, 1:latched)
-    // 4D_XL1 - 4D option on interrupt (0:6D used, 1:4D used)
-    tempRegValue = 0;
-    if (settings.gyro.enableZ) tempRegValue |= (1<<5);
-    if (settings.gyro.enableY) tempRegValue |= (1<<4);
-    if (settings.gyro.enableX) tempRegValue |= (1<<3);
-    if (settings.gyro.latchInterrupt) tempRegValue |= (1<<1);
-    xgWriteByte(CTRL_REG4, tempRegValue);
-    
-    // ORIENT_CFG_G (Default value: 0x00)
-    // [0][0][SignX_G][SignY_G][SignZ_G][Orient_2][Orient_1][Orient_0]
-    // SignX_G - Pitch axis (X) angular rate sign (0: positive, 1: negative)
-    // Orient [2:0] - Directional user orientation selection
-    tempRegValue = 0;
-    if (settings.gyro.flipX) tempRegValue |= (1<<5);
-    if (settings.gyro.flipY) tempRegValue |= (1<<4);
-    if (settings.gyro.flipZ) tempRegValue |= (1<<3);
-    xgWriteByte(ORIENT_CFG_G, tempRegValue);
-}
-
-void LSM9DS1::initAccel()
-{
-    uint8_t tempRegValue = 0;
-    
-    //  CTRL_REG5_XL (0x1F) (Default value: 0x38)
-    //  [DEC_1][DEC_0][Zen_XL][Yen_XL][Zen_XL][0][0][0]
-    //  DEC[0:1] - Decimation of accel data on OUT REG and FIFO.
-    //      00: None, 01: 2 samples, 10: 4 samples 11: 8 samples
-    //  Zen_XL - Z-axis output enabled
-    //  Yen_XL - Y-axis output enabled
-    //  Xen_XL - X-axis output enabled
-    if (settings.accel.enableZ) tempRegValue |= (1<<5);
-    if (settings.accel.enableY) tempRegValue |= (1<<4);
-    if (settings.accel.enableX) tempRegValue |= (1<<3);
-    
-    xgWriteByte(CTRL_REG5_XL, tempRegValue);
-    
-    // CTRL_REG6_XL (0x20) (Default value: 0x00)
-    // [ODR_XL2][ODR_XL1][ODR_XL0][FS1_XL][FS0_XL][BW_SCAL_ODR][BW_XL1][BW_XL0]
-    // ODR_XL[2:0] - Output data rate & power mode selection
-    // FS_XL[1:0] - Full-scale selection
-    // BW_SCAL_ODR - Bandwidth selection
-    // BW_XL[1:0] - Anti-aliasing filter bandwidth selection
-    tempRegValue = 0;
-    // To disable the accel, set the sampleRate bits to 0.
-    if (settings.accel.enabled)
-    {
-        tempRegValue |= (settings.accel.sampleRate & 0x07) << 5;
-    }
-    switch (settings.accel.scale)
-    {
-        case 4:
-            tempRegValue |= (0x2 << 3);
-            break;
-        case 8:
-            tempRegValue |= (0x3 << 3);
-            break;
-        case 16:
-            tempRegValue |= (0x1 << 3);
-            break;
-        // Otherwise it'll be set to 2g (0x0 << 3)
-    }
-    if (settings.accel.bandwidth >= 0)
-    {
-        tempRegValue |= (1<<2); // Set BW_SCAL_ODR
-        tempRegValue |= (settings.accel.bandwidth & 0x03);
-    }
-    xgWriteByte(CTRL_REG6_XL, tempRegValue);
-    
-    // CTRL_REG7_XL (0x21) (Default value: 0x00)
-    // [HR][DCF1][DCF0][0][0][FDS][0][HPIS1]
-    // HR - High resolution mode (0: disable, 1: enable)
-    // DCF[1:0] - Digital filter cutoff frequency
-    // FDS - Filtered data selection
-    // HPIS1 - HPF enabled for interrupt function
-    tempRegValue = 0;
-    if (settings.accel.highResEnable)
-    {
-        tempRegValue |= (1<<7); // Set HR bit
-        tempRegValue |= (settings.accel.highResBandwidth & 0x3) << 5;
-    }
-    xgWriteByte(CTRL_REG7_XL, tempRegValue);
-}
-
-// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
-// them, scales them to  gs and deg/s, respectively, and then passes the biases to the main sketch
-// for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
-// the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
-// subtract the biases ourselves. This results in a more accurate measurement in general and can
-// remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
-// is good practice.
-void LSM9DS1::calibrate(bool autoCalc)
-{  
-    uint8_t data[6] = {0, 0, 0, 0, 0, 0};
-    uint8_t samples = 0;
-    int ii;
-    int32_t aBiasRawTemp[3] = {0, 0, 0};
-    int32_t gBiasRawTemp[3] = {0, 0, 0};
-    
-    // Turn on FIFO and set threshold to 32 samples
-    enableFIFO(true);
-    setFIFO(FIFO_THS, 0x1F);
-    while (samples < 0x1F)
-    {
-        samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples
-    }
-    for(ii = 0; ii < samples ; ii++) 
-    {   // Read the gyro data stored in the FIFO
-        readGyro();
-        gBiasRawTemp[0] += gx;
-        gBiasRawTemp[1] += gy;
-        gBiasRawTemp[2] += gz;
-        readAccel();
-        aBiasRawTemp[0] += ax;
-        aBiasRawTemp[1] += ay;
-        aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up!
-    }  
-    for (ii = 0; ii < 3; ii++)
-    {
-        gBiasRaw[ii] = gBiasRawTemp[ii] / samples;
-        gBias[ii] = calcGyro(gBiasRaw[ii]);
-        aBiasRaw[ii] = aBiasRawTemp[ii] / samples;
-        aBias[ii] = calcAccel(aBiasRaw[ii]);
-    }
-    
-    enableFIFO(false);
-    setFIFO(FIFO_OFF, 0x00);
-    
-    if (autoCalc) _autoCalc = true;
-}
-
-void LSM9DS1::calibrateMag(bool loadIn)
-{
-    int i, j;
-    int16_t magMin[3] = {0, 0, 0};
-    int16_t magMax[3] = {0, 0, 0}; // The road warrior
-    
-    for (i=0; i<128; i++)
-    {
-        while (!magAvailable())
-            ;
-        readMag();
-        int16_t magTemp[3] = {0, 0, 0};
-        magTemp[0] = mx;        
-        magTemp[1] = my;
-        magTemp[2] = mz;
-        for (j = 0; j < 3; j++)
-        {
-            if (magTemp[j] > magMax[j]) magMax[j] = magTemp[j];
-            if (magTemp[j] < magMin[j]) magMin[j] = magTemp[j];
-        }
-    }
-    for (j = 0; j < 3; j++)
-    {
-        mBiasRaw[j] = (magMax[j] + magMin[j]) / 2;
-        mBias[j] = calcMag(mBiasRaw[j]);
-        if (loadIn)
-            magOffset(j, mBiasRaw[j]);
-    }
-    
-}
-void LSM9DS1::magOffset(uint8_t axis, int16_t offset)
-{
-    if (axis > 2)
-        return;
-    uint8_t msb, lsb;
-    msb = (offset & 0xFF00) >> 8;
-    lsb = offset & 0x00FF;
-    mWriteByte(OFFSET_X_REG_L_M + (2 * axis), lsb);
-    mWriteByte(OFFSET_X_REG_H_M + (2 * axis), msb);
-}
-
-void LSM9DS1::initMag()
-{
-    uint8_t tempRegValue = 0;
-    
-    // CTRL_REG1_M (Default value: 0x10)
-    // [TEMP_COMP][OM1][OM0][DO2][DO1][DO0][0][ST]
-    // TEMP_COMP - Temperature compensation
-    // OM[1:0] - X & Y axes op mode selection
-    //  00:low-power, 01:medium performance
-    //  10: high performance, 11:ultra-high performance
-    // DO[2:0] - Output data rate selection
-    // ST - Self-test enable
-    if (settings.mag.tempCompensationEnable) tempRegValue |= (1<<7);
-    tempRegValue |= (settings.mag.XYPerformance & 0x3) << 5;
-    tempRegValue |= (settings.mag.sampleRate & 0x7) << 2;
-    mWriteByte(CTRL_REG1_M, tempRegValue);
-    
-    // CTRL_REG2_M (Default value 0x00)
-    // [0][FS1][FS0][0][REBOOT][SOFT_RST][0][0]
-    // FS[1:0] - Full-scale configuration
-    // REBOOT - Reboot memory content (0:normal, 1:reboot)
-    // SOFT_RST - Reset config and user registers (0:default, 1:reset)
-    tempRegValue = 0;
-    switch (settings.mag.scale)
-    {
-    case 8:
-        tempRegValue |= (0x1 << 5);
-        break;
-    case 12:
-        tempRegValue |= (0x2 << 5);
-        break;
-    case 16:
-        tempRegValue |= (0x3 << 5);
-        break;
-    // Otherwise we'll default to 4 gauss (00)
-    }
-    mWriteByte(CTRL_REG2_M, tempRegValue); // +/-4Gauss
-    
-    // CTRL_REG3_M (Default value: 0x03)
-    // [I2C_DISABLE][0][LP][0][0][SIM][MD1][MD0]
-    // I2C_DISABLE - Disable I2C interace (0:enable, 1:disable)
-    // LP - Low-power mode cofiguration (1:enable)
-    // SIM - SPI mode selection (0:write-only, 1:read/write enable)
-    // MD[1:0] - Operating mode
-    //  00:continuous conversion, 01:single-conversion,
-    //  10,11: Power-down
-    tempRegValue = 0;
-    if (settings.mag.lowPowerEnable) tempRegValue |= (1<<5);
-    tempRegValue |= (settings.mag.operatingMode & 0x3);
-    mWriteByte(CTRL_REG3_M, tempRegValue); // Continuous conversion mode
-    
-    // CTRL_REG4_M (Default value: 0x00)
-    // [0][0][0][0][OMZ1][OMZ0][BLE][0]
-    // OMZ[1:0] - Z-axis operative mode selection
-    //  00:low-power mode, 01:medium performance
-    //  10:high performance, 10:ultra-high performance
-    // BLE - Big/little endian data
-    tempRegValue = 0;
-    tempRegValue = (settings.mag.ZPerformance & 0x3) << 2;
-    mWriteByte(CTRL_REG4_M, tempRegValue);
-    
-    // CTRL_REG5_M (Default value: 0x00)
-    // [0][BDU][0][0][0][0][0][0]
-    // BDU - Block data update for magnetic data
-    //  0:continuous, 1:not updated until MSB/LSB are read
-    tempRegValue = 0;
-    mWriteByte(CTRL_REG5_M, tempRegValue);
-}
-
-uint8_t LSM9DS1::accelAvailable()
-{
-    uint8_t status = xgReadByte(STATUS_REG_1);
-    
-    return (status & (1<<0));
-}
-
-uint8_t LSM9DS1::gyroAvailable()
-{
-    uint8_t status = xgReadByte(STATUS_REG_1);
-    
-    return ((status & (1<<1)) >> 1);
-}
-
-uint8_t LSM9DS1::tempAvailable()
-{
-    uint8_t status = xgReadByte(STATUS_REG_1);
-    
-    return ((status & (1<<2)) >> 2);
-}
-
-uint8_t LSM9DS1::magAvailable(lsm9ds1_axis axis)
-{
-    uint8_t status;
-    status = mReadByte(STATUS_REG_M);
-    
-    return ((status & (1<<axis)) >> axis);
-}
-
-void LSM9DS1::readAccel()
-{
-    uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp   
-    xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL
-    ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
-    ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
-    az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
-    if (_autoCalc)
-    {
-        ax -= aBiasRaw[X_AXIS];
-        ay -= aBiasRaw[Y_AXIS];
-        az -= aBiasRaw[Z_AXIS];
-    }
-}
-
-int16_t LSM9DS1::readAccel(lsm9ds1_axis axis)
-{
-    uint8_t temp[2];
-    int16_t value;
-    xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2);
-    value = (temp[1] << 8) | temp[0];
-    
-    if (_autoCalc)
-        value -= aBiasRaw[axis];
-    
-    return value;
-}
-
-void LSM9DS1::readMag()
-{
-    uint8_t temp[6]; // We'll read six bytes from the mag into temp 
-    mReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
-    mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
-    my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
-    mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
-}
-
-int16_t LSM9DS1::readMag(lsm9ds1_axis axis)
-{
-    uint8_t temp[2];
-    mReadBytes(OUT_X_L_M + (2 * axis), temp, 2);
-    return (temp[1] << 8) | temp[0];
-}
-
-void LSM9DS1::readTemp()
-{
-    uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
-    xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L
-    temperature = ((int16_t)temp[1] << 8) | temp[0];
-}
-
-void LSM9DS1::readGyro()
-{
-    uint8_t temp[6]; // We'll read six bytes from the gyro into temp
-    xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
-    gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
-    gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
-    gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
-    if (_autoCalc)
-    {
-        gx -= gBiasRaw[X_AXIS];
-        gy -= gBiasRaw[Y_AXIS];
-        gz -= gBiasRaw[Z_AXIS];
-    }
-}
-
-int16_t LSM9DS1::readGyro(lsm9ds1_axis axis)
-{
-    uint8_t temp[2];
-    int16_t value;
-    
-    xgReadBytes(OUT_X_L_G + (2 * axis), temp, 2);
-    
-    value = (temp[1] << 8) | temp[0];
-    
-    if (_autoCalc)
-        value -= gBiasRaw[axis];
-    
-    return value;
-}
-
-float LSM9DS1::calcGyro(int16_t gyro)
-{
-    // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
-    return gRes * gyro; 
-}
-
-float LSM9DS1::calcAccel(int16_t accel)
-{
-    // Return the accel raw reading times our pre-calculated g's / (ADC tick):
-    return aRes * accel;
-}
-
-float LSM9DS1::calcMag(int16_t mag)
-{
-    // Return the mag raw reading times our pre-calculated Gs / (ADC tick):
-    return mRes * mag;
-}
-
-void LSM9DS1::setGyroScale(uint16_t gScl)
-{
-    // Read current value of CTRL_REG1_G:
-    uint8_t ctrl1RegValue = xgReadByte(CTRL_REG1_G);
-    // Mask out scale bits (3 & 4):
-    ctrl1RegValue &= 0xE7;
-    switch (gScl)
-    {
-        case 500:
-            ctrl1RegValue |= (0x1 << 3);
-            settings.gyro.scale = 500;
-            break;
-        case 2000:
-            ctrl1RegValue |= (0x3 << 3);
-            settings.gyro.scale = 2000;
-            break;
-        default: // Otherwise we'll set it to 245 dps (0x0 << 4)
-            settings.gyro.scale = 245;
-            break;
-    }
-    xgWriteByte(CTRL_REG1_G, ctrl1RegValue);
-    
-    calcgRes(); 
-}
-
-void LSM9DS1::setAccelScale(uint8_t aScl)
-{
-    // We need to preserve the other bytes in CTRL_REG6_XL. So, first read it:
-    uint8_t tempRegValue = xgReadByte(CTRL_REG6_XL);
-    // Mask out accel scale bits:
-    tempRegValue &= 0xE7;
-    
-    switch (aScl)
-    {
-        case 4:
-            tempRegValue |= (0x2 << 3);
-            settings.accel.scale = 4;
-            break;
-        case 8:
-            tempRegValue |= (0x3 << 3);
-            settings.accel.scale = 8;
-            break;
-        case 16:
-            tempRegValue |= (0x1 << 3);
-            settings.accel.scale = 16;
-            break;
-        default: // Otherwise it'll be set to 2g (0x0 << 3)
-            settings.accel.scale = 2;
-            break;
-    }
-    xgWriteByte(CTRL_REG6_XL, tempRegValue);
-    
-    // Then calculate a new aRes, which relies on aScale being set correctly:
-    calcaRes();
-}
-
-void LSM9DS1::setMagScale(uint8_t mScl)
-{
-    // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
-    uint8_t temp = mReadByte(CTRL_REG2_M);
-    // Then mask out the mag scale bits:
-    temp &= 0xFF^(0x3 << 5);
-    
-    switch (mScl)
-    {
-    case 8:
-        temp |= (0x1 << 5);
-        settings.mag.scale = 8;
-        break;
-    case 12:
-        temp |= (0x2 << 5);
-        settings.mag.scale = 12;
-        break;
-    case 16:
-        temp |= (0x3 << 5);
-        settings.mag.scale = 16;
-        break;
-    default: // Otherwise we'll default to 4 gauss (00)
-        settings.mag.scale = 4;
-        break;
-    }   
-    
-    // And write the new register value back into CTRL_REG6_XM:
-    mWriteByte(CTRL_REG2_M, temp);
-    
-    // We've updated the sensor, but we also need to update our class variables
-    // First update mScale:
-    //mScale = mScl;
-    // Then calculate a new mRes, which relies on mScale being set correctly:
-    calcmRes();
-}
-
-void LSM9DS1::setGyroODR(uint8_t gRate)
-{
-    // Only do this if gRate is not 0 (which would disable the gyro)
-    if ((gRate & 0x07) != 0)
-    {
-        // We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
-        uint8_t temp = xgReadByte(CTRL_REG1_G);
-        // Then mask out the gyro ODR bits:
-        temp &= 0xFF^(0x7 << 5);
-        temp |= (gRate & 0x07) << 5;
-        // Update our settings struct
-        settings.gyro.sampleRate = gRate & 0x07;
-        // And write the new register value back into CTRL_REG1_G:
-        xgWriteByte(CTRL_REG1_G, temp);
-    }
-}
-
-void LSM9DS1::setAccelODR(uint8_t aRate)
-{
-    // Only do this if aRate is not 0 (which would disable the accel)
-    if ((aRate & 0x07) != 0)
-    {
-        // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
-        uint8_t temp = xgReadByte(CTRL_REG6_XL);
-        // Then mask out the accel ODR bits:
-        temp &= 0x1F;
-        // Then shift in our new ODR bits:
-        temp |= ((aRate & 0x07) << 5);
-        settings.accel.sampleRate = aRate & 0x07;
-        // And write the new register value back into CTRL_REG1_XM:
-        xgWriteByte(CTRL_REG6_XL, temp);
-    }
-}
-
-void LSM9DS1::setMagODR(uint8_t mRate)
-{
-    // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
-    uint8_t temp = mReadByte(CTRL_REG1_M);
-    // Then mask out the mag ODR bits:
-    temp &= 0xFF^(0x7 << 2);
-    // Then shift in our new ODR bits:
-    temp |= ((mRate & 0x07) << 2);
-    settings.mag.sampleRate = mRate & 0x07;
-    // And write the new register value back into CTRL_REG5_XM:
-    mWriteByte(CTRL_REG1_M, temp);
-}
-
-void LSM9DS1::calcgRes()
-{
-    gRes = ((float) settings.gyro.scale) / 32768.0;
-}
-
-void LSM9DS1::calcaRes()
-{
-    aRes = ((float) settings.accel.scale) / 32768.0;
-}
-
-void LSM9DS1::calcmRes()
-{
-    //mRes = ((float) settings.mag.scale) / 32768.0;
-    switch (settings.mag.scale)
-    {
-    case 4:
-        mRes = magSensitivity[0];
-        break;
-    case 8:
-        mRes = magSensitivity[1];
-        break;
-    case 12:
-        mRes = magSensitivity[2];
-        break;
-    case 16:
-        mRes = magSensitivity[3];
-        break;
-    }
-    
-}
-
-void LSM9DS1::configInt(interrupt_select interrupt, uint8_t generator,
-                         h_lactive activeLow, pp_od pushPull)
-{
-    // Write to INT1_CTRL or INT2_CTRL. [interupt] should already be one of
-    // those two values.
-    // [generator] should be an OR'd list of values from the interrupt_generators enum
-    xgWriteByte(interrupt, generator);
-    
-    // Configure CTRL_REG8
-    uint8_t temp;
-    temp = xgReadByte(CTRL_REG8);
-    
-    if (activeLow) temp |= (1<<5);
-    else temp &= ~(1<<5);
-    
-    if (pushPull) temp &= ~(1<<4);
-    else temp |= (1<<4);
-    
-    xgWriteByte(CTRL_REG8, temp);
-}
-
-void LSM9DS1::configInactivity(uint8_t duration, uint8_t threshold, bool sleepOn)
-{
-    uint8_t temp = 0;
-    
-    temp = threshold & 0x7F;
-    if (sleepOn) temp |= (1<<7);
-    xgWriteByte(ACT_THS, temp);
-    
-    xgWriteByte(ACT_DUR, duration);
-}
-
-uint8_t LSM9DS1::getInactivity()
-{
-    uint8_t temp = xgReadByte(STATUS_REG_0);
-    temp &= (0x10);
-    return temp;
-}
-
-void LSM9DS1::configAccelInt(uint8_t generator, bool andInterrupts)
-{
-    // Use variables from accel_interrupt_generator, OR'd together to create
-    // the [generator]value.
-    uint8_t temp = generator;
-    if (andInterrupts) temp |= 0x80;
-    xgWriteByte(INT_GEN_CFG_XL, temp);
-}
-
-void LSM9DS1::configAccelThs(uint8_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait)
-{
-    // Write threshold value to INT_GEN_THS_?_XL.
-    // axis will be 0, 1, or 2 (x, y, z respectively)
-    xgWriteByte(INT_GEN_THS_X_XL + axis, threshold);
-    
-    // Write duration and wait to INT_GEN_DUR_XL
-    uint8_t temp;
-    temp = (duration & 0x7F);
-    if (wait) temp |= 0x80;
-    xgWriteByte(INT_GEN_DUR_XL, temp);
-}
-
-uint8_t LSM9DS1::getAccelIntSrc()
-{
-    uint8_t intSrc = xgReadByte(INT_GEN_SRC_XL);
-    
-    // Check if the IA_XL (interrupt active) bit is set
-    if (intSrc & (1<<6))
-    {
-        return (intSrc & 0x3F);
-    }
-    
-    return 0;
-}
-
-void LSM9DS1::configGyroInt(uint8_t generator, bool aoi, bool latch)
-{
-    // Use variables from accel_interrupt_generator, OR'd together to create
-    // the [generator]value.
-    uint8_t temp = generator;
-    if (aoi) temp |= 0x80;
-    if (latch) temp |= 0x40;
-    xgWriteByte(INT_GEN_CFG_G, temp);
-}
-
-void LSM9DS1::configGyroThs(int16_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait)
-{
-    uint8_t buffer[2];
-    buffer[0] = (threshold & 0x7F00) >> 8;
-    buffer[1] = (threshold & 0x00FF);
-    // Write threshold value to INT_GEN_THS_?H_G and  INT_GEN_THS_?L_G.
-    // axis will be 0, 1, or 2 (x, y, z respectively)
-    xgWriteByte(INT_GEN_THS_XH_G + (axis * 2), buffer[0]);
-    xgWriteByte(INT_GEN_THS_XH_G + 1 + (axis * 2), buffer[1]);
-    
-    // Write duration and wait to INT_GEN_DUR_XL
-    uint8_t temp;
-    temp = (duration & 0x7F);
-    if (wait) temp |= 0x80;
-    xgWriteByte(INT_GEN_DUR_G, temp);
-}
-
-uint8_t LSM9DS1::getGyroIntSrc()
-{
-    uint8_t intSrc = xgReadByte(INT_GEN_SRC_G);
-    
-    // Check if the IA_G (interrupt active) bit is set
-    if (intSrc & (1<<6))
-    {
-        return (intSrc & 0x3F);
-    }
-    
-    return 0;
-}
-
-void LSM9DS1::configMagInt(uint8_t generator, h_lactive activeLow, bool latch)
-{
-    // Mask out non-generator bits (0-4)
-    uint8_t config = (generator & 0xE0);    
-    // IEA bit is 0 for active-low, 1 for active-high.
-    if (activeLow == INT_ACTIVE_HIGH) config |= (1<<2);
-    // IEL bit is 0 for latched, 1 for not-latched
-    if (!latch) config |= (1<<1);
-    // As long as we have at least 1 generator, enable the interrupt
-    if (generator != 0) config |= (1<<0);
-    
-    mWriteByte(INT_CFG_M, config);
-}
-
-void LSM9DS1::configMagThs(uint16_t threshold)
-{
-    // Write high eight bits of [threshold] to INT_THS_H_M
-    mWriteByte(INT_THS_H_M, uint8_t((threshold & 0x7F00) >> 8));
-    // Write low eight bits of [threshold] to INT_THS_L_M
-    mWriteByte(INT_THS_L_M, uint8_t(threshold & 0x00FF));
-}
-
-uint8_t LSM9DS1::getMagIntSrc()
-{
-    uint8_t intSrc = mReadByte(INT_SRC_M);
-    
-    // Check if the INT (interrupt active) bit is set
-    if (intSrc & (1<<0))
-    {
-        return (intSrc & 0xFE);
-    }
-    
-    return 0;
-}
-
-void LSM9DS1::sleepGyro(bool enable)
-{
-    uint8_t temp = xgReadByte(CTRL_REG9);
-    if (enable) temp |= (1<<6);
-    else temp &= ~(1<<6);
-    xgWriteByte(CTRL_REG9, temp);
-}
-
-void LSM9DS1::enableFIFO(bool enable)
-{
-    uint8_t temp = xgReadByte(CTRL_REG9);
-    if (enable) temp |= (1<<1);
-    else temp &= ~(1<<1);
-    xgWriteByte(CTRL_REG9, temp);
-}
-
-void LSM9DS1::setFIFO(fifoMode_type fifoMode, uint8_t fifoThs)
-{
-    // Limit threshold - 0x1F (31) is the maximum. If more than that was asked
-    // limit it to the maximum.
-    uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F;
-    xgWriteByte(FIFO_CTRL, ((fifoMode & 0x7) << 5) | (threshold & 0x1F));
-}
-
-uint8_t LSM9DS1::getFIFOSamples()
-{
-    return (xgReadByte(FIFO_SRC) & 0x3F);
-}
-
-void LSM9DS1::constrainScales()
-{
-    if ((settings.gyro.scale != 245) && (settings.gyro.scale != 500) && 
-        (settings.gyro.scale != 2000))
-    {
-        settings.gyro.scale = 245;
-    }
-        
-    if ((settings.accel.scale != 2) && (settings.accel.scale != 4) &&
-        (settings.accel.scale != 8) && (settings.accel.scale != 16))
-    {
-        settings.accel.scale = 2;
-    }
-        
-    if ((settings.mag.scale != 4) && (settings.mag.scale != 8) &&
-        (settings.mag.scale != 12) && (settings.mag.scale != 16))
-    {
-        settings.mag.scale = 4;
-    }
-}
-
-void LSM9DS1::xgWriteByte(uint8_t subAddress, uint8_t data)
-{
-    // Whether we're using I2C or SPI, write a byte using the
-    // gyro-specific I2C address or SPI CS pin.
-    if (settings.device.commInterface == IMU_MODE_I2C) {
-        printf("yo");
-        I2CwriteByte(_xgAddress, subAddress, data);
-    } else if (settings.device.commInterface == IMU_MODE_SPI) {
-        SPIwriteByte(_xgAddress, subAddress, data);
-    }
-}
-
-void LSM9DS1::mWriteByte(uint8_t subAddress, uint8_t data)
-{
-    // Whether we're using I2C or SPI, write a byte using the
-    // accelerometer-specific I2C address or SPI CS pin.
-    if (settings.device.commInterface == IMU_MODE_I2C)
-        return I2CwriteByte(_mAddress, subAddress, data);
-    else if (settings.device.commInterface == IMU_MODE_SPI)
-        return SPIwriteByte(_mAddress, subAddress, data);
-}
-
-uint8_t LSM9DS1::xgReadByte(uint8_t subAddress)
-{
-    // Whether we're using I2C or SPI, read a byte using the
-    // gyro-specific I2C address or SPI CS pin.
-    if (settings.device.commInterface == IMU_MODE_I2C)
-        return I2CreadByte(_xgAddress, subAddress);
-    else if (settings.device.commInterface == IMU_MODE_SPI)
-        return SPIreadByte(_xgAddress, subAddress);
-}
-
-void LSM9DS1::xgReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
-{
-    // Whether we're using I2C or SPI, read multiple bytes using the
-    // gyro-specific I2C address or SPI CS pin.
-    if (settings.device.commInterface == IMU_MODE_I2C) {
-        I2CreadBytes(_xgAddress, subAddress, dest, count);
-    } else if (settings.device.commInterface == IMU_MODE_SPI) {
-        SPIreadBytes(_xgAddress, subAddress, dest, count);
-    }
-}
-
-uint8_t LSM9DS1::mReadByte(uint8_t subAddress)
-{
-    // Whether we're using I2C or SPI, read a byte using the
-    // accelerometer-specific I2C address or SPI CS pin.
-    if (settings.device.commInterface == IMU_MODE_I2C)
-        return I2CreadByte(_mAddress, subAddress);
-    else if (settings.device.commInterface == IMU_MODE_SPI)
-        return SPIreadByte(_mAddress, subAddress);
-}
-
-void LSM9DS1::mReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
-{
-    // Whether we're using I2C or SPI, read multiple bytes using the
-    // accelerometer-specific I2C address or SPI CS pin.
-    if (settings.device.commInterface == IMU_MODE_I2C)
-        I2CreadBytes(_mAddress, subAddress, dest, count);
-    else if (settings.device.commInterface == IMU_MODE_SPI)
-        SPIreadBytes(_mAddress, subAddress, dest, count);
-}
-
-void LSM9DS1::initSPI()
-{
-    /* 
-    pinMode(_xgAddress, OUTPUT);
-    digitalWrite(_xgAddress, HIGH);
-    pinMode(_mAddress, OUTPUT);
-    digitalWrite(_mAddress, HIGH);
-    
-    SPI.begin();
-    // Maximum SPI frequency is 10MHz, could divide by 2 here:
-    SPI.setClockDivider(SPI_CLOCK_DIV2);
-    // Data is read and written MSb first.
-    SPI.setBitOrder(MSBFIRST);
-    // Data is captured on rising edge of clock (CPHA = 0)
-    // Base value of the clock is HIGH (CPOL = 1)
-    SPI.setDataMode(SPI_MODE0);
-    */
-}
-
-void LSM9DS1::SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data)
-{
-    /*
-    digitalWrite(csPin, LOW); // Initiate communication
-    
-    // If write, bit 0 (MSB) should be 0
-    // If single write, bit 1 should be 0
-    SPI.transfer(subAddress & 0x3F); // Send Address
-    SPI.transfer(data); // Send data
-    
-    digitalWrite(csPin, HIGH); // Close communication
-    */
-}
-
-uint8_t LSM9DS1::SPIreadByte(uint8_t csPin, uint8_t subAddress)
-{
-    uint8_t temp;
-    // Use the multiple read function to read 1 byte. 
-    // Value is returned to `temp`.
-    SPIreadBytes(csPin, subAddress, &temp, 1);
-    return temp;
-}
-
-void LSM9DS1::SPIreadBytes(uint8_t csPin, uint8_t subAddress,
-                            uint8_t * dest, uint8_t count)
-{
-    // To indicate a read, set bit 0 (msb) of first byte to 1
-    uint8_t rAddress = 0x80 | (subAddress & 0x3F);
-    // Mag SPI port is different. If we're reading multiple bytes, 
-    // set bit 1 to 1. The remaining six bytes are the address to be read
-    if ((csPin == _mAddress) && count > 1)
-        rAddress |= 0x40;
-    
-    /* 
-    digitalWrite(csPin, LOW); // Initiate communication
-    SPI.transfer(rAddress);
-    for (int i=0; i<count; i++)
-    {
-        dest[i] = SPI.transfer(0x00); // Read into destination array
-    }
-    digitalWrite(csPin, HIGH); // Close communication
-    */
-}
-
-void LSM9DS1::initI2C()
-{
-    /* 
-    Wire.begin();   // Initialize I2C library
-    */
-    
-    //already initialized in constructor!
-}
-
-// Wire.h read and write protocols
-void LSM9DS1::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
-{
-    /* 
-    Wire.beginTransmission(address);  // Initialize the Tx buffer
-    Wire.write(subAddress);           // Put slave register address in Tx buffer
-    Wire.write(data);                 // Put data in Tx buffer
-    Wire.endTransmission();           // Send the Tx buffer
-    */
-    char temp_data[2] = {subAddress, data};
-    i2c.write(address, temp_data, 2);
-}
-
-uint8_t LSM9DS1::I2CreadByte(uint8_t address, uint8_t subAddress)
-{
-    /* 
-    int timeout = LSM9DS1_COMMUNICATION_TIMEOUT;
-    uint8_t data; // `data` will store the register data    
-    
-    Wire.beginTransmission(address);         // Initialize the Tx buffer
-    Wire.write(subAddress);                  // Put slave register address in Tx buffer
-    Wire.endTransmission(true);             // Send the Tx buffer, but send a restart to keep connection alive
-    Wire.requestFrom(address, (uint8_t) 1);  // Read one byte from slave register address 
-    while ((Wire.available() < 1) && (timeout-- > 0))
-        delay(1);
-    
-    if (timeout <= 0)
-        return 255; //! Bad! 255 will be misinterpreted as a good value.
-    
-    data = Wire.read();                      // Fill Rx buffer with result
-    return data;                             // Return data read from slave register
-    */
-    char data;
-    char temp[1] = {subAddress};
-    
-    i2c.write(address, temp, 1);
-    //i2c.write(address & 0xFE);
-    temp[1] = 0x00;
-    i2c.write(address, temp, 1);
-    //i2c.write( address | 0x01);
-    int a = i2c.read(address, &data, 1);
-    return data;
-}
-
-uint8_t LSM9DS1::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count)
-{  
-    /* 
-    int timeout = LSM9DS1_COMMUNICATION_TIMEOUT;
-    Wire.beginTransmission(address);   // Initialize the Tx buffer
-    // Next send the register to be read. OR with 0x80 to indicate multi-read.
-    Wire.write(subAddress | 0x80);     // Put slave register address in Tx buffer
-
-    Wire.endTransmission(true);             // Send the Tx buffer, but send a restart to keep connection alive
-    uint8_t i = 0;
-    Wire.requestFrom(address, count);  // Read bytes from slave register address 
-    while ((Wire.available() < count) && (timeout-- > 0))
-        delay(1);
-    if (timeout <= 0)
-        return -1;
-    
-    for (int i=0; i<count;)
-    {
-        if (Wire.available())
-        {
-            dest[i++] = Wire.read();
-        }
-    }
-    return count;
-    */
-    int i;
-    char temp_dest[count];
-    char temp[1] = {subAddress};
-    i2c.write(address, temp, 1);
-    i2c.read(address, temp_dest, count);
-    
-    //i2c doesn't take uint8_ts, but rather chars so do this nasty af conversion
-    for (i=0; i < count; i++) {
-        dest[i] = temp_dest[i];    
-    }
-    return count;
-}
--- a/LSM9DS1.h	Wed Feb 03 18:24:29 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,557 +0,0 @@
-/******************************************************************************
-SFE_LSM9DS1.h
-SFE_LSM9DS1 Library Header File
-Jim Lindblom @ SparkFun Electronics
-Original Creation Date: February 27, 2015
-https://github.com/sparkfun/LSM9DS1_Breakout
-
-This file prototypes the LSM9DS1 class, implemented in SFE_LSM9DS1.cpp. In
-addition, it defines every register in the LSM9DS1 (both the Gyro and Accel/
-Magnetometer registers).
-
-Development environment specifics:
-    IDE: Arduino 1.6.0
-    Hardware Platform: Arduino Uno
-    LSM9DS1 Breakout Version: 1.0
-
-This code is beerware; if you see me (or any other SparkFun employee) at the
-local, and you've found our code helpful, please buy us a round!
-
-Distributed as-is; no warranty is given.
-******************************************************************************/
-#ifndef __SparkFunLSM9DS1_H__
-#define __SparkFunLSM9DS1_H__
-
-//#if defined(ARDUINO) && ARDUINO >= 100
-//  #include "Arduino.h"
-//#else
-//  #include "WProgram.h"
-//  #include "pins_arduino.h"
-//#endif
-
-#include "mbed.h"
-#include <stdint.h>
-#include "LSM9DS1_Registers.h"
-#include "LSM9DS1_Types.h"
-
-#define LSM9DS1_AG_ADDR(sa0)    ((sa0) == 0 ? 0x6A : 0x6B)
-#define LSM9DS1_M_ADDR(sa1)     ((sa1) == 0 ? 0x1C : 0x1E)
-
-enum lsm9ds1_axis {
-    X_AXIS,
-    Y_AXIS,
-    Z_AXIS,
-    ALL_AXIS
-};
-
-class LSM9DS1
-{
-public:
-    IMUSettings settings;
-    
-    // We'll store the gyro, accel, and magnetometer readings in a series of
-    // public class variables. Each sensor gets three variables -- one for each
-    // axis. Call readGyro(), readAccel(), and readMag() first, before using
-    // these variables!
-    // These values are the RAW signed 16-bit readings from the sensors.
-    int16_t gx, gy, gz; // x, y, and z axis readings of the gyroscope
-    int16_t ax, ay, az; // x, y, and z axis readings of the accelerometer
-    int16_t mx, my, mz; // x, y, and z axis readings of the magnetometer
-    int16_t temperature; // Chip temperature
-    float gBias[3], aBias[3], mBias[3];
-    int16_t gBiasRaw[3], aBiasRaw[3], mBiasRaw[3];
-    
-    // LSM9DS1 -- LSM9DS1 class constructor
-    // The constructor will set up a handful of private variables, and set the
-    // communication mode as well.
-    /**Input:
-    *  - interface = Either IMU_MODE_SPI or IMU_MODE_I2C, whichever you're using
-    *              to talk to the IC.
-    *  - xgAddr = If IMU_MODE_I2C, this is the I2C address of the accel/gyroscope.
-    *              If IMU_MODE_SPI, this is the chip select pin of the gyro (CS_AG)
-    *  - mAddr = If IMU_MODE_I2C, this is the I2C address of the magnetometer.
-    *              If IMU_MODE_SPI, this is the cs pin of the magnetometer (CS_M)
-    
-    */
-    LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr);
-    //LSM9DS1(interface_mode interface, uint8_t xgAddr, uint8_t mAddr);
-    //LSM9DS1();
-       
-    
-    /** begin() -- Initialize the gyro, accelerometer, and magnetometer.
-    *This will set up the scale and output rate of each sensor. The values set
-    * in the IMUSettings struct will take effect after calling this function.
-    */
-    uint16_t begin();
-    
-    void calibrate(bool autoCalc = true);
-    void calibrateMag(bool loadIn = true);
-    void magOffset(uint8_t axis, int16_t offset);
-    
-    /** accelAvailable() -- Polls the accelerometer status register to check
-    * if new data is available.
-    * Output:  1 - New data available
-    *          0 - No new data available
-    */
-    uint8_t accelAvailable();
-    
-    /** gyroAvailable() -- Polls the gyroscope status register to check
-    * if new data is available.
-    * Output:  1 - New data available
-    *          0 - No new data available
-    */
-    uint8_t gyroAvailable();
-    
-    /** gyroAvailable() -- Polls the temperature status register to check
-    * if new data is available.
-    * Output:  1 - New data available
-    *          0 - No new data available
-    */
-    uint8_t tempAvailable();
-    
-    /** magAvailable() -- Polls the accelerometer status register to check
-    * if new data is available.
-    * Input:
-    *  - axis can be either X_AXIS, Y_AXIS, Z_AXIS, to check for new data
-    *    on one specific axis. Or ALL_AXIS (default) to check for new data
-    *    on all axes.
-    * Output:  1 - New data available
-    *          0 - No new data available
-    */
-    uint8_t magAvailable(lsm9ds1_axis axis = ALL_AXIS);
-    
-    /** readGyro() -- Read the gyroscope output registers.
-    * This function will read all six gyroscope output registers.
-    * The readings are stored in the class' gx, gy, and gz variables. Read
-    * those _after_ calling readGyro().
-    */
-    void readGyro();
-    
-    /** int16_t readGyro(axis) -- Read a specific axis of the gyroscope.
-    * [axis] can be any of X_AXIS, Y_AXIS, or Z_AXIS.
-    * Input:
-    *  - axis: can be either X_AXIS, Y_AXIS, or Z_AXIS.
-    * Output:
-    *  A 16-bit signed integer with sensor data on requested axis.
-    */
-    int16_t readGyro(lsm9ds1_axis axis);
-    
-    /** readAccel() -- Read the accelerometer output registers.
-    * This function will read all six accelerometer output registers.
-    * The readings are stored in the class' ax, ay, and az variables. Read
-    * those _after_ calling readAccel().
-    */
-    void readAccel();
-    
-    /** int16_t readAccel(axis) -- Read a specific axis of the accelerometer.
-    * [axis] can be any of X_AXIS, Y_AXIS, or Z_AXIS.
-    * Input:
-    *  - axis: can be either X_AXIS, Y_AXIS, or Z_AXIS.
-    * Output:
-    *  A 16-bit signed integer with sensor data on requested axis.
-    */
-    int16_t readAccel(lsm9ds1_axis axis);
-    
-    /** readMag() -- Read the magnetometer output registers.
-    * This function will read all six magnetometer output registers.
-    * The readings are stored in the class' mx, my, and mz variables. Read
-    * those _after_ calling readMag().
-    */
-    void readMag();
-    
-    /** int16_t readMag(axis) -- Read a specific axis of the magnetometer.
-    * [axis] can be any of X_AXIS, Y_AXIS, or Z_AXIS.
-    * Input:
-    *  - axis: can be either X_AXIS, Y_AXIS, or Z_AXIS.
-    * Output:
-    *  A 16-bit signed integer with sensor data on requested axis.
-    */
-    int16_t readMag(lsm9ds1_axis axis);
-
-    /** readTemp() -- Read the temperature output register.
-    * This function will read two temperature output registers.
-    * The combined readings are stored in the class' temperature variables. Read
-    * those _after_ calling readTemp().
-    */
-    void readTemp();
-    
-    /** calcGyro() -- Convert from RAW signed 16-bit value to degrees per second
-    * This function reads in a signed 16-bit value and returns the scaled
-    * DPS. This function relies on gScale and gRes being correct.
-    * Input:
-    *  - gyro = A signed 16-bit raw reading from the gyroscope.
-    */
-    float calcGyro(int16_t gyro);
-    
-    /** calcAccel() -- Convert from RAW signed 16-bit value to gravity (g's).
-    * This function reads in a signed 16-bit value and returns the scaled
-    * g's. This function relies on aScale and aRes being correct.
-    * Input:
-    *  - accel = A signed 16-bit raw reading from the accelerometer.
-    */
-    float calcAccel(int16_t accel);
-    
-    /** calcMag() -- Convert from RAW signed 16-bit value to Gauss (Gs)
-    * This function reads in a signed 16-bit value and returns the scaled
-    * Gs. This function relies on mScale and mRes being correct.
-    * Input:
-    *  - mag = A signed 16-bit raw reading from the magnetometer.
-    */
-    float calcMag(int16_t mag);
-    
-    /** setGyroScale() -- Set the full-scale range of the gyroscope.
-    * This function can be called to set the scale of the gyroscope to 
-    * 245, 500, or 200 degrees per second.
-    * Input:
-    *  - gScl = The desired gyroscope scale. Must be one of three possible
-    *      values from the gyro_scale.
-    */
-    void setGyroScale(uint16_t gScl);
-    
-    /** setAccelScale() -- Set the full-scale range of the accelerometer.
-    * This function can be called to set the scale of the accelerometer to
-    * 2, 4, 6, 8, or 16 g's.
-    * Input:
-    *  - aScl = The desired accelerometer scale. Must be one of five possible
-    *      values from the accel_scale.
-    */
-    void setAccelScale(uint8_t aScl);
-    
-    /** setMagScale() -- Set the full-scale range of the magnetometer.
-    * This function can be called to set the scale of the magnetometer to
-    * 2, 4, 8, or 12 Gs.
-    * Input:
-    *  - mScl = The desired magnetometer scale. Must be one of four possible
-    *      values from the mag_scale.
-    */
-    void setMagScale(uint8_t mScl);
-    
-    /** setGyroODR() -- Set the output data rate and bandwidth of the gyroscope
-    * Input:
-    *  - gRate = The desired output rate and cutoff frequency of the gyro.
-    */
-    void setGyroODR(uint8_t gRate);
-    
-    // setAccelODR() -- Set the output data rate of the accelerometer
-    // Input:
-    //  - aRate = The desired output rate of the accel.
-    void setAccelODR(uint8_t aRate);    
-    
-    // setMagODR() -- Set the output data rate of the magnetometer
-    // Input:
-    //  - mRate = The desired output rate of the mag.
-    void setMagODR(uint8_t mRate);
-    
-    // configInactivity() -- Configure inactivity interrupt parameters
-    // Input:
-    //  - duration = Inactivity duration - actual value depends on gyro ODR
-    //  - threshold = Activity Threshold
-    //  - sleepOn = Gyroscope operating mode during inactivity.
-    //    true: gyroscope in sleep mode
-    //    false: gyroscope in power-down
-    void configInactivity(uint8_t duration, uint8_t threshold, bool sleepOn);
-    
-    // configAccelInt() -- Configure Accelerometer Interrupt Generator
-    // Input:
-    //  - generator = Interrupt axis/high-low events
-    //    Any OR'd combination of ZHIE_XL, ZLIE_XL, YHIE_XL, YLIE_XL, XHIE_XL, XLIE_XL
-    //  - andInterrupts = AND/OR combination of interrupt events
-    //    true: AND combination
-    //    false: OR combination
-    void configAccelInt(uint8_t generator, bool andInterrupts = false);
-    
-    // configAccelThs() -- Configure the threshold of an accelereomter axis
-    // Input:
-    //  - threshold = Interrupt threshold. Possible values: 0-255.
-    //    Multiply by 128 to get the actual raw accel value.
-    //  - axis = Axis to be configured. Either X_AXIS, Y_AXIS, or Z_AXIS
-    //  - duration = Duration value must be above or below threshold to trigger interrupt
-    //  - wait = Wait function on duration counter
-    //    true: Wait for duration samples before exiting interrupt
-    //    false: Wait function off
-    void configAccelThs(uint8_t threshold, lsm9ds1_axis axis, uint8_t duration = 0, bool wait = 0);
-    
-    // configGyroInt() -- Configure Gyroscope Interrupt Generator
-    // Input:
-    //  - generator = Interrupt axis/high-low events
-    //    Any OR'd combination of ZHIE_G, ZLIE_G, YHIE_G, YLIE_G, XHIE_G, XLIE_G
-    //  - aoi = AND/OR combination of interrupt events
-    //    true: AND combination
-    //    false: OR combination
-    //  - latch: latch gyroscope interrupt request.
-    void configGyroInt(uint8_t generator, bool aoi, bool latch);
-    
-    // configGyroThs() -- Configure the threshold of a gyroscope axis
-    // Input:
-    //  - threshold = Interrupt threshold. Possible values: 0-0x7FF.
-    //    Value is equivalent to raw gyroscope value.
-    //  - axis = Axis to be configured. Either X_AXIS, Y_AXIS, or Z_AXIS
-    //  - duration = Duration value must be above or below threshold to trigger interrupt
-    //  - wait = Wait function on duration counter
-    //    true: Wait for duration samples before exiting interrupt
-    //    false: Wait function off
-    void configGyroThs(int16_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait);
-    
-    // configInt() -- Configure INT1 or INT2 (Gyro and Accel Interrupts only)
-    // Input:
-    //  - interrupt = Select INT1 or INT2
-    //    Possible values: XG_INT1 or XG_INT2
-    //  - generator = Or'd combination of interrupt generators.
-    //    Possible values: INT_DRDY_XL, INT_DRDY_G, INT1_BOOT (INT1 only), INT2_DRDY_TEMP (INT2 only)
-    //    INT_FTH, INT_OVR, INT_FSS5, INT_IG_XL (INT1 only), INT1_IG_G (INT1 only), INT2_INACT (INT2 only)
-    //  - activeLow = Interrupt active configuration
-    //    Can be either INT_ACTIVE_HIGH or INT_ACTIVE_LOW
-    //  - pushPull =  Push-pull or open drain interrupt configuration
-    //    Can be either INT_PUSH_PULL or INT_OPEN_DRAIN
-    void configInt(interrupt_select interupt, uint8_t generator,
-                   h_lactive activeLow = INT_ACTIVE_LOW, pp_od pushPull = INT_PUSH_PULL);
-                   
-    /** configMagInt() -- Configure Magnetometer Interrupt Generator
-    * Input:
-    *  - generator = Interrupt axis/high-low events
-    *    Any OR'd combination of ZIEN, YIEN, XIEN
-    *  - activeLow = Interrupt active configuration
-    *    Can be either INT_ACTIVE_HIGH or INT_ACTIVE_LOW
-    *  - latch: latch gyroscope interrupt request.
-    */
-    void configMagInt(uint8_t generator, h_lactive activeLow, bool latch = true);
-    
-    /** configMagThs() -- Configure the threshold of a gyroscope axis
-    * Input:
-    *  - threshold = Interrupt threshold. Possible values: 0-0x7FF.
-    *    Value is equivalent to raw magnetometer value.
-    */
-    void configMagThs(uint16_t threshold);
-    
-    //! getGyroIntSrc() -- Get contents of Gyroscope interrupt source register
-    uint8_t getGyroIntSrc();
-    
-    //! getGyroIntSrc() -- Get contents of accelerometer interrupt source register
-    uint8_t getAccelIntSrc();
-    
-    //! getGyroIntSrc() -- Get contents of magnetometer interrupt source register
-    uint8_t getMagIntSrc();
-    
-    //! getGyroIntSrc() -- Get status of inactivity interrupt
-    uint8_t getInactivity();
-    
-    /** sleepGyro() -- Sleep or wake the gyroscope
-    * Input:
-    *  - enable: True = sleep gyro. False = wake gyro.
-    */
-    void sleepGyro(bool enable = true);
-    
-    /** enableFIFO() - Enable or disable the FIFO
-    * Input:
-    *  - enable: true = enable, false = disable.
-    */
-    void enableFIFO(bool enable = true);
-    
-    /** setFIFO() - Configure FIFO mode and Threshold
-    * Input:
-    *  - fifoMode: Set FIFO mode to off, FIFO (stop when full), continuous, bypass
-    *    Possible inputs: FIFO_OFF, FIFO_THS, FIFO_CONT_TRIGGER, FIFO_OFF_TRIGGER, FIFO_CONT
-    *  - fifoThs: FIFO threshold level setting
-    *    Any value from 0-0x1F is acceptable.
-    */
-    void setFIFO(fifoMode_type fifoMode, uint8_t fifoThs);
-    
-    //! getFIFOSamples() - Get number of FIFO samples
-    uint8_t getFIFOSamples();
-        
-
-protected:  
-    // x_mAddress and gAddress store the I2C address or SPI chip select pin
-    // for each sensor.
-    uint8_t _mAddress, _xgAddress;
-    
-    // gRes, aRes, and mRes store the current resolution for each sensor. 
-    // Units of these values would be DPS (or g's or Gs's) per ADC tick.
-    // This value is calculated as (sensor scale) / (2^15).
-    float gRes, aRes, mRes;
-    
-    // _autoCalc keeps track of whether we're automatically subtracting off
-    // accelerometer and gyroscope bias calculated in calibrate().
-    bool _autoCalc;
-    
-    // init() -- Sets up gyro, accel, and mag settings to default.
-    // - interface - Sets the interface mode (IMU_MODE_I2C or IMU_MODE_SPI)
-    // - xgAddr - Sets either the I2C address of the accel/gyro or SPI chip 
-    //   select pin connected to the CS_XG pin.
-    // - mAddr - Sets either the I2C address of the magnetometer or SPI chip 
-    //   select pin connected to the CS_M pin.
-    void init(interface_mode interface, uint8_t xgAddr, uint8_t mAddr);
-    
-    // initGyro() -- Sets up the gyroscope to begin reading.
-    // This function steps through all five gyroscope control registers.
-    // Upon exit, the following parameters will be set:
-    //  - CTRL_REG1_G = 0x0F: Normal operation mode, all axes enabled. 
-    //      95 Hz ODR, 12.5 Hz cutoff frequency.
-    //  - CTRL_REG2_G = 0x00: HPF set to normal mode, cutoff frequency
-    //      set to 7.2 Hz (depends on ODR).
-    //  - CTRL_REG3_G = 0x88: Interrupt enabled on INT_G (set to push-pull and
-    //      active high). Data-ready output enabled on DRDY_G.
-    //  - CTRL_REG4_G = 0x00: Continuous update mode. Data LSB stored in lower
-    //      address. Scale set to 245 DPS. SPI mode set to 4-wire.
-    //  - CTRL_REG5_G = 0x00: FIFO disabled. HPF disabled.
-    void initGyro();
-    
-    // initAccel() -- Sets up the accelerometer to begin reading.
-    // This function steps through all accelerometer related control registers.
-    // Upon exit these registers will be set as:
-    //  - CTRL_REG0_XM = 0x00: FIFO disabled. HPF bypassed. Normal mode.
-    //  - CTRL_REG1_XM = 0x57: 100 Hz data rate. Continuous update.
-    //      all axes enabled.
-    //  - CTRL_REG2_XM = 0x00:  2g scale. 773 Hz anti-alias filter BW.
-    //  - CTRL_REG3_XM = 0x04: Accel data ready signal on INT1_XM pin.
-    void initAccel();
-    
-    // initMag() -- Sets up the magnetometer to begin reading.
-    // This function steps through all magnetometer-related control registers.
-    // Upon exit these registers will be set as:
-    //  - CTRL_REG4_XM = 0x04: Mag data ready signal on INT2_XM pin.
-    //  - CTRL_REG5_XM = 0x14: 100 Hz update rate. Low resolution. Interrupt
-    //      requests don't latch. Temperature sensor disabled.
-    //  - CTRL_REG6_XM = 0x00:  2 Gs scale.
-    //  - CTRL_REG7_XM = 0x00: Continuous conversion mode. Normal HPF mode.
-    //  - INT_CTRL_REG_M = 0x09: Interrupt active-high. Enable interrupts.
-    void initMag();
-    
-    // gReadByte() -- Reads a byte from a specified gyroscope register.
-    // Input:
-    //  - subAddress = Register to be read from.
-    // Output:
-    //  - An 8-bit value read from the requested address.
-    uint8_t mReadByte(uint8_t subAddress);
-    
-    // gReadBytes() -- Reads a number of bytes -- beginning at an address
-    // and incrementing from there -- from the gyroscope.
-    // Input:
-    //  - subAddress = Register to be read from.
-    //  - * dest = A pointer to an array of uint8_t's. Values read will be
-    //      stored in here on return.
-    //  - count = The number of bytes to be read.
-    // Output: No value is returned, but the `dest` array will store
-    //  the data read upon exit.
-    void mReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count);
-    
-    // gWriteByte() -- Write a byte to a register in the gyroscope.
-    // Input:
-    //  - subAddress = Register to be written to.
-    //  - data = data to be written to the register.
-    void mWriteByte(uint8_t subAddress, uint8_t data);
-    
-    // xmReadByte() -- Read a byte from a register in the accel/mag sensor
-    // Input:
-    //  - subAddress = Register to be read from.
-    // Output:
-    //  - An 8-bit value read from the requested register.
-    uint8_t xgReadByte(uint8_t subAddress);
-    
-    // xmReadBytes() -- Reads a number of bytes -- beginning at an address
-    // and incrementing from there -- from the accelerometer/magnetometer.
-    // Input:
-    //  - subAddress = Register to be read from.
-    //  - * dest = A pointer to an array of uint8_t's. Values read will be
-    //      stored in here on return.
-    //  - count = The number of bytes to be read.
-    // Output: No value is returned, but the `dest` array will store
-    //  the data read upon exit.
-    void xgReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count);
-    
-    // xmWriteByte() -- Write a byte to a register in the accel/mag sensor.
-    // Input:
-    //  - subAddress = Register to be written to.
-    //  - data = data to be written to the register.
-    void xgWriteByte(uint8_t subAddress, uint8_t data);
-    
-    // calcgRes() -- Calculate the resolution of the gyroscope.
-    // This function will set the value of the gRes variable. gScale must
-    // be set prior to calling this function.
-    void calcgRes();
-    
-    // calcmRes() -- Calculate the resolution of the magnetometer.
-    // This function will set the value of the mRes variable. mScale must
-    // be set prior to calling this function.
-    void calcmRes();
-    
-    // calcaRes() -- Calculate the resolution of the accelerometer.
-    // This function will set the value of the aRes variable. aScale must
-    // be set prior to calling this function.
-    void calcaRes();
-    
-    //////////////////////
-    // Helper Functions //
-    //////////////////////
-    void constrainScales();
-    
-    ///////////////////
-    // SPI Functions //
-    ///////////////////
-    // initSPI() -- Initialize the SPI hardware.
-    // This function will setup all SPI pins and related hardware.
-    void initSPI();
-    
-    // SPIwriteByte() -- Write a byte out of SPI to a register in the device
-    // Input:
-    //  - csPin = The chip select pin of the slave device.
-    //  - subAddress = The register to be written to.
-    //  - data = Byte to be written to the register.
-    void SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data);
-    
-    // SPIreadByte() -- Read a single byte from a register over SPI.
-    // Input:
-    //  - csPin = The chip select pin of the slave device.
-    //  - subAddress = The register to be read from.
-    // Output:
-    //  - The byte read from the requested address.
-    uint8_t SPIreadByte(uint8_t csPin, uint8_t subAddress);
-    
-    // SPIreadBytes() -- Read a series of bytes, starting at a register via SPI
-    // Input:
-    //  - csPin = The chip select pin of a slave device.
-    //  - subAddress = The register to begin reading.
-    //  - * dest = Pointer to an array where we'll store the readings.
-    //  - count = Number of registers to be read.
-    // Output: No value is returned by the function, but the registers read are
-    //      all stored in the *dest array given.
-    void SPIreadBytes(uint8_t csPin, uint8_t subAddress, 
-                            uint8_t * dest, uint8_t count);
-    
-    ///////////////////
-    // I2C Functions //
-    ///////////////////
-    // initI2C() -- Initialize the I2C hardware.
-    // This function will setup all I2C pins and related hardware.
-    void initI2C();
-    
-    // I2CwriteByte() -- Write a byte out of I2C to a register in the device
-    // Input:
-    //  - address = The 7-bit I2C address of the slave device.
-    //  - subAddress = The register to be written to.
-    //  - data = Byte to be written to the register.
-    void I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data);
-    
-    // I2CreadByte() -- Read a single byte from a register over I2C.
-    // Input:
-    //  - address = The 7-bit I2C address of the slave device.
-    //  - subAddress = The register to be read from.
-    // Output:
-    //  - The byte read from the requested address.
-    uint8_t I2CreadByte(uint8_t address, uint8_t subAddress);
-    
-    // I2CreadBytes() -- Read a series of bytes, starting at a register via SPI
-    // Input:
-    //  - address = The 7-bit I2C address of the slave device.
-    //  - subAddress = The register to begin reading.
-    //  - * dest = Pointer to an array where we'll store the readings.
-    //  - count = Number of registers to be read.
-    // Output: No value is returned by the function, but the registers read are
-    //      all stored in the *dest array given.
-    uint8_t I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count);
-    
-private:
-    I2C i2c;
-};
-
-#endif // SFE_LSM9DS1_H //
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS1_Library_cal.lib	Fri Dec 06 13:47:56 2019 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/4180_1/code/LSM9DS1_Library_cal/#36abf8e18ade
--- a/LSM9DS1_Registers.h	Wed Feb 03 18:24:29 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,112 +0,0 @@
-/******************************************************************************
-LSM9DS1_Registers.h
-SFE_LSM9DS1 Library - LSM9DS1 Register Map
-Jim Lindblom @ SparkFun Electronics
-Original Creation Date: April 21, 2015
-https://github.com/sparkfun/LSM9DS1_Breakout
-
-This file defines all registers internal to the gyro/accel and magnetometer
-devices in the LSM9DS1.
-
-Development environment specifics:
-    IDE: Arduino 1.6.0
-    Hardware Platform: Arduino Uno
-    LSM9DS1 Breakout Version: 1.0
-
-This code is beerware; if you see me (or any other SparkFun employee) at the
-local, and you've found our code helpful, please buy us a round!
-
-Distributed as-is; no warranty is given.
-******************************************************************************/
-
-#ifndef __LSM9DS1_Registers_H__
-#define __LSM9DS1_Registers_H__
-
-/////////////////////////////////////////
-// LSM9DS1 Accel/Gyro (XL/G) Registers //
-/////////////////////////////////////////
-#define ACT_THS             0x04
-#define ACT_DUR             0x05
-#define INT_GEN_CFG_XL      0x06
-#define INT_GEN_THS_X_XL    0x07
-#define INT_GEN_THS_Y_XL    0x08
-#define INT_GEN_THS_Z_XL    0x09
-#define INT_GEN_DUR_XL      0x0A
-#define REFERENCE_G         0x0B
-#define INT1_CTRL           0x0C
-#define INT2_CTRL           0x0D
-#define WHO_AM_I_XG         0x0F
-#define CTRL_REG1_G         0x10
-#define CTRL_REG2_G         0x11
-#define CTRL_REG3_G         0x12
-#define ORIENT_CFG_G        0x13
-#define INT_GEN_SRC_G       0x14
-#define OUT_TEMP_L          0x15
-#define OUT_TEMP_H          0x16
-#define STATUS_REG_0        0x17
-#define OUT_X_L_G           0x18
-#define OUT_X_H_G           0x19
-#define OUT_Y_L_G           0x1A
-#define OUT_Y_H_G           0x1B
-#define OUT_Z_L_G           0x1C
-#define OUT_Z_H_G           0x1D
-#define CTRL_REG4           0x1E
-#define CTRL_REG5_XL        0x1F
-#define CTRL_REG6_XL        0x20
-#define CTRL_REG7_XL        0x21
-#define CTRL_REG8           0x22
-#define CTRL_REG9           0x23
-#define CTRL_REG10          0x24
-#define INT_GEN_SRC_XL      0x26
-#define STATUS_REG_1        0x27
-#define OUT_X_L_XL          0x28
-#define OUT_X_H_XL          0x29
-#define OUT_Y_L_XL          0x2A
-#define OUT_Y_H_XL          0x2B
-#define OUT_Z_L_XL          0x2C
-#define OUT_Z_H_XL          0x2D
-#define FIFO_CTRL           0x2E
-#define FIFO_SRC            0x2F
-#define INT_GEN_CFG_G       0x30
-#define INT_GEN_THS_XH_G    0x31
-#define INT_GEN_THS_XL_G    0x32
-#define INT_GEN_THS_YH_G    0x33
-#define INT_GEN_THS_YL_G    0x34
-#define INT_GEN_THS_ZH_G    0x35
-#define INT_GEN_THS_ZL_G    0x36
-#define INT_GEN_DUR_G       0x37
-
-///////////////////////////////
-// LSM9DS1 Magneto Registers //
-///////////////////////////////
-#define OFFSET_X_REG_L_M    0x05
-#define OFFSET_X_REG_H_M    0x06
-#define OFFSET_Y_REG_L_M    0x07
-#define OFFSET_Y_REG_H_M    0x08
-#define OFFSET_Z_REG_L_M    0x09
-#define OFFSET_Z_REG_H_M    0x0A
-#define WHO_AM_I_M          0x0F
-#define CTRL_REG1_M         0x20
-#define CTRL_REG2_M         0x21
-#define CTRL_REG3_M         0x22
-#define CTRL_REG4_M         0x23
-#define CTRL_REG5_M         0x24
-#define STATUS_REG_M        0x27
-#define OUT_X_L_M           0x28
-#define OUT_X_H_M           0x29
-#define OUT_Y_L_M           0x2A
-#define OUT_Y_H_M           0x2B
-#define OUT_Z_L_M           0x2C
-#define OUT_Z_H_M           0x2D
-#define INT_CFG_M           0x30
-#define INT_SRC_M           0x30
-#define INT_THS_L_M         0x32
-#define INT_THS_H_M         0x33
-
-////////////////////////////////
-// LSM9DS1 WHO_AM_I Responses //
-////////////////////////////////
-#define WHO_AM_I_AG_RSP     0x68
-#define WHO_AM_I_M_RSP      0x3D
-
-#endif
\ No newline at end of file
--- a/LSM9DS1_Types.h	Wed Feb 03 18:24:29 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,251 +0,0 @@
-/******************************************************************************
-LSM9DS1_Types.h
-SFE_LSM9DS1 Library - LSM9DS1 Types and Enumerations
-Jim Lindblom @ SparkFun Electronics
-Original Creation Date: April 21, 2015
-https://github.com/sparkfun/LSM9DS1_Breakout
-
-This file defines all types and enumerations used by the LSM9DS1 class.
-
-Development environment specifics:
-    IDE: Arduino 1.6.0
-    Hardware Platform: Arduino Uno
-    LSM9DS1 Breakout Version: 1.0
-
-This code is beerware; if you see me (or any other SparkFun employee) at the
-local, and you've found our code helpful, please buy us a round!
-
-Distributed as-is; no warranty is given.
-******************************************************************************/
-
-#ifndef __LSM9DS1_Types_H__
-#define __LSM9DS1_Types_H__
-
-#include "LSM9DS1_Registers.h"
-
-// The LSM9DS1 functions over both I2C or SPI. This library supports both.
-// But the interface mode used must be sent to the LSM9DS1 constructor. Use
-// one of these two as the first parameter of the constructor.
-enum interface_mode
-{
-    IMU_MODE_SPI,
-    IMU_MODE_I2C,
-};
-
-// accel_scale defines all possible FSR's of the accelerometer:
-enum accel_scale
-{
-    A_SCALE_2G, // 00:  2g
-    A_SCALE_16G,// 01:  16g
-    A_SCALE_4G, // 10:  4g
-    A_SCALE_8G  // 11:  8g
-};
-
-// gyro_scale defines the possible full-scale ranges of the gyroscope:
-enum gyro_scale
-{
-    G_SCALE_245DPS,     // 00:  245 degrees per second
-    G_SCALE_500DPS,     // 01:  500 dps
-    G_SCALE_2000DPS,    // 11:  2000 dps
-};
-
-// mag_scale defines all possible FSR's of the magnetometer:
-enum mag_scale
-{
-    M_SCALE_4GS,    // 00:  4Gs
-    M_SCALE_8GS,    // 01:  8Gs
-    M_SCALE_12GS,   // 10:  12Gs
-    M_SCALE_16GS,   // 11:  16Gs
-};
-
-// gyro_odr defines all possible data rate/bandwidth combos of the gyro:
-enum gyro_odr
-{
-    //! TODO 
-    G_ODR_PD,   // Power down (0)
-    G_ODR_149,  // 14.9 Hz (1)
-    G_ODR_595,  // 59.5 Hz (2)
-    G_ODR_119,  // 119 Hz (3)
-    G_ODR_238,  // 238 Hz (4)
-    G_ODR_476,  // 476 Hz (5)
-    G_ODR_952   // 952 Hz (6)
-};
-// accel_oder defines all possible output data rates of the accelerometer:
-enum accel_odr
-{
-    XL_POWER_DOWN,  // Power-down mode (0x0)
-    XL_ODR_10,      // 10 Hz (0x1)
-    XL_ODR_50,      // 50 Hz (0x02)
-    XL_ODR_119,     // 119 Hz (0x3)
-    XL_ODR_238,     // 238 Hz (0x4)
-    XL_ODR_476,     // 476 Hz (0x5)
-    XL_ODR_952      // 952 Hz (0x6)
-};
-
-// accel_abw defines all possible anti-aliasing filter rates of the accelerometer:
-enum accel_abw
-{
-    A_ABW_408,      // 408 Hz (0x0)
-    A_ABW_211,      // 211 Hz (0x1)
-    A_ABW_105,      // 105 Hz (0x2)
-    A_ABW_50,       //  50 Hz (0x3)
-};
-
-
-// mag_odr defines all possible output data rates of the magnetometer:
-enum mag_odr
-{
-    M_ODR_0625, // 0.625 Hz (0)
-    M_ODR_125,  // 1.25 Hz (1)
-    M_ODR_250,  // 2.5 Hz (2)
-    M_ODR_5,    // 5 Hz (3)
-    M_ODR_10,   // 10 Hz (4)
-    M_ODR_20,   // 20 Hz (5)
-    M_ODR_40,   // 40 Hz (6)
-    M_ODR_80    // 80 Hz (7)
-};
-
-enum interrupt_select
-{
-    XG_INT1 = INT1_CTRL,
-    XG_INT2 = INT2_CTRL
-};
-
-enum interrupt_generators
-{
-    INT_DRDY_XL = (1<<0),    // Accelerometer data ready (INT1 & INT2)
-    INT_DRDY_G = (1<<1),     // Gyroscope data ready (INT1 & INT2)
-    INT1_BOOT = (1<<2),  // Boot status (INT1)
-    INT2_DRDY_TEMP = (1<<2),// Temp data ready (INT2)
-    INT_FTH = (1<<3),        // FIFO threshold interrupt (INT1 & INT2)
-    INT_OVR = (1<<4),        // Overrun interrupt (INT1 & INT2)
-    INT_FSS5 = (1<<5),       // FSS5 interrupt (INT1 & INT2)
-    INT_IG_XL = (1<<6),  // Accel interrupt generator (INT1)
-    INT1_IG_G = (1<<7),  // Gyro interrupt enable (INT1)
-    INT2_INACT = (1<<7),     // Inactivity interrupt output (INT2)
-};  
-
-enum accel_interrupt_generator
-{
-    XLIE_XL = (1<<0),
-    XHIE_XL = (1<<1),
-    YLIE_XL = (1<<2),
-    YHIE_XL = (1<<3),
-    ZLIE_XL = (1<<4),
-    ZHIE_XL = (1<<5),
-    GEN_6D = (1<<6)
-};
-
-enum gyro_interrupt_generator
-{
-    XLIE_G = (1<<0),
-    XHIE_G = (1<<1),
-    YLIE_G = (1<<2),
-    YHIE_G = (1<<3),
-    ZLIE_G = (1<<4),
-    ZHIE_G = (1<<5)
-};
-
-enum mag_interrupt_generator
-{
-    ZIEN = (1<<5),
-    YIEN = (1<<6),
-    XIEN = (1<<7)
-};
-
-enum h_lactive
-{
-    INT_ACTIVE_HIGH,
-    INT_ACTIVE_LOW
-};
-
-enum pp_od
-{
-    INT_PUSH_PULL,
-    INT_OPEN_DRAIN
-};
-
-enum fifoMode_type
-{
-    FIFO_OFF = 0,
-    FIFO_THS = 1,
-    FIFO_CONT_TRIGGER = 3,
-    FIFO_OFF_TRIGGER = 4,
-    FIFO_CONT = 5
-};
-
-struct gyroSettings
-{
-    // Gyroscope settings:
-    uint8_t enabled;
-    uint16_t scale; // Changed this to 16-bit
-    uint8_t sampleRate;
-    // New gyro stuff:
-    uint8_t bandwidth;
-    uint8_t lowPowerEnable;
-    uint8_t HPFEnable;
-    uint8_t HPFCutoff;
-    uint8_t flipX;
-    uint8_t flipY;
-    uint8_t flipZ;
-    uint8_t orientation;
-    uint8_t enableX;
-    uint8_t enableY;
-    uint8_t enableZ;
-    uint8_t latchInterrupt;
-};
-
-struct deviceSettings
-{
-    uint8_t commInterface; // Can be I2C, SPI 4-wire or SPI 3-wire
-    uint8_t agAddress;  // I2C address or SPI CS pin
-    uint8_t mAddress;   // I2C address or SPI CS pin
-};
-
-struct accelSettings
-{
-    // Accelerometer settings:
-    uint8_t enabled;
-    uint8_t scale;
-    uint8_t sampleRate;
-    // New accel stuff:
-    uint8_t enableX;
-    uint8_t enableY;
-    uint8_t enableZ;
-    int8_t  bandwidth;
-    uint8_t highResEnable;
-    uint8_t highResBandwidth;
-};
-
-struct magSettings
-{
-    // Magnetometer settings:
-    uint8_t enabled;
-    uint8_t scale;
-    uint8_t sampleRate;
-    // New mag stuff:
-    uint8_t tempCompensationEnable;
-    uint8_t XYPerformance;
-    uint8_t ZPerformance;
-    uint8_t lowPowerEnable;
-    uint8_t operatingMode;
-};
-
-struct temperatureSettings
-{
-    // Temperature settings
-    uint8_t enabled;
-};
-
-struct IMUSettings
-{
-    deviceSettings device;
-    
-    gyroSettings gyro;
-    accelSettings accel;
-    magSettings mag;
-    
-    temperatureSettings temp;
-};
-
-#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Fri Dec 06 13:47:56 2019 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Motor/#f265e441bcd9
--- a/PinDetect.lib	Wed Feb 03 18:24:29 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b
--- a/main.cpp	Wed Feb 03 18:24:29 2016 +0000
+++ b/main.cpp	Fri Dec 06 13:47:56 2019 +0000
@@ -1,29 +1,107 @@
+/*
+    4180Balancer
+    Names: John Lee, Nyle Malik, Austin Lowe
+    Date: 12/6/19
+    Description:
+        Balances a robot on two wheels using a PID controller for fine tuning the 
+        motor speed and behavior. It uses an IMU to send input to the PID controller
+        and calibrate the robot's desired angle.
+*/
+
 #include "LSM9DS1.h"
+#include "Motor.h"
+#include "mbed.h"
+#include <time.h>
+#include <stdlib.h>
+
+#define RAD_TO_DEG 57.29578
+#define M_PI 3.14159265358979323846
+#define FILTER_CONSTANT 0.98
+#define DT 0.001
 
 DigitalOut myled(LED1);
+LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
+Motor left(p21, p7, p8);
+Motor right(p22, p11, p12);
+Ticker PID;
 Serial pc(USBTX, USBRX);
 
+//Globals
+//Configure these
+float scalingFactor = 2000;
+float KP = 100;
+float KI = 100;
+float KD = 1000;
+
+float aBias = 0;
+float gBias = 0;
+float aAngle = 0;
+float gAngle = 0;
+float intAngle = 0;
+float derivAngle = 0;
+float propAngle = 0;
+float angle = 0;   
+float speed = 0;
+
+void calibrate()
+{
+    for(int i=0; i<100; i++) {                            
+        imu.readAccel();                                  
+        imu.readGyro();                                   
+        aBias=aBias+(-1)*atan2((double)(imu.ay),(double)(imu.az))*180/3.142-90; 
+        gBias=gBias+imu.gx;                               
+    }
+    aBias=aBias/100;                                      
+    gBias=gBias/100;                                      
+}
+
+void pid()
+{
+     // Save the previous data
+     derivAngle = propAngle;
+     
+     //Read data from the IMU
+     imu.readAccel();
+     imu.readGyro();
+     
+     //Controls and PID calculations
+     aAngle = (-1) * atan2((double)(imu.ay), (double)(imu.az)) * RAD_TO_DEG - 90 - aBias;
+     gAngle = (-1)*(imu.gx - gBias) * DT; 
+     propAngle = FILTER_CONSTANT * (propAngle + gAngle) + (1 - FILTER_CONSTANT) * aAngle - angle;
+     derivAngle = propAngle - derivAngle;
+     intAngle += (propAngle * DT);     {
+     speed = -((KP * propAngle) + (KI * intAngle) + (KD * derivAngle)) / scalingFactor;
+     if (speed < -1)
+     {
+         speed = -1;
+     }else if (speed > 1)
+     {
+         speed = 1;
+     }
+     
+     // Set the speed of the motors
+     left.speed(speed);
+     right.speed(speed);  
+}
+
 int main() {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 lol(p9, p10, 0xD6, 0x3C);
-    lol.begin();
-    if (!lol.begin()) {
+    
+    imu.begin();
+    if (!imu.begin()) 
+    {
         pc.printf("Failed to communicate with LSM9DS1.\n");
+        exit(0);
     }
-    lol.calibrate();
-    while(1) {
-        lol.readTemp();
-        lol.readMag();
-        lol.readGyro();
-        
-        //pc.printf("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
-        //pc.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        myled = 1;
-        wait(2);
-        myled = 0;
-        wait(2);
+    
+    // Calibrate the IMU and set the biases
+    calibrate();
+    
+    // Attach ticker to run every 10ms
+    // This handles the pid calculations for controlling the motors
+    PID.attach(&pid, 0.001);
+    
+    // Program loop
+    while(1)
+    {    
     }
 }