Diff: main.cpp

Revision:

0:58395e885409

Child:

4:6f6a9602e92d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 28 20:02:30 2015 +0000
@@ -0,0 +1,438 @@
+#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();
+*/
+    }
+
+}
\ No newline at end of file