hige dura / Mbed 2 deprecated 9DOF_SensorStick

SparkFun_9DOF_SensorStick

Dependencies:   FatFileSystem mbed

Fork of 9DOF_SensorStick by hige dura

main.cpp@5:12e37af16f2e, 2012-09-03 (annotated)

Committer:
higedura
Date:
Mon Sep 03 03:44:13 2012 +0000
Revision:
5:12e37af16f2e
Parent:
4:cc2b1a6de723 
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
higedura 3:5b192b38b3bb 1 #include "mbed.h"
higedura 0:80d32420bc63 2 #include "ADXL345_I2C.h"
higedura 0:80d32420bc63 3 #include "ITG3200.h"
higedura 2:3ffce3e97527 4 #include "HMC5883L.h"
higedura 3:5b192b38b3bb 5 #include "SDFileSystem.h"
higedura 0:80d32420bc63 6
higedura 0:80d32420bc63 7 ADXL345_I2C accelerometer(p9, p10);
higedura 0:80d32420bc63 8 ITG3200 gyro(p9, p10);
higedura 2:3ffce3e97527 9 HMC5883L compass(p9, p10);
higedura 3:5b192b38b3bb 10 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
higedura 5:12e37af16f2e 11 DigitalIn stop(p20);
higedura 0:80d32420bc63 12 Serial pc(USBTX, USBRX);
higedura 0:80d32420bc63 13
higedura 2:3ffce3e97527 14 #define N 3
higedura 2:3ffce3e97527 15
higedura 3:5b192b38b3bb 16 int preparing_acc();
higedura 3:5b192b38b3bb 17
higedura 0:80d32420bc63 18 int main(){
higedura 2:3ffce3e97527 19
higedura 3:5b192b38b3bb 20 float dt = 0.01;
higedura 2:3ffce3e97527 21 float t = 0;
higedura 3:5b192b38b3bb 22 pc.baud(921600);
higedura 2:3ffce3e97527 23 int bitAcc[N] = {0}; // Buffer of the accelerometer
higedura 2:3ffce3e97527 24 int getMag[N] = {0}; // Buffer of the compass
higedura 2:3ffce3e97527 25 double Acc [N] = {0};
higedura 2:3ffce3e97527 26 double Gyro [N] = {0};
higedura 2:3ffce3e97527 27 int Mag [N] = {0};
higedura 0:80d32420bc63 28
higedura 3:5b192b38b3bb 29 // SD file system
higedura 3:5b192b38b3bb 30 mkdir("/sd/mydir", 0777);
higedura 0:80d32420bc63 31
higedura 3:5b192b38b3bb 32 FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
higedura 3:5b192b38b3bb 33 if( fp==NULL ){ error("Could not open file for write\n"); }
higedura 0:80d32420bc63 34
higedura 5:12e37af16f2e 35 // *** start up ***
higedura 3:5b192b38b3bb 36 // These are here to test whether any of the initialization fails. It will print the failure.
higedura 3:5b192b38b3bb 37 if (accelerometer.setPowerControl(0x00)){
higedura 3:5b192b38b3bb 38 pc.printf("didn't intitialize power control\n\r");
higedura 3:5b192b38b3bb 39 return 0;
higedura 3:5b192b38b3bb 40 }
higedura 3:5b192b38b3bb 41 // Full resolution, +/-16g, 4mg/LSB.
higedura 3:5b192b38b3bb 42 wait(.001);
higedura 3:5b192b38b3bb 43
higedura 3:5b192b38b3bb 44 if(accelerometer.setDataFormatControl(0x0B)){
higedura 3:5b192b38b3bb 45 pc.printf("didn't set data format\n\r");
higedura 3:5b192b38b3bb 46 return 0;
higedura 3:5b192b38b3bb 47 }
higedura 3:5b192b38b3bb 48 wait(.001);
higedura 3:5b192b38b3bb 49
higedura 3:5b192b38b3bb 50 // 3.2kHz data rate.
higedura 3:5b192b38b3bb 51 if(accelerometer.setDataRate(ADXL345_3200HZ)){
higedura 3:5b192b38b3bb 52 pc.printf("didn't set data rate\n\r");
higedura 3:5b192b38b3bb 53 return 0;
higedura 3:5b192b38b3bb 54 }
higedura 3:5b192b38b3bb 55 wait(.001);
higedura 3:5b192b38b3bb 56
higedura 3:5b192b38b3bb 57 if(accelerometer.setPowerControl(MeasurementMode)) {
higedura 3:5b192b38b3bb 58 pc.printf("didn't set the power control to measurement\n\r");
higedura 3:5b192b38b3bb 59 return 0;
higedura 5:12e37af16f2e 60 }
higedura 5:12e37af16f2e 61
higedura 5:12e37af16f2e 62 gyro.setLpBandwidth(LPFBW_42HZ);
higedura 5:12e37af16f2e 63 compass.setDefault();
higedura 5:12e37af16f2e 64 wait(0.1); // Wait some time for all sensors (Need at least 5ms)
higedura 5:12e37af16f2e 65 // *** start up ***
higedura 5:12e37af16f2e 66
higedura 5:12e37af16f2e 67 pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
higedura 5:12e37af16f2e 68 pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r");
higedura 5:12e37af16f2e 69
higedura 5:12e37af16f2e 70 while( stop==0 ){
higedura 5:12e37af16f2e 71 //for( int i=0;i<10000;i++ ){
higedura 5:12e37af16f2e 72
higedura 5:12e37af16f2e 73 // Updating accelerometer and compass
higedura 5:12e37af16f2e 74 accelerometer.getOutput(bitAcc);
higedura 5:12e37af16f2e 75 compass.readData(getMag);
higedura 5:12e37af16f2e 76
higedura 5:12e37af16f2e 77 // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
higedura 5:12e37af16f2e 78 Acc[0] = ((int16_t)bitAcc[0])*0.004;
higedura 5:12e37af16f2e 79 Acc[1] = ((int16_t)bitAcc[1])*0.004;
higedura 5:12e37af16f2e 80 Acc[2] = ((int16_t)bitAcc[2])*0.004;
higedura 5:12e37af16f2e 81
higedura 5:12e37af16f2e 82 Gyro[0] = (gyro.getGyroX())/14.375;
higedura 5:12e37af16f2e 83 Gyro[1] = (gyro.getGyroY())/14.375;
higedura 5:12e37af16f2e 84 Gyro[2] = (gyro.getGyroZ())/14.375;
higedura 5:12e37af16f2e 85 // Calibration YAL 9DOF Compass:X, Y, Z
higedura 5:12e37af16f2e 86 Mag[0] = (int16_t)getMag[0];
higedura 5:12e37af16f2e 87 Mag[1] = (int16_t)getMag[1];
higedura 5:12e37af16f2e 88 Mag[2] = (int16_t)getMag[2];
higedura 5:12e37af16f2e 89
higedura 5:12e37af16f2e 90 // Low pass filter for acc
higedura 5:12e37af16f2e 91 //for ( int i=0;i<N;i++ ){
higedura 5:12e37af16f2e 92 // if( -0.05<Acc[i] && Acc[i]<0.05 ){ Acc[i]=0; }
higedura 5:12e37af16f2e 93 //}
higedura 5:12e37af16f2e 94
higedura 5:12e37af16f2e 95 // Low pass filter for gyro
higedura 5:12e37af16f2e 96 //for ( int i=0;i<N;i++ ){
higedura 5:12e37af16f2e 97 // if( -1.0<Gyro[i] && Gyro[i]<1.0 ){ Gyro[i]=0; }
higedura 5:12e37af16f2e 98 //}
higedura 5:12e37af16f2e 99
higedura 5:12e37af16f2e 100 //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]);
higedura 5:12e37af16f2e 101 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]);
higedura 5:12e37af16f2e 102 //pc.printf("%7.1f, %7.1f, %7.1f\n\r",Gyro[0], Gyro[1], Gyro[2]);
higedura 5:12e37af16f2e 103 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]);
higedura 5:12e37af16f2e 104
higedura 5:12e37af16f2e 105 t += dt;
higedura 5:12e37af16f2e 106 wait(dt);
higedura 5:12e37af16f2e 107
higedura 5:12e37af16f2e 108 }
higedura 5:12e37af16f2e 109
higedura 5:12e37af16f2e 110 fclose(fp);
higedura 5:12e37af16f2e 111
higedura 5:12e37af16f2e 112 }
higedura 5:12e37af16f2e 113