File content as of revision 9:b537a858040e:

#include "mbed.h"
#include "LSM9DS0.h"
#include "rtos.h"
#include "PID.h"
#include "motordriver.h"
#include "MODSERIAL.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.05 // Define rate for PID loop
//Multiply the linear and angular speed to change from [-1,1] to [-Multiplier, Multiplier]
#define LINEAR_MULTIPLIER 5
#define ANGULAR_MULTIPLIER 90

#define LINEAR_MAX_SPEED 3.0
#define ANGULAR_MAX_SPEED 90.0

Serial pc(USBTX, USBRX);
LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
Mutex pcMutex;
Mutex setPointMutex;
Mutex processValueMutex;
MODSERIAL xbee(p13, p14, 64, 128);
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);
int usePID = 1;
int commReceived = 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;

float leftMotorCO = 0;
float rightMotorCO = 0;


//setpoints from xbee
float setLinear = 0;
float setAngular = 0;
int enabled = 0;
int lin_reverse;
int ang_reverse;

/**
 * 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 debug(void const *args)
{
    while(1) {
        //pc.printf("linearOutput: %f    angularOutput: %f\n",linearOutput, angularOutput);
        //pc.printf("leftMotorCO: %f     rightMotorCO: %f\n", leftMotorCO, rightMotorCO);
        pc.printf("setLinear: %f     setAngular: %f\n", setLinear, setAngular);
        pc.printf("v_x: %f     angularSpeed: %f\n", v_x, w_z);
        pc.printf("linearOutput: %f     angularOutput: %f\n", linearOutput, angularOutput);
        pc.printf("leftMotorCO: %f     rightMotorCO: %f\n\n", leftMotorCO, rightMotorCO);
        Thread::wait(2000);
    }
}


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 parseCommand(char buff[], int length) {
    float *tempForward, *tempAngular;
    volatile char forwardBuffer [4];
    volatile char angularBuffer [4];
    int tempEnabled, tempPID;
    char statusChar;
    
    float *tempLinearPGain, *tempLinearIGain, *tempLinearDGain;
    float *tempAngularPGain, *tempAngularIGain, *tempAngularDGain;
    
    // Main control parsing
    if (0xCC == buff[0] && 10 == length) {
    
        tempForward = (float*) (buff + 1);
        tempAngular = (float*) (buff + 5);
        statusChar = buff[9]; //contains data for enable and enablePID
        tempPID = (int) (statusChar & 0x0F); //extract the last 4 bits and type cast to an int
        tempEnabled = (int) (statusChar & 0xF0); //extract the first 4 bits and type cast to an int

        //Change global variables
        setPointMutex.lock();
        setLinear = *tempForward;
        setAngular = *tempAngular;
        
        /*
        if (setLinear < 0) {
            setLinear = -setLinear; // make pos
            lin_reverse = 1;
        } else lin_reverse = 0;

        if (setAngular < 0) {
            setAngular = -setAngular; //make pos
            ang_reverse = 1;
        } else ang_reverse = 0;
        */
        enabled = tempEnabled;
        if (tempPID) {
            startPID();
        } else {
            stopPID();
        }
        setPointMutex.unlock();
    // Parsing change PID values command
    } else if (0x55 == buff[0] && 25 == length) {
        tempLinearPGain = (float*) (buff + 1);
        tempLinearIGain = (float*) (buff + 5);
        tempLinearDGain = (float*) (buff + 9);
        tempAngularPGain = (float*) (buff + 13);
        tempAngularIGain = (float*) (buff + 17);
        tempAngularDGain = (float*) (buff + 21);
        linearController.setTunings(*tempLinearPGain, *tempLinearIGain, *tempLinearDGain);
        angularController.setTunings(*tempAngularPGain, *tempAngularIGain, *tempAngularDGain);
    }

}

char waitForChar() {
    while (!xbee.readable()) {
        Thread::wait(10);
    }
    return xbee.getc();
}

