File content as of revision 0:58395e885409:

#include "mbed.h"
#include "LSM9DS0.h"
#include "rtos.h"
#include "PID.h"
#include "motordriver.h"
//#include "IMUfilter.h"

//Gravity at Earth's surface in m/s/s
#define g0 9.812865328
//Number of samples to average.
#define SAMPLES 4
//Number of samples to be averaged for a null bias calculation
//during calibration.
#define CALIBRATION_SAMPLES 1024
//Convert from radians to degrees.
#define toDegrees(x) (x * 57.2957795)
//Convert from degrees to radians.
#define toRadians(x) (x * 0.01745329252)
//LSM9DS0 gyroscope sensitivity is 8.75 (millidegrees/sec)/digit when set to +-2G
#define GYROSCOPE_GAIN (0.007476806640625)
//Full scale resolution on the accelerometer is 0.0001220703125g/LSB.
#define ACCELEROMETER_GAIN (0.00006103515625 * g0)
//Sampling gyroscope at 100Hz.
#define GYRO_RATE   0.01
//Sampling accelerometer at 200Hz.
#define ACC_RATE    0.005
// SDO_XM and SDO_G are both grounded, so our addresses are:
#define LSM9DS0_XM  0x1E // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G   0x6A // Would be 0x6A if SDO_G is LOW
#define RATE  0.1 // Define rate for PID loop


Serial pc(USBTX, USBRX);
LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
Mutex pcMutex;
PID linearController(1.0, 0.0, 0.0, RATE);
PID angularController(1.0, 0.0, 0.0, RATE);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
bool usePID = 1;
bool commRecieved = 0;

//Gloabal Variables for comm system below
Ticker commTicker;


//Global Variables for motor control below
Motor  leftMotor(p21, p22, p23, 1); // pwm, fwd, rev
Motor rightMotor(p26, p25, p24, 1); // pwm, fwd, rev

//Offsets for the gyroscope.
//The readings we take when the gyroscope is stationary won't be 0, so we'll
//average a set of readings we do get when the gyroscope is stationary and
//take those away from subsequent readings to ensure the gyroscope is offset
//or "biased" to 0.
double w_xBias;
double w_yBias;
double w_zBias;
 
//Offsets for the accelerometer.
//Same as with the gyroscope.
double a_xBias;
double a_yBias;
double a_zBias;
 
//Accumulators used for oversampling and then averaging.
double a_xAccumulator = 0;
double a_yAccumulator = 0;
double a_zAccumulator = 0;
double w_xAccumulator = 0;
double w_yAccumulator = 0;
double w_zAccumulator = 0;
 
//Accelerometer and gyroscope readings for x, y, z axes.
double a_x;
double a_y;
double a_z;
double w_x;
double w_y;
double w_z;

//Previous Acceleromerter and gyroscope readings for integration
double last_a_x;
double last_a_y;
double last_a_z;
 
//Buffer for accelerometer readings.
int readings[3];
//Number of accelerometer samples we're on.
int accelerometerSamples = 0;
//Number of gyroscope samples we're on.
int gyroscopeSamples = 0;

//current readings of linear velocity
double v_x = 0;
double v_y = 0;
double v_z = 0;
double v_mag2 = 0; // linear velocity squared. sqrt() is comupationally difficult
 
//values for pwm wave to motors
float linearOutput = 0;
float angularOutput = 0;
/**
 * Prototypes
 */
//Calculate the null bias.
void calibrateAccelerometer(void);
//Take a set of samples and average them.
void sampleAccelerometer(void);
//Calculate the null bias.
void calibrateGyroscope(void);
//Take a set of samples and average them.
void sampleGyroscope(void);


 
void parseInput(void const *args) {
 while(1) {
     // Andrew to parse code here
     
     // if: manual boat control
     // stopPID();
     //leftMotor.speed(intput [-1,1]);
     //rightMotor.speed(input [-1,1]);
     
     //if come back from coast stop
     // startPID();

     
     //if coast
     //
     //stopPID();
     //leftMotor.coast();
     //rightMotor.coast();
     commRecieved = 1;
     }
 }
 
void startPID() {
    linearController.setMode(AUTO_MODE);
    angularController.setMode(AUTO_MODE);
    usePID = 1;
    }
    
