RK4_euler
Dependencies: FatFileSystem mbed
Fork of RK4_euler by
main.cpp
- Committer:
- higedura
- Date:
- 2012-09-20
- Revision:
- 6:07f4aaae5339
- Parent:
- 5:12e37af16f2e
- Child:
- 7:ec00db826804
File content as of revision 6:07f4aaae5339:
#include "mbed.h" #include "ITG3200.h" #include "HMC5883L.h" #define N 3 #define chan 6 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; pc.baud(921600); double range = 0; int getMag[N] = {0}; // Buffer of the compass double Gyro [N] = {0}; int Mag [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); // *** 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; // 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]; for( int i=0;i<6;i++ ){ servo[i] = pwm_pulse[i]; } 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); t += dt; wait(dt); } } 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(); } void aile_down(){ t_aile.stop(); pwm_pulse[0] = t_aile.read(); t_aile.reset(); } void elev_down(){ t_elev.stop(); pwm_pulse[1] = t_elev.read(); t_elev.reset(); } 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(); } void switch1_down(){ t_switch1.stop(); pwm_pulse[4] = t_switch1.read(); t_switch1.reset(); } void switch2_down(){ t_switch2.stop(); pwm_pulse[5] = t_switch2.read(); t_switch2.reset(); }