main.cpp

00001 #include "ADXL345_I2C.h"
00002 #include "ADXL345HL.h"
00003 #include "ITG3200HL.h"
00004 
00005 #include "IMUfilter.h"
00006 
00007 
00008 //ADXL345_I2C accelerometer(p28, p27);
00009 ADXL345HL accel;
00010 ITG3200HL gyro;
00011 //ITG3200 gyro(p28, p27);
00012 Serial pc(USBTX, USBRX);
00013 IMUfilter imuFilter(0.1, 10);
00014 
00015 int main()
00016 {
00017     accel.init(20,4,0.004);
00018     accel.calibrateAccelerometer();
00019     gyro.init(20,4,0.004);
00020     gyro.calibrate();
00021     while (1) {
00022 
00023         wait(0.1);
00024         double* accelreadings=accel.sampleAccelerometer();
00025         double* gyroreadings=gyro.sample();
00026         imuFilter.updateFilter(gyroreadings[0],gyroreadings[1],gyroreadings[2],accelreadings[0],accelreadings[1],accelreadings[2]);
00027         imuFilter.computeEuler();
00028         pc.printf("roll: %f, pitch: %f, yaw: %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
00029         //pc.printf("aceelerometer: %f, %f, %f\n", readings[0], readings[1], readings[2]);
00030         //pc.printf("gyro: %i, %i, %i\n", gyros[0],gyros[1],gyros[2]);
00031     }
00032 
00033 }