hige dura
RK4_euler
Dependencies: FatFileSystem mbed
Fork of
RK4_euler
by 
hige dura
Diff: main.cpp
- Revision:
- 7:ec00db826804
- Parent:
- 6:07f4aaae5339
--- a/main.cpp Thu Sep 20 13:00:24 2012 +0000
+++ b/main.cpp Thu Nov 29 15:22:06 2012 +0000
@@ -1,87 +1,55 @@
#include "mbed.h"
#include "ITG3200.h"
-#include "HMC5883L.h"
#define N 3
-#define chan 6
+#define pi 3.14159265
+
+double* RK4( double, double[N], double[N] );
+double* func( double[N], double[N] );
ITG3200 gyro(p9, p10);
-HMC5883L compass(p9, p10);
-InterruptIn ch1(p13); // aileron
-InterruptIn ch2(p14); // elevator
-InterruptIn ch3(p15); // throttle
-InterruptIn ch4(p16); // rudder
-InterruptIn ch5(p17); // switch1
-InterruptIn ch6(p18); // switch2
-AnalogIn sonar(p19); // sonar
-PwmOut ch1_out(p21);
-PwmOut ch2_out(p22);
-PwmOut ch3_out(p23);
-PwmOut ch4_out(p24);
-PwmOut ch5_out(p25);
-PwmOut ch6_out(p26);
Serial pc(USBTX, USBRX);
-Timer t_aile; Timer t_elev; Timer t_thro; Timer t_rudd; Timer t_switch1; Timer t_switch2;
-
-void aile_up(); void elev_up(); void thro_up(); void rudd_up(); void switch1_up(); void switch2_up();
-void aile_down(); void elev_down(); void thro_down(); void rudd_down(); void switch1_down(); void switch2_down();
-
-double pwm_pulse[chan] = {0.001,0.001,0.001,0.001,0.001,0.001}; // aile, elev, thro, rudd, switch1, switch2
-
int main(){
- float dt = 0.1;
- float t = 0;
+ // keisan zyou no kizami haba
+ float dt = 0.01;
+ // zissai ni wait suru zikan
+ float dt_wait = 0.01;
+ // hyouzi you
+ float t = 0;
+
pc.baud(921600);
- double range = 0;
- int getMag[N] = {0}; // Buffer of the compass
- double Gyro [N] = {0};
- int Mag [N] = {0};
+ // x = [ p, q, r ]
+ // y = [ phi, the, psi ]
+ double x_rad[N] = {0};
+ double x_deg[N] = {0};
+ double y_rad[N] = {0};
+ double y_deg[N] = {0};
- double servo[6] = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001};
-
- ch1_out.period(0.02); ch2_out.period(0.02); ch3_out.period(0.02); ch4_out.period(0.02); ch5_out.period(0.02); ch6_out.period(0.02);
- // pulsewidth 0.001~0.002
- ch1_out.pulsewidth(0.001); ch2_out.pulsewidth(0.001); ch3_out.pulsewidth(0.001); ch4_out.pulsewidth(0.001); ch5_out.pulsewidth(0.001); ch6_out.pulsewidth(0.001);
+ double* pybuf; // pointer for gyro
// *** start up ***
gyro.setLpBandwidth(LPFBW_42HZ);
- compass.setDefault();
wait(0.1); // Wait some time for all sensors (Need at least 5ms)
// *** start up ***
- // Reciever function
- ch1.rise(&aile_up); ch1.fall(&aile_down);
- ch2.rise(&elev_up); ch2.fall(&elev_down);
- ch3.rise(&thro_up); ch3.fall(&thro_down);
- ch4.rise(&rudd_up); ch4.fall(&rudd_down);
- ch5.rise(&switch1_up); ch5.fall(&switch1_down);
- ch6.rise(&switch2_up); ch6.fall(&switch2_down);
-
- pc.printf("\n\rHiko-robo!!\n\r");
-
while(1){
- range = sonar;
+ x_deg[0] = (gyro.getGyroX())/14.375+9.4;
+ x_deg[1] = (gyro.getGyroY())/14.375-0.8;
+ x_deg[2] = (gyro.getGyroZ())/14.375-4;
+
+ for ( int i=0;i<N;i++ ){ x_rad[i] = x_deg[i]*pi/180; }
- // Updating accelerometer and compass
- compass.readData(getMag);
-
- Gyro[0] = (gyro.getGyroX())/14.375;
- Gyro[1] = (gyro.getGyroY())/14.375;
- Gyro[2] = (gyro.getGyroZ())/14.375;
- // Calibration YAL 9DOF Compass:X, Y, Z
- Mag[0] = (int16_t)getMag[0];
- Mag[1] = (int16_t)getMag[1];
- Mag[2] = (int16_t)getMag[2];
+ // RK4
+ pybuf = RK4(dt,y_rad,x_rad);
+ for ( int i=0;i<N;i++ ){ y_rad[i] = *pybuf; pybuf = pybuf+1; }
- for( int i=0;i<6;i++ ){ servo[i] = pwm_pulse[i]; }
+ for ( int i=0;i<N;i++ ){ y_deg[i] = y_rad[i]*180/pi; }
- ch1_out.pulsewidth(servo[0]); ch2_out.pulsewidth(servo[1]); ch3_out.pulsewidth(servo[2]); ch4_out.pulsewidth(servo[3]); ch5_out.pulsewidth(servo[4]); ch6_out.pulsewidth(servo[5]);
-
- pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, Gyro[0], Gyro[1], Gyro[2], range, pwm_pulse[0]*1000, pwm_pulse[1]*1000, pwm_pulse[2]*1000, pwm_pulse[3]*1000, pwm_pulse[4]*1000, pwm_pulse[5]*1000);
+ 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]);
t += dt;
wait(dt);
@@ -90,45 +58,51 @@
}
-void aile_up(){ t_aile.start(); }
-void elev_up(){ t_elev.start(); }
-void thro_up(){ t_thro.start(); }
-void rudd_up(){ t_rudd.start(); }
-void switch1_up(){ t_switch1.start(); }
-void switch2_up(){ t_switch2.start(); }
+double* RK4( double dt, double y[N], double x[N] ){
+
+ double yBuf[N] = {0};
+ double k[N][4] = {0};
+
+ double* p_y = y;
+
+ double* pk1;
+ double* pk2;
+ double* pk3;
+ double* pk4;
+
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]; }
+ pk1 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
-void aile_down(){
- t_aile.stop();
- pwm_pulse[0] = t_aile.read();
- t_aile.reset();
-}
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
+ pk2 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
+
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
+ pk3 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
-void elev_down(){
- t_elev.stop();
- pwm_pulse[1] = t_elev.read();
- t_elev.reset();
+ for ( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
+ pk4 = func (yBuf,x);
+ for ( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
+
+ 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; }
+
+ return p_y;
+
}
-void thro_down(){
- t_thro.stop();
- pwm_pulse[2] = t_thro.read();
- t_thro.reset();
-}
-
-void rudd_down(){
- t_rudd.stop();
- pwm_pulse[3] = t_rudd.read();
- t_rudd.reset();
-}
+double* func( double y[N], double x[N] ){
-void switch1_down(){
- t_switch1.stop();
- pwm_pulse[4] = t_switch1.read();
- t_switch1.reset();
-}
+ double f[N] = {0};
+ double* p_f = f;
+
+ f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
+ f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
+ f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
+
+ wait(.000000001);
+
+ return p_f;
-void switch2_down(){
- t_switch2.stop();
- pwm_pulse[5] = t_switch2.read();
- t_switch2.reset();
}
\ No newline at end of file