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();
     }
 
 }