Air Mouse Project
Dependencies: ADXL345_I2C IMUfilter ITG3200_lib USBDevice mbed
Fork of IMU by
Diff: main.cpp
- Revision:
- 1:c8232c909f29
- Parent:
- 0:a070fa765ed2
--- a/main.cpp Thu Nov 18 01:19:16 2010 +0000 +++ b/main.cpp Sun Mar 23 22:07:18 2014 +0000 @@ -4,16 +4,21 @@ * Calculate the roll, pitch and yaw angles. */ #include "IMUfilter.h" -#include "LIS331.h" +#include "ADXL345_I2C.h" #include "ITG3200.h" +#include "USBMouse.h" + +#define MOVERATE 1 +#define MAXMOVE 100 +#define MOVETHRESHOLD 3 //Gravity at Earth's surface in m/s/s #define g0 9.812865328 //Number of samples to average. -#define SAMPLES 4 +#define SAMPLES 2 //Number of samples to be averaged for a null bias calculation //during calibration. -#define CALIBRATION_SAMPLES 128 +#define CALIBRATION_SAMPLES 32 //Convert from radians to degrees. #define toDegrees(x) (x * 57.2957795) //Convert from degrees to radians. @@ -22,24 +27,36 @@ #define GYROSCOPE_GAIN (1 / 14.375) //Full scale resolution on the ADXL345 is 4mg/LSB. #define ACCELEROMETER_GAIN (0.000061035 * g0) -//Sampling gyroscope at 200Hz. +//Sampling gyroscope at . #define GYRO_RATE 0.005 -//Sampling accelerometer at 200Hz. +//Sampling accelerometer at #define ACC_RATE 0.005 -//Updating filter at 40Hz. -#define FILTER_RATE 0.025 +#define FILTER_RATE 0.015 + +DigitalIn leftClick(p16); +DigitalIn rightClick(p15); Serial pc(USBTX, USBRX); //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); -LIS331 accelerometer(p9, p10); -ITG3200 gyroscope(p9, p10); +ADXL345_I2C accelerometer(p28, p27); +ITG3200 gyroscope(p28, p27); +USBMouse mouse; + Ticker accelerometerTicker; Ticker gyroscopeTicker; Ticker filterTicker; +/** + * IMU filter example. + * + * Calculate the roll, pitch and yaw angles. + */ + + + //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 @@ -95,22 +112,107 @@ void sampleGyroscope(void); //Update the filter and calculate the Euler angles. void filter(void); +void processMove(void); +void processClick(void); void initializeAccelerometer(void) { //Go into standby mode to configure the device. - //accelerometer.setPowerControl(0x00); + accelerometer.setPowerControl(0x00); //Full resolution, +/-16g, 4mg/LSB. - //accelerometer.setDataFormatControl(0x0B); + accelerometer.setDataFormatControl(0x0B); //200Hz data rate. - //accelerometer.setDataRate(ADXL345_200HZ); + accelerometer.setDataRate(ADXL345_1600HZ); //Measurement mode. - //accelerometer.setPowerControl(0x08); + accelerometer.setPowerControl(0x08); //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf wait_ms(22); } +void initializeGyroscope(void) { + + //Low pass filter bandwidth of 42Hz. + gyroscope.setLpBandwidth(LPFBW_42HZ); + gyroscope.setSampleRateDivider(0); + pc.printf("%d\n", gyroscope.getSampleRateDivider()); + pc.printf("%d\n", gyroscope.getInternalSampleRate()); + wait_ms(22); +} + + +void calibrateAccelerometer(void) { + + pc.printf("Calibrating Accelerometer\n"); + 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 - 981); + + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + pc.printf("Calibration Complete\n"); +} + + + +void calibrateGyroscope(void) { + + pc.printf("Calibrating Gyro\n"); + 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; + + pc.printf("Calibration Complete\n"); +} + + void sampleAccelerometer(void) { //Have we taken enough samples? @@ -129,90 +231,17 @@ } else { //Take another sample. - a_xAccumulator += (int16_t) accelerometer.getAccelX(); - a_yAccumulator += (int16_t) accelerometer.getAccelY(); - a_zAccumulator += (int16_t) accelerometer.getAccelZ(); + accelerometer.getOutput(readings); + a_xAccumulator += (int16_t) readings[0]; + a_yAccumulator += (int16_t) readings[1]; + a_zAccumulator += (int16_t) readings[2]; accelerometerSamples++; - + //pc.printf("Sample Accl %d", 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++) { - - a_xAccumulator += (int16_t) accelerometer.getAccelX(); - a_yAccumulator += (int16_t) accelerometer.getAccelY(); - a_zAccumulator += (int16_t) accelerometer.getAccelZ(); - - - 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 - 1000); - - 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) { @@ -237,13 +266,12 @@ w_zAccumulator += gyroscope.getGyroZ(); gyroscopeSamples++; - + //pc.printf("Sample Gyro %d", 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. @@ -251,6 +279,69 @@ } +void processClick() +{ + static bool preRightClick = false; + + if (leftClick == 0) + { + mouse.press(MOUSE_LEFT); + } + else + { + mouse.release(MOUSE_LEFT); + } + + // Right Mouse Click ___ Falling Edge Detection + if (rightClick == 0 && preRightClick == false) + { + preRightClick = true; + } + else if (rightClick == 1 && preRightClick == true) + { preRightClick = false; + mouse.click(MOUSE_RIGHT); + } +} + + +void processMove(void) +{ + int16_t move_x, move_y; + + + move_x = (int16_t)(-MOVERATE*toDegrees(imuFilter.getRoll())); + move_y = (int16_t)(-MOVERATE*toDegrees(imuFilter.getPitch())); + + if (move_x <= MOVETHRESHOLD && move_x >= -MOVETHRESHOLD) + move_x = 0; + else if (move_x > MOVETHRESHOLD){ + if (move_x > MAXMOVE+MOVETHRESHOLD) move_x = MAXMOVE; + else move_x -=MOVETHRESHOLD; + } + + else{ + if (move_x < -MAXMOVE-MOVETHRESHOLD) move_x = -MAXMOVE; + else move_x+=MOVETHRESHOLD; + } + + + if (move_y <= MOVETHRESHOLD && move_y >= -MOVETHRESHOLD) + move_y = 0; + else if (move_y > MOVETHRESHOLD){ + if (move_y > MAXMOVE+MOVETHRESHOLD) move_y = MAXMOVE; + else move_y -=MOVETHRESHOLD; + } + + else{ + if (move_y < -MAXMOVE-MOVETHRESHOLD) move_y = -MAXMOVE; + else move_y+=MOVETHRESHOLD; + } + + pc.printf("move_x = %d, move_ y = %d\n", move_x,move_y); + + mouse.move(move_x, move_y); + } + int main() { //pc.printf("Starting IMU filter test...\n"); @@ -261,29 +352,30 @@ initializeGyroscope(); calibrateGyroscope(); + + + leftClick.mode(PullUp); + rightClick.mode(PullUp); //pc.printf("Initialized Successfully...\n\r"); //Set up timers. //Accelerometer data rate is 200Hz, so we'll sample at this speed. - accelerometerTicker.attach(&sampleAccelerometer, 0.005); + accelerometerTicker.attach(&sampleAccelerometer, GYRO_RATE); //Gyroscope data rate is 200Hz, so we'll sample at this speed. - gyroscopeTicker.attach(&sampleGyroscope, 0.005); + gyroscopeTicker.attach(&sampleGyroscope, ACC_RATE); //Update the filter variables at the correct rate. filterTicker.attach(&filter, FILTER_RATE); //pc.printf("Timers Setup...Entering Loop...\n\r"); + while (1) { - wait(FILTER_RATE); - - - pc.printf("%f,%f,%f\n\r",toDegrees(imuFilter.getRoll()), - toDegrees(imuFilter.getPitch()), - toDegrees(imuFilter.getYaw())); - - + wait(0.01); + pc.printf("angle_x = %f, angle_ y = %f\n",-toDegrees(imuFilter.getRoll()),-toDegrees(imuFilter.getPitch())); + processClick(); + processMove(); } }