Revision 0:84d5aa80fd77, committed 2014-04-27

Comitter:

Strikewolf

Date:

Sun Apr 27 04:31:07 2014 +0000

Child:

1:dacf7db790f6

Commit message:

-v1.0 of human controlled car;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_RPY.h	Sun Apr 27 04:31:07 2014 +0000
@@ -0,0 +1,281 @@
+    
+
+    /**
+     * IMU filter example.
+     *
+     * Calculate the roll, pitch and yaw angles.
+     */
+    #include "IMUfilter.h"
+    #include "ADXL345_I2C.h"
+    #include "ITG3200.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 128
+    //Convert from radians to degrees.
+    #define toDegrees(x) (x * 57.2957795)
+    //Convert from degrees to radians.
+    #define toRadians(x) (x * 0.01745329252)
+    //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+    #define GYROSCOPE_GAIN (1 / 14.375)
+    //Full scale resolution on the ADXL345 is 4mg/LSB.
+    #define ACCELEROMETER_GAIN (0.004 * g0)
+    //Sampling gyroscope at 200Hz.
+    #define GYRO_RATE   0.005
+    //Sampling accelerometer at 200Hz.
+    #define ACC_RATE    0.005
+    //Updating filter at 40Hz.
+    #define FILTER_RATE 0.1
+     
+    //At rest the gyroscope is centred around 0 and goes between about
+    //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
+    //5/15 = 0.3 degrees/sec.
+    IMUfilter imuFilter(FILTER_RATE, 0.3);
+    ADXL345_I2C accelerometer(p28, p27);
+    ITG3200 gyroscope(p28, p27);
+    Ticker accelerometerTicker;
+    Ticker gyroscopeTicker;
+    Ticker filterTicker;
+     
+    //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.
+    volatile double a_xAccumulator = 0;
+    volatile double a_yAccumulator = 0;
+    volatile double a_zAccumulator = 0;
+    volatile double w_xAccumulator = 0;
+    volatile double w_yAccumulator = 0;
+    volatile double w_zAccumulator = 0;
+     
+    //Accelerometer and gyroscope readings for x, y, z axes.
+    volatile double a_x;
+    volatile double a_y;
+    volatile double a_z;
+    volatile double w_x;
+    volatile double w_y;
+    volatile double w_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;
+     
+    /**
+     * Prototypes
+     */
+    //Set up the ADXL345 appropriately.
+    void initializeAcceleromter(void);
+    //Calculate the null bias.
+    void calibrateAccelerometer(void);
+    //Take a set of samples and average them.
+    void sampleAccelerometer(void);
+    //Set up the ITG3200 appropriately.
+    void initializeGyroscope(void);
+    //Calculate the null bias.
+    void calibrateGyroscope(void);
+    //Take a set of samples and average them.
+    void sampleGyroscope(void);
+    //Update the filter and calculate the Euler angles.
+    void filter(void);
+     
+    void initializeAccelerometer(void)
+    {
+     
+        //Go into standby mode to configure the device.
+        accelerometer.setPowerControl(0x00);
+        //Full resolution, +/-16g, 4mg/LSB.
+        accelerometer.setDataFormatControl(0x0B);
+        //200Hz data rate.
+        accelerometer.setDataRate(ADXL345_200HZ);
+        //Measurement mode.
+        accelerometer.setPowerControl(0x08);
+        //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+        wait_ms(22);
+     
+    }
+     
+    void sampleAccelerometer(void)
+    {
+     
+        //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;
+     
+        } else {
+            //Take another sample.
+            accelerometer.getOutput(readings);
+     
+            a_xAccumulator += (int16_t) readings[0];
+            a_yAccumulator += (int16_t) readings[1];
+            a_zAccumulator += (int16_t) readings[2];
+     
+            accelerometerSamples++;
+     
+        }
+     
+    }
+     
+    void calibrateAccelerometer(void)
+    {
+     
+        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++) {
+     
+            accelerometer.getOutput(readings);
+     
+            a_xAccumulator += (int16_t) readings[0];
+            a_yAccumulator += (int16_t) readings[1];
+            a_zAccumulator += (int16_t) readings[2];
+     
+            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 - 250);
+     
+        a_xAccumulator = 0;
+        a_yAccumulator = 0;
+        a_zAccumulator = 0;
+     
+    }
+     
+    void initializeGyroscope(void)
+    {
+     
+        //Low pass filter bandwidth of 42Hz.
+        gyroscope.setLpBandwidth(LPFBW_42HZ);
+        //Internal sample rate of 200Hz. (1kHz / 5).
+        gyroscope.setSampleRateDivider(4);
+     
+    }
+     
+    void calibrateGyroscope(void)
+    {
+     
+        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++) {
+     
+            w_xAccumulator += gyroscope.getGyroX();
+            w_yAccumulator += gyroscope.getGyroY();
+            w_zAccumulator += gyroscope.getGyroZ();
+            wait(GYRO_RATE);
+     
+        }
+     
+        //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;
+     
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+     
+    }
+     
+    void sampleGyroscope(void)
+    {
+     
+        //Have we taken enough samples?
+        if (gyroscopeSamples == SAMPLES) {
+     
+            //Average the samples, remove the bias, and calculate the angular
+            //velocity in rad/s.
+            w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+            w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+            w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+     
+            w_xAccumulator = 0;
+            w_yAccumulator = 0;
+            w_zAccumulator = 0;
+            gyroscopeSamples = 0;
+     
+        } else {
+            //Take another sample.
+            w_xAccumulator += gyroscope.getGyroX();
+            w_yAccumulator += gyroscope.getGyroY();
+            w_zAccumulator += gyroscope.getGyroZ();
+     
+            gyroscopeSamples++;
+     
+        }
+     
+    }
+     
+    void filter(void)
+    {
+     
+        //Update the filter variables.
+        imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
+        //Calculate the new Euler angles.
+        imuFilter.computeEuler();
+     
+    }
+     
+    void rpy_init()
+    {
+        //Initialize inertial sensors.
+        initializeAccelerometer();
+        calibrateAccelerometer();
+        initializeGyroscope();
+        //calibrateGyroscope();
+     
+     
+       /* //Set up timers.
+        //Accelerometer data rate is 200Hz, so we'll sample at this speed.
+        accelerometerTicker.attach(&sampleAccelerometer, 0.005);
+        //Gyroscope data rate is 200Hz, so we'll sample at this speed.
+        gyroscopeTicker.attach(&sampleGyroscope, 0.005);
+        //Update the filter variables at the correct rate.
+        filterTicker.attach(&filter, FILTER_RATE);*/
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Sun Apr 27 04:31:07 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Apr 27 04:31:07 2014 +0000
@@ -0,0 +1,203 @@
+#include "mbed.h"
+
+//Radio Calibration Signals - Need to implement calibration routine...
+#define CHAN2_MID 1470  //Elevator Midpoint
+#define CHAN1_MID 1470  //Rudder Midpoint
+#define DEADZONE 105
+#define MAXMIN_OFFSET 400
+
+//Drive Modes
+typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
+DRIVEMODE mode;
+float xpwm, ypwm;
+
+//H-bridge
+PwmOut rightMotorPWM(p22);  //Channel A
+PwmOut leftMotorPWM(p21);   //Channel B
+DigitalOut leftMotor1(p23);
+DigitalOut leftMotor2(p24);
+DigitalOut rightMotor1(p25);
+DigitalOut rightMotor2(p26);
+
+//Radio 
+Serial pc(USBTX, USBRX);
+DigitalIn rudder(p11);
+DigitalIn elevator(p12);
+
+//Xbee
+Serial xbee(p13, p14);
+DigitalOut xbeeReset(p15);
+
+void setTurnLeft(float speed) {
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+    
+    //Set motor function
+    leftMotor1 = 0;
+    leftMotor2 = 1;
+    rightMotor1 = 1;
+    rightMotor2 = 0;
+}
+
+void setTurnRight(float speed) {
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+    
+    //Set motor function
+    leftMotor1 = 1;
+    leftMotor2 = 0;
+    rightMotor1 = 0;
+    rightMotor2 = 1;
+}
+
+//Stop with braking
+void stop() {
+    rightMotorPWM = 0;
+    leftMotorPWM = 0;
+    leftMotor1 = 0;
+    leftMotor2 = 1;
+    rightMotor1 = 1;
+    rightMotor2 = 0;
+}
+
+
+int getRadioX() {
+    Timer timer;
+    timer.reset();
+    int dur;
+    while(!rudder);
+    timer.start();
+    while(rudder);
+    dur = timer.read_us();
+    return dur;
+}
+
+int getRadioY() {
+    Timer timer;
+    timer.reset();
+    int dur;
+    while(!elevator);
+    timer.start();
+    while(elevator);
+    dur = timer.read_us();
+    return dur;
+}
+
+//Primary function for setting motors for radio driving
+void radioDrive() {
+    int radioY = getRadioY();
+    int radioX = getRadioX();
+    
+    //Identify drive mode from radio values
+    if (radioY < CHAN2_MID - DEADZONE)
+        mode = FORWARD;
+    if (radioY > CHAN2_MID + DEADZONE)
+        mode = REVERSE;
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+        radioX < CHAN1_MID - DEADZONE)
+        mode = CENTERLEFT;
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+        radioX > CHAN1_MID + DEADZONE)
+        mode = CENTERRIGHT;        
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+        radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
+        mode = STOP;
+        
+    //Normalize values for PWM magnitude
+    xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
+    ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
+        
+    //Set Motors Accordingly
+    switch(mode) {
+        case FORWARD:
+            //Handle variable turns
+            if (radioX > CHAN1_MID + DEADZONE) {
+                rightMotorPWM = ypwm - 0.8*xpwm;
+                leftMotorPWM = ypwm;
+            } else if (radioX < CHAN1_MID - DEADZONE) {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm - 0.8*xpwm;
+            } else {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm; 
+            }
+            leftMotor1 =  0;
+            leftMotor2 = 1;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+            break;
+        
+        case REVERSE:
+            //Handle variable turns
+            if (radioX > CHAN1_MID + DEADZONE) {
+                rightMotorPWM = ypwm - 0.8*xpwm;
+                leftMotorPWM = ypwm;
+            } else if (radioX < CHAN1_MID - DEADZONE) {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm - 0.8*xpwm;
+            } else {
+                rightMotorPWM = ypwm;
+                leftMotorPWM = ypwm; 
+            }
+            leftMotor1 =  1;
+            leftMotor2 = 0;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+            
+        case CENTERRIGHT:
+            rightMotorPWM = xpwm;
+            leftMotorPWM = xpwm;
+            leftMotor1 = 1;
+            leftMotor2 = 0;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+            break;
+
+        case CENTERLEFT:
+            rightMotorPWM = xpwm;
+            leftMotorPWM = xpwm;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+
+        case STOP:
+            rightMotorPWM = 0;
+            leftMotorPWM = 0;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+    }
+}
+
+//Transmit position and heading
+void xbeeTelemetry() {
+    xbee.printf("Human Update \r\n");   
+    wait_ms(10);
+}
+
+bool isGameOver() {
+    return xbee.readable();
+}
+        
+
+
+int main() {
+    
+    //Init Xbee
+    xbeeReset = 0;
+    wait_ms(1);
+    xbeeReset = 1;
+    wait_ms(1);
+    
+    while(1) {
+        xbee.printf("HELLO!\r\n");
+        wait(0.1);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Apr 27 04:31:07 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6473597d706e
\ No newline at end of file