hige dura / Mbed 2 deprecated RK4_euler

RK4_euler

Dependencies:   FatFileSystem mbed

Fork of RK4_euler by hige dura

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?

        

            
                
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

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

}