void stopPID() {
    linearController.setMode(MANUAL_MODE);
    angularController.setMode(MANUAL_MODE);
    usePID = 0;
    }

void checkCOMRecieved() {
 if (commRecieved == 0) {
    stopPID();
    leftMotor.coast();
    rightMotor.coast();
    led1 = led2 = led3 = led4 = 1;
    }
     commRecieved = 0;
}

void sampleAccelerometer(void const *args) {
 while(1) {
    while(usePID != 1) {
    Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
    }
    //Have we taken enough samples?
    if (accelerometerSamples == SAMPLES) {
 
        //Average the samples, remove the bias, and calculate the acceleration
        //in m/s/s.
        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
 
        a_xAccumulator = 0;
        a_yAccumulator = 0;
        a_zAccumulator = 0;
        accelerometerSamples = 0;
        pcMutex.lock();
        pc.printf("a_x: %f  a_y: %f  a_z: %f \n",a_x,a_y,a_z);
        pcMutex.unlock();
        
        // integrate to get velocity. make sure to subtract gravity downwards!
        
        v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES;
        v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES;
        v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES;
        
        last_a_x = a_x;
        last_a_y = a_y;
        last_a_z = a_z;
        pcMutex.lock();
        pc.printf("v_x: %f  v_y: %f  v_z: %f \n",v_x,v_y,v_z);
        pcMutex.unlock();
 
    } else {
        //Take another sample.
        imu.readAccel();
        pcMutex.lock();
        pc.printf("A: ");
        pc.printf("%d", imu.ax);
        pc.printf(", ");
        pc.printf("%d",imu.ay);
        pc.printf(", ");
        pc.printf("%d\n",imu.az);
        pcMutex.unlock();
        
        a_xAccumulator += imu.ax;
        a_yAccumulator += imu.ay;
        a_zAccumulator += imu.az;
 
        accelerometerSamples++;
        Thread::wait(ACC_RATE * 1000);
    }


}
}
 
void calibrateAccelerometer(void) {

    led1 = 1;
    
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    
    
    //Take a number of readings and average them
    //to calculate the zero g offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
 
        imu.readAccel();
 
        a_xAccumulator += (double) imu.ax;
        a_yAccumulator += (double) imu.ay;
        a_zAccumulator += (double) imu.az;

        wait(ACC_RATE);
 
    }
 
    a_xAccumulator /= CALIBRATION_SAMPLES;
    a_yAccumulator /= CALIBRATION_SAMPLES;
    a_zAccumulator /= CALIBRATION_SAMPLES;
 
    //At 4mg/LSB, 250 LSBs is 1g.
    a_xBias = a_xAccumulator;
    a_yBias = a_yAccumulator;
    //a_zBias = (a_zAccumulator - (1/0.00006103515625));
    a_zBias = a_zAccumulator; // calibrate so that gravity is already out of the equation
    
    pcMutex.lock();
    pc.printf("A_Bias: ");
    pc.printf("%f", a_xBias);
    pc.printf(", ");
    pc.printf("%f",a_yBias);
    pc.printf(", ");
    pc.printf("%f\n",a_zBias);
    pcMutex.unlock();
    
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    pc.printf("done calibrating accel\n");
    led1 = 0;
}
 
 
void calibrateGyroscope(void) {
    led2 = 1;
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
 
    //Take a number of readings and average them
    //to calculate the gyroscope bias offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
        imu.readGyro();
        w_xAccumulator += (double) imu.gx;
        w_yAccumulator += (double) imu.gy;
        w_zAccumulator += (double) imu.gz;
        Thread::wait(GYRO_RATE * 1000);
 
    }
 
    //Average the samples.
    w_xAccumulator /= CALIBRATION_SAMPLES;
    w_yAccumulator /= CALIBRATION_SAMPLES;
    w_zAccumulator /= CALIBRATION_SAMPLES;
 
    w_xBias = w_xAccumulator;
    w_yBias = w_yAccumulator;
    w_zBias = w_zAccumulator;
     
    pcMutex.lock();
    pc.printf("G_Bias: ");
    pc.printf("%f", w_xBias);
    pc.printf(", ");
    pc.printf("%f",w_yBias);
    pc.printf(", ");
    pc.printf("%f\n",w_zBias);
    pcMutex.unlock();
    
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
    pc.printf("done calibrating gyro\n");
    led2 = 0;
}
 
