Revision 1:0fd5faa076e7, committed 2012-02-23

Comitter:

jcytam

Date:

Thu Feb 23 23:01:36 2012 +0000

Parent:

0:0309bb86b703

Commit message:

Balancing Robot PS3

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JON_IMU.lib	Thu Feb 23 23:01:36 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/jcytam/code/JON_IMU/#35e67dd4c8a8

--- 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

--- a/JON_IMU/IMU.h	Thu Feb 23 22:59:51 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,51 +0,0 @@
-#ifndef IMU_H
-#define IMU_H
-
-//****************************************************************************/
-// Includes
-//****************************************************************************/
-#include "mbed.h"
-
-//****************************************************************************/
-// Defines
-//****************************************************************************/
-#define Rad2Deg   57.2957795
-#define ZERO_G    1.65
-
-class IMU {
-public:
-
-    IMU(PinName XGyro, PinName YAccel, PinName ZAccel, PinName AZ);
-
-    void initialise(void);
-    
-    void update(void);
-    
-    float getRoll(void);
-    float getGyrox(void);
-    float getAccelx(void);
-    
-private:
-    float X, Y, Z, Xrate, Yrate, Gyro_offset;
-    float GYROX_accom, GYROY_accom;
-    float AccAngleX, AccAngleY, AccAngleZ, GyroAngleX, GyroAngleY;
-    float Roll, Pitch;
-    Timer t;
-    
-    AnalogIn _XGyro;
-    AnalogIn _YAccel;
-    AnalogIn _ZAccel;
-    DigitalOut _AZ;
-
-    float reading[100];
-    float w, median ;
-    long middle, samples;
-    
-    void Median(void);
-};
-
-
-
-
-
-#endif
\ No newline at end of file