Jonathan Tam / Mbed 2 deprecated BalancingRobot_NOINTERRUPTS_OOP_PS3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

JON_IMU/IMU.cpp@0:0309bb86b703, 2012-02-23 (annotated)

Committer:
jcytam
Date:
Thu Feb 23 22:59:51 2012 +0000
Revision:
0:0309bb86b703
balancing robot ps3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jcytam 0:0309bb86b703 1 #include "IMU.h"
jcytam 0:0309bb86b703 2 #include "mbed.h"
jcytam 0:0309bb86b703 3
jcytam 0:0309bb86b703 4 IMU::IMU(PinName XGyro, PinName YAccel, PinName ZAccel, PinName AZ):
jcytam 0:0309bb86b703 5 _XGyro(XGyro), _YAccel(YAccel), _ZAccel(ZAccel), _AZ(AZ) {
jcytam 0:0309bb86b703 6 _AZ = 0;
jcytam 0:0309bb86b703 7 }
jcytam 0:0309bb86b703 8
jcytam 0:0309bb86b703 9 void IMU::initialise(void) {
jcytam 0:0309bb86b703 10 _AZ = 1;
jcytam 0:0309bb86b703 11 wait_us(1500);
jcytam 0:0309bb86b703 12 _AZ = 0;
jcytam 0:0309bb86b703 13 wait(0.01); //requires 7ms to calibrate
jcytam 0:0309bb86b703 14
jcytam 0:0309bb86b703 15 samples = 100;
jcytam 0:0309bb86b703 16 for (int i=0; i<100; i++) reading[i]=_XGyro.read();
jcytam 0:0309bb86b703 17 Median();
jcytam 0:0309bb86b703 18
jcytam 0:0309bb86b703 19 Gyro_offset = median;
jcytam 0:0309bb86b703 20 t.start();
jcytam 0:0309bb86b703 21 }
jcytam 0:0309bb86b703 22
jcytam 0:0309bb86b703 23 //****************************************************************************/
jcytam 0:0309bb86b703 24 // Median Filter
jcytam 0:0309bb86b703 25 //****************************************************************************/
jcytam 0:0309bb86b703 26 void IMU::Median(void) {
jcytam 0:0309bb86b703 27 //sort the value of the array
jcytam 0:0309bb86b703 28 for (int i=0; i< samples-1; i++) {
jcytam 0:0309bb86b703 29 for (int j=i+1; j< samples; j++) {
jcytam 0:0309bb86b703 30 if (reading[i] > reading[j]) {
jcytam 0:0309bb86b703 31
jcytam 0:0309bb86b703 32 w = reading[i];
jcytam 0:0309bb86b703 33 reading[i] = reading[j];
jcytam 0:0309bb86b703 34 reading[j] = w;
jcytam 0:0309bb86b703 35 }
jcytam 0:0309bb86b703 36 }
jcytam 0:0309bb86b703 37
jcytam 0:0309bb86b703 38 }
jcytam 0:0309bb86b703 39
jcytam 0:0309bb86b703 40 //find the middle value of the sample
jcytam 0:0309bb86b703 41 middle = (samples+1)/2;
jcytam 0:0309bb86b703 42 median = reading[middle];
jcytam 0:0309bb86b703 43 }
jcytam 0:0309bb86b703 44
jcytam 0:0309bb86b703 45 void IMU::update(void) {
jcytam 0:0309bb86b703 46
jcytam 0:0309bb86b703 47 Xrate = -(_XGyro.read() -Gyro_offset)*3.3/0.002; //Voltage - Gyro_offset / Sensitivity
jcytam 0:0309bb86b703 48 GyroAngleX += Xrate*t.read();
jcytam 0:0309bb86b703 49
jcytam 0:0309bb86b703 50 //Accelerometer Y
jcytam 0:0309bb86b703 51 samples = 60;
jcytam 0:0309bb86b703 52 for (int i=0; i<samples; i++) reading[i]=_YAccel.read();
jcytam 0:0309bb86b703 53 Median(); //100 point Median Filter
jcytam 0:0309bb86b703 54
jcytam 0:0309bb86b703 55 Y = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity ( + 0.03711048?)
jcytam 0:0309bb86b703 56
jcytam 0:0309bb86b703 57 //Accelerometer Z
jcytam 0:0309bb86b703 58
jcytam 0:0309bb86b703 59 samples = 60;
jcytam 0:0309bb86b703 60 for (int i=0; i<samples; i++) reading[i]=_ZAccel.read();
jcytam 0:0309bb86b703 61 Median(); //100 point Median Filter
jcytam 0:0309bb86b703 62
jcytam 0:0309bb86b703 63 Z = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity
jcytam 0:0309bb86b703 64
jcytam 0:0309bb86b703 65 //Calculate Angle + map range
jcytam 0:0309bb86b703 66 AccAngleX = Rad2Deg* atan2(-Y,-Z) - 90; //arctan = O/A, radian to degree
jcytam 0:0309bb86b703 67
jcytam 0:0309bb86b703 68 if (AccAngleX<-180) AccAngleX=AccAngleX+360; //Maintain -180 to 180
jcytam 0:0309bb86b703 69
jcytam 0:0309bb86b703 70 //Complementary Filter
jcytam 0:0309bb86b703 71 Roll = (0.98)*(Roll + Xrate*t.read()) + (0.02)*(AccAngleX);
jcytam 0:0309bb86b703 72 t.reset();
jcytam 0:0309bb86b703 73 }
jcytam 0:0309bb86b703 74
jcytam 0:0309bb86b703 75
jcytam 0:0309bb86b703 76 float IMU::getRoll(void) {
jcytam 0:0309bb86b703 77
jcytam 0:0309bb86b703 78 return Roll;
jcytam 0:0309bb86b703 79 }
jcytam 0:0309bb86b703 80
jcytam 0:0309bb86b703 81 float IMU::getGyrox(void) {
jcytam 0:0309bb86b703 82
jcytam 0:0309bb86b703 83 return GyroAngleX;
jcytam 0:0309bb86b703 84 }
jcytam 0:0309bb86b703 85
jcytam 0:0309bb86b703 86 float IMU::getAccelx(void) {
jcytam 0:0309bb86b703 87
jcytam 0:0309bb86b703 88 return AccAngleX;
jcytam 0:0309bb86b703 89 }