void parseInput(void const *args)
{
    static int i = 0;
    static int length = 0;
    static char commBuffer [64];
    
    while (xbee.readable()) {
        xbee.getc();
    }
    
    while (1) {
        if (0x55 != waitForChar()) {
            continue;
        }
        if (0x55 != waitForChar()) {
            continue;
        }
        length = waitForChar();
        
        for (i = 0; i < length; ++i) {
            commBuffer[i] = waitForChar();
        }
        if (waitForChar() != 0xAA) {
            continue;
        }
        commReceived = 1;
        parseCommand(commBuffer, length);
        //Thread::wait(100);
    }
}


void checkCOMRecieved()
{
    if (commReceived == 0) {
        stopPID();
        enabled = 0;
        led1 = led2 = led3 = led4 = 1;
    } else {
        led1 = led2 = led3 = led4 = 0;
    }
    commReceived = 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) * g0;// * ACCELEROMETER_GAIN;
            a_y = -((a_yAccumulator / SAMPLES) - a_yBias) * g0;// * ACCELEROMETER_GAIN;
            a_z = -((a_zAccumulator / SAMPLES) - a_zBias) * g0;// * 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
    pc.printf("A_Bias     ax: %f     ay: %f     az: %f \n", a_xBias,a_yBias,a_zBias);
    /*
    pcMutex.lock();
    pc.printf("A_Bias     ax: %f     ay: %f     az: %f \n", a_xBias,a_yBias,a_zBias);
    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(-1.0, 1.0);
    angularController.setOutputLimits(-1.0, 1.0);
    linearController.setSetPoint(0.0);
    angularController.setSetPoint(0.0);
    linearController.setMode(AUTO_MODE);
    angularController.setMode(AUTO_MODE);

}


int main()
{
    //float linearOutput = 0;
    //float angularOutput = 0;

    /*
    float leftMotorCO = 0;
    float rightMotorCO = 0;
*/
    pc.printf("Starting IMU filter test...\n");
    setup();


    //Initialize inertial sensors.
    calibrateAccelerometer();
    calibrateGyroscope();

    Thread accelThread(sampleAccelerometer);
    Thread gyroThread(sampleGyroscope);
    Thread commThread(parseInput);
    Thread debugThread(debug);
    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) {
            setPointMutex.lock();
            linearController.setSetPoint(setLinear);
            angularController.setSetPoint(setAngular);
            setPointMutex.unlock();
            if (abs(w_z) < 2.0f) w_z = 0; // if less than 2 deg/sec, assume it is noise and make it eqal to 0.
            //v_mag2 = v_x * v_x + v_y * v_y;
            processValueMutex.lock();
            linearController.setProcessValue(v_x/LINEAR_MAX_SPEED);
            angularController.setProcessValue(w_z/ANGULAR_MAX_SPEED); // w_z = degrees per second
            processValueMutex.unlock();
            //pc.printf("linearOutput: %f",linearOutput);
            linearOutput = linearController.compute();
            angularOutput = angularController.compute();
            
            /*
            if (lin_reverse == 0) {
                linearOutput = linearController.compute();
            } else {
                linearOutput = -linearController.compute();
            }
            if (ang_reverse == 0) {
                angularOutput = angularController.compute();
            } else {
                angularOutput = -angularController.compute();
            }
            */
            
        } else {// PID Disabled
            linearOutput = setLinear;
            angularOutput = setAngular;
        }

        if (enabled) {

            leftMotorCO = (linearOutput - angularOutput);
            rightMotorCO = (linearOutput + angularOutput);

            leftMotor.speed(leftMotorCO);
            rightMotor.speed(rightMotorCO);


            /*
            leftMotor.speed(setLinear);
            rightMotor.speed(setAngular);
            */
        } else {
            leftMotor.coast();
            rightMotor.coast();
        }

        Thread::wait(RATE * 1000);
        /*
               //pcMutex.lock();
               pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
                         toDegrees(imuFilter.getPitch()),
                         toDegrees(imuFilter.getYaw()));
               //pcMutex.unlock();
        */
    }

}