Diff: JON_IMU/IMU.cpp

Revision:

1:0fd5faa076e7

Parent:

0:0309bb86b703

--- a/JON_IMU/IMU.cpp	Thu Feb 23 22:59:51 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,89 +0,0 @@
-#include "IMU.h"
-#include "mbed.h"
-
-IMU::IMU(PinName XGyro, PinName YAccel, PinName ZAccel, PinName AZ):
-        _XGyro(XGyro), _YAccel(YAccel), _ZAccel(ZAccel), _AZ(AZ) {
-    _AZ = 0;
-}
-
-void IMU::initialise(void) {
-    _AZ = 1;
-    wait_us(1500);
-    _AZ = 0;
-    wait(0.01); //requires 7ms to calibrate
-
-    samples = 100;
-    for (int i=0; i<100; i++)    reading[i]=_XGyro.read();
-    Median();
-
-    Gyro_offset = median;
-    t.start();
-}
-
-//****************************************************************************/
-// Median Filter
-//****************************************************************************/
-void IMU::Median(void)  {
-    //sort the value of the array
-    for (int i=0; i< samples-1; i++) {
-        for (int j=i+1; j< samples; j++) {
-            if (reading[i] > reading[j]) {
-
-                w = reading[i];
-                reading[i] = reading[j];
-                reading[j] = w;
-            }
-        }
-
-    }
-
-    //find the middle value of the sample
-    middle = (samples+1)/2;
-    median = reading[middle];
-}
-
-void IMU::update(void) {
-
-    Xrate = -(_XGyro.read() -Gyro_offset)*3.3/0.002; //Voltage - Gyro_offset / Sensitivity
-    GyroAngleX += Xrate*t.read();
-
-    //Accelerometer Y
-    samples = 60;
-    for (int i=0; i<samples; i++)    reading[i]=_YAccel.read();
-    Median(); //100 point Median Filter
-
-    Y = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity ( + 0.03711048?)
-
-    //Accelerometer Z
-
-    samples = 60;
-    for (int i=0; i<samples; i++)    reading[i]=_ZAccel.read();
-    Median(); //100 point Median Filter
-
-    Z = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity
-
-    //Calculate Angle + map range
-    AccAngleX = Rad2Deg* atan2(-Y,-Z) - 90; //arctan = O/A, radian to degree
-
-    if (AccAngleX<-180)  AccAngleX=AccAngleX+360; //Maintain -180 to 180
-
-    //Complementary Filter
-    Roll = (0.98)*(Roll + Xrate*t.read()) + (0.02)*(AccAngleX);
-    t.reset();
-}
-
-
-float IMU::getRoll(void) {
-
-    return Roll;
-}
-
-float IMU::getGyrox(void) {
-
-    return GyroAngleX;
-}
-
-float IMU::getAccelx(void) {
-
-    return AccAngleX;
-}
\ No newline at end of file