hige dura / Mbed 2 deprecated RK4_euler

RK4_euler

Dependencies:   FatFileSystem mbed

Fork of RK4_euler by hige dura

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?

UserRevisionLine numberNew 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 }