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