RK4_euler
Dependencies: FatFileSystem mbed
Fork of RK4_euler by
main.cpp
- Committer:
- higedura
- Date:
- 2012-09-03
- Revision:
- 5:12e37af16f2e
- Parent:
- 4:cc2b1a6de723
- Child:
- 6:07f4aaae5339
File content as of revision 5:12e37af16f2e:
#include "mbed.h" #include "ADXL345_I2C.h" #include "ITG3200.h" #include "HMC5883L.h" #include "SDFileSystem.h" ADXL345_I2C accelerometer(p9, p10); ITG3200 gyro(p9, p10); HMC5883L compass(p9, p10); SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board DigitalIn stop(p20); Serial pc(USBTX, USBRX); #define N 3 int preparing_acc(); int main(){ float dt = 0.01; float t = 0; pc.baud(921600); int bitAcc[N] = {0}; // Buffer of the accelerometer int getMag[N] = {0}; // Buffer of the compass double Acc [N] = {0}; double Gyro [N] = {0}; int Mag [N] = {0}; // SD file system mkdir("/sd/mydir", 0777); FILE *fp = fopen("/sd/mydir/sdtest.txt", "w"); if( fp==NULL ){ error("Could not open file for write\n"); } // *** start up *** // These are here to test whether any of the initialization fails. It will print the failure. if (accelerometer.setPowerControl(0x00)){ pc.printf("didn't intitialize power control\n\r"); return 0; } // Full resolution, +/-16g, 4mg/LSB. wait(.001); if(accelerometer.setDataFormatControl(0x0B)){ pc.printf("didn't set data format\n\r"); return 0; } wait(.001); // 3.2kHz data rate. if(accelerometer.setDataRate(ADXL345_3200HZ)){ pc.printf("didn't set data rate\n\r"); return 0; } wait(.001); if(accelerometer.setPowerControl(MeasurementMode)) { pc.printf("didn't set the power control to measurement\n\r"); return 0; } gyro.setLpBandwidth(LPFBW_42HZ); compass.setDefault(); wait(0.1); // Wait some time for all sensors (Need at least 5ms) // *** start up *** pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r"); pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r"); while( stop==0 ){ //for( int i=0;i<10000;i++ ){ // Updating accelerometer and compass accelerometer.getOutput(bitAcc); compass.readData(getMag); // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga]) Acc[0] = ((int16_t)bitAcc[0])*0.004; Acc[1] = ((int16_t)bitAcc[1])*0.004; Acc[2] = ((int16_t)bitAcc[2])*0.004; Gyro[0] = (gyro.getGyroX())/14.375; Gyro[1] = (gyro.getGyroY())/14.375; Gyro[2] = (gyro.getGyroZ())/14.375; // Calibration YAL 9DOF Compass:X, Y, Z Mag[0] = (int16_t)getMag[0]; Mag[1] = (int16_t)getMag[1]; Mag[2] = (int16_t)getMag[2]; // Low pass filter for acc //for ( int i=0;i<N;i++ ){ // if( -0.05<Acc[i] && Acc[i]<0.05 ){ Acc[i]=0; } //} // Low pass filter for gyro //for ( int i=0;i<N;i++ ){ // if( -1.0<Gyro[i] && Gyro[i]<1.0 ){ Gyro[i]=0; } //} //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]); 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]); //pc.printf("%7.1f, %7.1f, %7.1f\n\r",Gyro[0], Gyro[1], Gyro[2]); 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]); t += dt; wait(dt); } fclose(fp); }