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