RK4_euler
Dependencies: FatFileSystem mbed
Fork of RK4_euler by
main.cpp@7:ec00db826804, 2012-11-29 (annotated)
- Committer:
- higedura
- Date:
- Thu Nov 29 15:22:06 2012 +0000
- Revision:
- 7:ec00db826804
- Parent:
- 6:07f4aaae5339
RK4_euler
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 "ITG3200.h" |
higedura | 0:80d32420bc63 | 3 | |
higedura | 6:07f4aaae5339 | 4 | #define N 3 |
higedura | 7:ec00db826804 | 5 | #define pi 3.14159265 |
higedura | 7:ec00db826804 | 6 | |
higedura | 7:ec00db826804 | 7 | double* RK4( double, double[N], double[N] ); |
higedura | 7:ec00db826804 | 8 | double* func( double[N], double[N] ); |
higedura | 6:07f4aaae5339 | 9 | |
higedura | 0:80d32420bc63 | 10 | ITG3200 gyro(p9, p10); |
higedura | 0:80d32420bc63 | 11 | Serial pc(USBTX, USBRX); |
higedura | 0:80d32420bc63 | 12 | |
higedura | 0:80d32420bc63 | 13 | int main(){ |
higedura | 2:3ffce3e97527 | 14 | |
higedura | 7:ec00db826804 | 15 | // keisan zyou no kizami haba |
higedura | 7:ec00db826804 | 16 | float dt = 0.01; |
higedura | 7:ec00db826804 | 17 | // zissai ni wait suru zikan |
higedura | 7:ec00db826804 | 18 | float dt_wait = 0.01; |
higedura | 7:ec00db826804 | 19 | // hyouzi you |
higedura | 7:ec00db826804 | 20 | float t = 0; |
higedura | 7:ec00db826804 | 21 | |
higedura | 3:5b192b38b3bb | 22 | pc.baud(921600); |
higedura | 6:07f4aaae5339 | 23 | |
higedura | 7:ec00db826804 | 24 | // x = [ p, q, r ] |
higedura | 7:ec00db826804 | 25 | // y = [ phi, the, psi ] |
higedura | 7:ec00db826804 | 26 | double x_rad[N] = {0}; |
higedura | 7:ec00db826804 | 27 | double x_deg[N] = {0}; |
higedura | 7:ec00db826804 | 28 | double y_rad[N] = {0}; |
higedura | 7:ec00db826804 | 29 | double y_deg[N] = {0}; |
higedura | 0:80d32420bc63 | 30 | |
higedura | 7:ec00db826804 | 31 | double* pybuf; // pointer for gyro |
higedura | 0:80d32420bc63 | 32 | |
higedura | 5:12e37af16f2e | 33 | // *** start up *** |
higedura | 5:12e37af16f2e | 34 | gyro.setLpBandwidth(LPFBW_42HZ); |
higedura | 5:12e37af16f2e | 35 | wait(0.1); // Wait some time for all sensors (Need at least 5ms) |
higedura | 5:12e37af16f2e | 36 | // *** start up *** |
higedura | 6:07f4aaae5339 | 37 | |
higedura | 6:07f4aaae5339 | 38 | while(1){ |
higedura | 5:12e37af16f2e | 39 | |
higedura | 7:ec00db826804 | 40 | x_deg[0] = (gyro.getGyroX())/14.375+9.4; |
higedura | 7:ec00db826804 | 41 | x_deg[1] = (gyro.getGyroY())/14.375-0.8; |
higedura | 7:ec00db826804 | 42 | x_deg[2] = (gyro.getGyroZ())/14.375-4; |
higedura | 7:ec00db826804 | 43 | |
higedura | 7:ec00db826804 | 44 | for ( int i=0;i<N;i++ ){ x_rad[i] = x_deg[i]*pi/180; } |
higedura | 6:07f4aaae5339 | 45 | |
higedura | 7:ec00db826804 | 46 | // RK4 |
higedura | 7:ec00db826804 | 47 | pybuf = RK4(dt,y_rad,x_rad); |
higedura | 7:ec00db826804 | 48 | for ( int i=0;i<N;i++ ){ y_rad[i] = *pybuf; pybuf = pybuf+1; } |
higedura | 5:12e37af16f2e | 49 | |
higedura | 7:ec00db826804 | 50 | for ( int i=0;i<N;i++ ){ y_deg[i] = y_rad[i]*180/pi; } |
higedura | 5:12e37af16f2e | 51 | |
higedura | 7:ec00db826804 | 52 | pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, x_deg[0], x_deg[1], x_deg[2], y_deg[0], y_deg[1], y_deg[2]); |
higedura | 5:12e37af16f2e | 53 | |
higedura | 5:12e37af16f2e | 54 | t += dt; |
higedura | 5:12e37af16f2e | 55 | wait(dt); |
higedura | 5:12e37af16f2e | 56 | |
higedura | 5:12e37af16f2e | 57 | } |
higedura | 5:12e37af16f2e | 58 | |
higedura | 5:12e37af16f2e | 59 | } |
higedura | 6:07f4aaae5339 | 60 | |
higedura | 7:ec00db826804 | 61 | double* RK4( double dt, double y[N], double x[N] ){ |
higedura | 7:ec00db826804 | 62 | |
higedura | 7:ec00db826804 | 63 | double yBuf[N] = {0}; |
higedura | 7:ec00db826804 | 64 | double k[N][4] = {0}; |
higedura | 7:ec00db826804 | 65 | |
higedura | 7:ec00db826804 | 66 | double* p_y = y; |
higedura | 7:ec00db826804 | 67 | |
higedura | 7:ec00db826804 | 68 | double* pk1; |
higedura | 7:ec00db826804 | 69 | double* pk2; |
higedura | 7:ec00db826804 | 70 | double* pk3; |
higedura | 7:ec00db826804 | 71 | double* pk4; |
higedura | 7:ec00db826804 | 72 | |
higedura | 7:ec00db826804 | 73 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]; } |
higedura | 7:ec00db826804 | 74 | pk1 = func (yBuf,x); |
higedura | 7:ec00db826804 | 75 | for ( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; } |
higedura | 6:07f4aaae5339 | 76 | |
higedura | 7:ec00db826804 | 77 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; } |
higedura | 7:ec00db826804 | 78 | pk2 = func (yBuf,x); |
higedura | 7:ec00db826804 | 79 | for ( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; } |
higedura | 7:ec00db826804 | 80 | |
higedura | 7:ec00db826804 | 81 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; } |
higedura | 7:ec00db826804 | 82 | pk3 = func (yBuf,x); |
higedura | 7:ec00db826804 | 83 | for ( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; } |
higedura | 6:07f4aaae5339 | 84 | |
higedura | 7:ec00db826804 | 85 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; } |
higedura | 7:ec00db826804 | 86 | pk4 = func (yBuf,x); |
higedura | 7:ec00db826804 | 87 | for ( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; } |
higedura | 7:ec00db826804 | 88 | |
higedura | 7:ec00db826804 | 89 | for ( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; } |
higedura | 7:ec00db826804 | 90 | |
higedura | 7:ec00db826804 | 91 | return p_y; |
higedura | 7:ec00db826804 | 92 | |
higedura | 6:07f4aaae5339 | 93 | } |
higedura | 6:07f4aaae5339 | 94 | |
higedura | 7:ec00db826804 | 95 | double* func( double y[N], double x[N] ){ |
higedura | 6:07f4aaae5339 | 96 | |
higedura | 7:ec00db826804 | 97 | double f[N] = {0}; |
higedura | 7:ec00db826804 | 98 | double* p_f = f; |
higedura | 7:ec00db826804 | 99 | |
higedura | 7:ec00db826804 | 100 | f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]); |
higedura | 7:ec00db826804 | 101 | f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]); |
higedura | 7:ec00db826804 | 102 | f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]); |
higedura | 7:ec00db826804 | 103 | |
higedura | 7:ec00db826804 | 104 | wait(.000000001); |
higedura | 7:ec00db826804 | 105 | |
higedura | 7:ec00db826804 | 106 | return p_f; |
higedura | 6:07f4aaae5339 | 107 | |
higedura | 6:07f4aaae5339 | 108 | } |