RK4_euler
Dependencies: FatFileSystem mbed
Fork of RK4_euler by
Diff: main.cpp
- Revision:
- 6:07f4aaae5339
- Parent:
- 5:12e37af16f2e
- Child:
- 7:ec00db826804
--- a/main.cpp Mon Sep 03 03:44:13 2012 +0000 +++ b/main.cpp Thu Sep 20 13:00:24 2012 +0000 @@ -1,83 +1,73 @@ #include "mbed.h" -#include "ADXL345_I2C.h" #include "ITG3200.h" #include "HMC5883L.h" -#include "SDFileSystem.h" -ADXL345_I2C accelerometer(p9, p10); +#define N 3 +#define chan 6 + ITG3200 gyro(p9, p10); HMC5883L compass(p9, p10); -SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board -DigitalIn stop(p20); +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); -#define N 3 +Timer t_aile; Timer t_elev; Timer t_thro; Timer t_rudd; Timer t_switch1; Timer t_switch2; -int preparing_acc(); +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.01; + float dt = 0.1; float t = 0; pc.baud(921600); - int bitAcc[N] = {0}; // Buffer of the accelerometer + + double range = 0; int getMag[N] = {0}; // Buffer of the compass - double Acc [N] = {0}; double Gyro [N] = {0}; int Mag [N] = {0}; - // SD file system - mkdir("/sd/mydir", 0777); - - FILE *fp = fopen("/sd/mydir/sdtest.txt", "w"); - if( fp==NULL ){ error("Could not open file for write\n"); } + 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 *** - // These are here to test whether any of the initialization fails. It will print the failure. - if (accelerometer.setPowerControl(0x00)){ - pc.printf("didn't intitialize power control\n\r"); - return 0; - } - // Full resolution, +/-16g, 4mg/LSB. - wait(.001); - - if(accelerometer.setDataFormatControl(0x0B)){ - pc.printf("didn't set data format\n\r"); - return 0; - } - wait(.001); - - // 3.2kHz data rate. - if(accelerometer.setDataRate(ADXL345_3200HZ)){ - pc.printf("didn't set data rate\n\r"); - return 0; - } - wait(.001); - - if(accelerometer.setPowerControl(MeasurementMode)) { - pc.printf("didn't set the power control to measurement\n\r"); - return 0; - } - 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){ - pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r"); - pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r"); - - while( stop==0 ){ - //for( int i=0;i<10000;i++ ){ - + range = sonar; + // Updating accelerometer and compass - accelerometer.getOutput(bitAcc); compass.readData(getMag); - - // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga]) - Acc[0] = ((int16_t)bitAcc[0])*0.004; - Acc[1] = ((int16_t)bitAcc[1])*0.004; - Acc[2] = ((int16_t)bitAcc[2])*0.004; Gyro[0] = (gyro.getGyroX())/14.375; Gyro[1] = (gyro.getGyroY())/14.375; @@ -87,27 +77,58 @@ Mag[1] = (int16_t)getMag[1]; Mag[2] = (int16_t)getMag[2]; - // Low pass filter for acc - //for ( int i=0;i<N;i++ ){ - // if( -0.05<Acc[i] && Acc[i]<0.05 ){ Acc[i]=0; } - //} + for( int i=0;i<6;i++ ){ servo[i] = pwm_pulse[i]; } - // Low pass filter for gyro - //for ( int i=0;i<N;i++ ){ - // if( -1.0<Gyro[i] && Gyro[i]<1.0 ){ Gyro[i]=0; } - //} - - //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]); - 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]); - //pc.printf("%7.1f, %7.1f, %7.1f\n\r",Gyro[0], Gyro[1], Gyro[2]); - fprintf(fp, "%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2]); + 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); } - fclose(fp); - } - \ No newline at end of file + +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(); +} \ No newline at end of file