main.cpp

00001 #include "mbed.h"
00002 #include "ADXL345_I2C.h"
00003 #include "ITG3200.h"
00004 #include "HMC5883L.h"
00005 #include "SDFileSystem.h"
00006 
00007 ADXL345_I2C accelerometer(p9, p10);
00008 ITG3200 gyro(p9, p10);
00009 HMC5883L compass(p9, p10);
00010 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00011 DigitalIn stop(p20);
00012 Serial pc(USBTX, USBRX);
00013 
00014 #define N 3
00015 
00016 int preparing_acc();
00017 
00018 int main(){ 
00019     
00020     float dt = 0.01;
00021     float t  = 0;    
00022     pc.baud(921600);
00023     int    bitAcc[N]    =   {0};    // Buffer of the accelerometer
00024     int    getMag[N]    =   {0};    // Buffer of the compass
00025     double Acc  [N]    =   {0};
00026     double Gyro [N]    =   {0};
00027     int    Mag  [N]    =   {0};
00028     
00029     // SD file system
00030     mkdir("/sd/mydir", 0777);
00031      
00032     FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
00033     if( fp==NULL ){     error("Could not open file for write\n");   }
00034     
00035     // *** start up ***
00036     // These are here to test whether any of the initialization fails. It will print the failure.
00037     if (accelerometer.setPowerControl(0x00)){
00038         pc.printf("didn't intitialize power control\n\r"); 
00039         return 0;
00040     }
00041     // Full resolution, +/-16g, 4mg/LSB.
00042     wait(.001);
00043      
00044     if(accelerometer.setDataFormatControl(0x0B)){
00045         pc.printf("didn't set data format\n\r");
00046         return 0;
00047     }
00048     wait(.001);
00049      
00050     // 3.2kHz data rate.
00051     if(accelerometer.setDataRate(ADXL345_3200HZ)){
00052         pc.printf("didn't set data rate\n\r");
00053         return 0;
00054     }
00055     wait(.001);
00056       
00057     if(accelerometer.setPowerControl(MeasurementMode)) {
00058         pc.printf("didn't set the power control to measurement\n\r"); 
00059         return 0;
00060     }
00061 
00062     gyro.setLpBandwidth(LPFBW_42HZ);
00063     compass.setDefault();
00064     wait(0.1);                        // Wait some time for all sensors (Need at least 5ms)
00065     // *** start up ***
00066         
00067     pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
00068     pc.printf("   Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n\r");
00069 
00070     while( stop==0 ){
00071     //for( int i=0;i<10000;i++ ){
00072     
00073         // Updating accelerometer and compass
00074         accelerometer.getOutput(bitAcc);
00075         compass.readData(getMag);
00076         
00077         // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
00078         Acc[0]  =  ((int16_t)bitAcc[0])*0.004;
00079         Acc[1]  =  ((int16_t)bitAcc[1])*0.004;
00080         Acc[2]  =  ((int16_t)bitAcc[2])*0.004;
00081 
00082         Gyro[0] =  (gyro.getGyroX())/14.375;
00083         Gyro[1] =  (gyro.getGyroY())/14.375;
00084         Gyro[2] =  (gyro.getGyroZ())/14.375;
00085         // Calibration YAL 9DOF Compass:X, Y, Z
00086         Mag[0]  =  (int16_t)getMag[0];
00087         Mag[1]  =  (int16_t)getMag[1];
00088         Mag[2]  =  (int16_t)getMag[2];
00089         
00090         // Low pass filter for acc
00091         //for ( int i=0;i<N;i++ ){
00092         //    if( -0.05<Acc[i] && Acc[i]<0.05 ){    Acc[i]=0;  }
00093         //}
00094         
00095         // Low pass filter for gyro
00096         //for ( int i=0;i<N;i++ ){
00097         //    if( -1.0<Gyro[i] && Gyro[i]<1.0 ){    Gyro[i]=0;  }
00098         //}
00099 
00100         //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2], Mag[0], Mag[1], Mag[2]);
00101         pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
00102         //pc.printf("%7.1f, %7.1f, %7.1f\n\r",Gyro[0], Gyro[1], Gyro[2]);
00103         fprintf(fp, "%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]);
00104         
00105         t += dt;
00106         wait(dt);
00107         
00108     }
00109     
00110     fclose(fp);
00111     
00112  }
00113