main.cpp

00001 #include "IMUfilter.h"
00002 
00003 Serial pc(USBTX, USBRX);
00004 
00005 //Change the second parameter (which is a tuning parameter)
00006 //to try and get optimal results. 
00007 IMUfilter imuFilter(0.1, 10);
00008 
00009 AnalogIn accelerometerX(p15);
00010 AnalogIn accelerometerY(p16);
00011 AnalogIn accelerometerZ(p17);
00012 AnalogIn gyroscopeX(p18);
00013 AnalogIn gyroscopeY(p19);
00014 AnalogIn gyroscopeZ(p20);
00015 
00016 int main() {
00017 
00018     while(1){
00019     
00020         wait(0.1);
00021         //Gyro and accelerometer values must be converted to rad/s and m/s/s
00022         //before being passed to the filter.
00023         imuFilter.updateFilter(gyroscopeX.read(),
00024                                gyroscopeY.read(),
00025                                gyroscopeZ.read(),
00026                                accelerometerX.read(),
00027                                accelerometerY.read(),
00028                                accelerometerZ.read());
00029         imuFilter.computeEuler();
00030         pc.printf("%f, %f, %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
00031     
00032     }
00033 
00034 }