void sampleGyroscope(void const *args) {
 while(1){
    while(usePID != 1) {
        Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
        }
    //Have we taken enough samples?
    if (gyroscopeSamples == SAMPLES) {
 
        //Average the samples, remove the bias, and calculate the angular
        //velocity in deg/s.
        w_x = ((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN;
        w_y = ((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN;
        w_z = ((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN;
        pcMutex.lock();
        pc.printf("w_x: %f  w_y: %f  w_z: %f \n",w_x,w_y,w_z);
        pcMutex.unlock();
        w_xAccumulator = 0;
        w_yAccumulator = 0;
        w_zAccumulator = 0;
        gyroscopeSamples = 0;
 
    } else {
        imu.readGyro();
        pcMutex.lock();
        pc.printf("G: ");
        pc.printf("%d", imu.gx);
        pc.printf(", ");
        pc.printf("%d",imu.gy);
        pc.printf(", ");
        pc.printf("%d\n",imu.gz);
        pcMutex.unlock();
        
        w_xAccumulator += imu.gx;
        w_yAccumulator += imu.gy;
        w_zAccumulator += imu.gz;
        
        gyroscopeSamples++;
        Thread::wait(GYRO_RATE * 1000);
    }

Thread::wait(GYRO_RATE * 1000);
}
}
 

 
 
 void setup()
{
    pc.baud(9600); // Start serial at 115200 bps
    // Use the begin() function to initialize the LSM9DS0 library.
    // You can either call it with no parameters (the easy way):
    //uint16_t status = dof.begin();
    
    //Or call it with declarations for sensor scales and data rates:
    //uint16_t status = imu.begin(dof.G_SCALE_2000DPS,
    //                            dof.A_SCALE_2G, dof.M_SCALE_2GS);
                                
    uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G, imu.M_SCALE_2GS,
                                imu.G_ODR_760_BW_100, imu.A_ODR_400, imu.M_ODR_50);
    
    // begin() returns a 16-bit value which includes both the gyro
    // and accelerometers WHO_AM_I response. You can check this to
    // make sure communication was successful.
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x");
    pc.printf("%x\n",status);
    pc.printf("Should be 0x49D4\n");
    pc.printf("\n");
    
    // Set up PID Loops
    linearController.setInputLimits(-1.0, 1.0);
    angularController.setInputLimits(-1.0, 1.0);
    linearController.setOutputLimits(0.0, 1.0);
    angularController.setOutputLimits(0.0, 1.0);
    linearController.setSetPoint(0.0);
    angularController.setSetPoint(0.0);
    linearController.setMode(AUTO_MODE);
    angularController.setMode(AUTO_MODE);
    
}


int main() {
 
    pc.printf("Starting IMU filter test...\n");
    setup();
    
    
    //Initialize inertial sensors.
    calibrateAccelerometer();
    calibrateGyroscope();

    Thread accelThread(sampleAccelerometer);
    Thread gyroThread(sampleGyroscope);
    Thread commThread(parseInput);
    commTicker.attach(&checkCOMRecieved, 2.0); // the address of the function to be attached (checkCOMRecieved) and the interval (2 seconds)
    /*
    pcMutex.lock();
   pc.printf("done attaching tickers\n\n");
   pcMutex.unlock();
*/
    while (1) {
        //pc.printf("in loop\n");    
        if(usePID != 1) {
            v_mag2 = v_x * v_x + v_y * v_y;
            linearController.setProcessValue(v_mag2);
            angularController.setProcessValue(w_z); // w_z = degrees per second 
            linearOutput = linearController.compute();
            angularOutput = angularController.compute();
            float leftMotorCO = 0.5 * (linearOutput + angularOutput);
            float rightMotorCO = 0.5 * (linearOutput - angularOutput);
            leftMotor.speed(leftMotorCO);
            rightMotor.speed(rightMotorCO);
            }
        Thread::wait(RATE * 1000);
 /*
        pcMutex.lock();
        pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
                  toDegrees(imuFilter.getPitch()),
                  toDegrees(imuFilter.getYaw()));
        pcMutex.unlock();
*/
    }

}