RK4_euler
Dependencies: FatFileSystem mbed
Fork of RK4_euler by
main.cpp@6:07f4aaae5339, 2012-09-20 (annotated)
- Committer:
- higedura
- Date:
- Thu Sep 20 13:00:24 2012 +0000
- Revision:
- 6:07f4aaae5339
- Parent:
- 5:12e37af16f2e
- Child:
- 7:ec00db826804
hikorobo
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 | 2:3ffce3e97527 | 3 | #include "HMC5883L.h" |
higedura | 0:80d32420bc63 | 4 | |
higedura | 6:07f4aaae5339 | 5 | #define N 3 |
higedura | 6:07f4aaae5339 | 6 | #define chan 6 |
higedura | 6:07f4aaae5339 | 7 | |
higedura | 0:80d32420bc63 | 8 | ITG3200 gyro(p9, p10); |
higedura | 2:3ffce3e97527 | 9 | HMC5883L compass(p9, p10); |
higedura | 6:07f4aaae5339 | 10 | InterruptIn ch1(p13); // aileron |
higedura | 6:07f4aaae5339 | 11 | InterruptIn ch2(p14); // elevator |
higedura | 6:07f4aaae5339 | 12 | InterruptIn ch3(p15); // throttle |
higedura | 6:07f4aaae5339 | 13 | InterruptIn ch4(p16); // rudder |
higedura | 6:07f4aaae5339 | 14 | InterruptIn ch5(p17); // switch1 |
higedura | 6:07f4aaae5339 | 15 | InterruptIn ch6(p18); // switch2 |
higedura | 6:07f4aaae5339 | 16 | AnalogIn sonar(p19); // sonar |
higedura | 6:07f4aaae5339 | 17 | PwmOut ch1_out(p21); |
higedura | 6:07f4aaae5339 | 18 | PwmOut ch2_out(p22); |
higedura | 6:07f4aaae5339 | 19 | PwmOut ch3_out(p23); |
higedura | 6:07f4aaae5339 | 20 | PwmOut ch4_out(p24); |
higedura | 6:07f4aaae5339 | 21 | PwmOut ch5_out(p25); |
higedura | 6:07f4aaae5339 | 22 | PwmOut ch6_out(p26); |
higedura | 0:80d32420bc63 | 23 | Serial pc(USBTX, USBRX); |
higedura | 0:80d32420bc63 | 24 | |
higedura | 6:07f4aaae5339 | 25 | Timer t_aile; Timer t_elev; Timer t_thro; Timer t_rudd; Timer t_switch1; Timer t_switch2; |
higedura | 2:3ffce3e97527 | 26 | |
higedura | 6:07f4aaae5339 | 27 | void aile_up(); void elev_up(); void thro_up(); void rudd_up(); void switch1_up(); void switch2_up(); |
higedura | 6:07f4aaae5339 | 28 | void aile_down(); void elev_down(); void thro_down(); void rudd_down(); void switch1_down(); void switch2_down(); |
higedura | 6:07f4aaae5339 | 29 | |
higedura | 6:07f4aaae5339 | 30 | double pwm_pulse[chan] = {0.001,0.001,0.001,0.001,0.001,0.001}; // aile, elev, thro, rudd, switch1, switch2 |
higedura | 3:5b192b38b3bb | 31 | |
higedura | 0:80d32420bc63 | 32 | int main(){ |
higedura | 2:3ffce3e97527 | 33 | |
higedura | 6:07f4aaae5339 | 34 | float dt = 0.1; |
higedura | 2:3ffce3e97527 | 35 | float t = 0; |
higedura | 3:5b192b38b3bb | 36 | pc.baud(921600); |
higedura | 6:07f4aaae5339 | 37 | |
higedura | 6:07f4aaae5339 | 38 | double range = 0; |
higedura | 2:3ffce3e97527 | 39 | int getMag[N] = {0}; // Buffer of the compass |
higedura | 2:3ffce3e97527 | 40 | double Gyro [N] = {0}; |
higedura | 2:3ffce3e97527 | 41 | int Mag [N] = {0}; |
higedura | 0:80d32420bc63 | 42 | |
higedura | 6:07f4aaae5339 | 43 | double servo[6] = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001}; |
higedura | 6:07f4aaae5339 | 44 | |
higedura | 6:07f4aaae5339 | 45 | 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); |
higedura | 6:07f4aaae5339 | 46 | // pulsewidth 0.001~0.002 |
higedura | 6:07f4aaae5339 | 47 | 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); |
higedura | 0:80d32420bc63 | 48 | |
higedura | 5:12e37af16f2e | 49 | // *** start up *** |
higedura | 5:12e37af16f2e | 50 | gyro.setLpBandwidth(LPFBW_42HZ); |
higedura | 5:12e37af16f2e | 51 | compass.setDefault(); |
higedura | 5:12e37af16f2e | 52 | wait(0.1); // Wait some time for all sensors (Need at least 5ms) |
higedura | 5:12e37af16f2e | 53 | // *** start up *** |
higedura | 6:07f4aaae5339 | 54 | |
higedura | 6:07f4aaae5339 | 55 | // Reciever function |
higedura | 6:07f4aaae5339 | 56 | ch1.rise(&aile_up); ch1.fall(&aile_down); |
higedura | 6:07f4aaae5339 | 57 | ch2.rise(&elev_up); ch2.fall(&elev_down); |
higedura | 6:07f4aaae5339 | 58 | ch3.rise(&thro_up); ch3.fall(&thro_down); |
higedura | 6:07f4aaae5339 | 59 | ch4.rise(&rudd_up); ch4.fall(&rudd_down); |
higedura | 6:07f4aaae5339 | 60 | ch5.rise(&switch1_up); ch5.fall(&switch1_down); |
higedura | 6:07f4aaae5339 | 61 | ch6.rise(&switch2_up); ch6.fall(&switch2_down); |
higedura | 6:07f4aaae5339 | 62 | |
higedura | 6:07f4aaae5339 | 63 | pc.printf("\n\rHiko-robo!!\n\r"); |
higedura | 6:07f4aaae5339 | 64 | |
higedura | 6:07f4aaae5339 | 65 | while(1){ |
higedura | 5:12e37af16f2e | 66 | |
higedura | 6:07f4aaae5339 | 67 | range = sonar; |
higedura | 6:07f4aaae5339 | 68 | |
higedura | 5:12e37af16f2e | 69 | // Updating accelerometer and compass |
higedura | 5:12e37af16f2e | 70 | compass.readData(getMag); |
higedura | 5:12e37af16f2e | 71 | |
higedura | 5:12e37af16f2e | 72 | Gyro[0] = (gyro.getGyroX())/14.375; |
higedura | 5:12e37af16f2e | 73 | Gyro[1] = (gyro.getGyroY())/14.375; |
higedura | 5:12e37af16f2e | 74 | Gyro[2] = (gyro.getGyroZ())/14.375; |
higedura | 5:12e37af16f2e | 75 | // Calibration YAL 9DOF Compass:X, Y, Z |
higedura | 5:12e37af16f2e | 76 | Mag[0] = (int16_t)getMag[0]; |
higedura | 5:12e37af16f2e | 77 | Mag[1] = (int16_t)getMag[1]; |
higedura | 5:12e37af16f2e | 78 | Mag[2] = (int16_t)getMag[2]; |
higedura | 5:12e37af16f2e | 79 | |
higedura | 6:07f4aaae5339 | 80 | for( int i=0;i<6;i++ ){ servo[i] = pwm_pulse[i]; } |
higedura | 5:12e37af16f2e | 81 | |
higedura | 6:07f4aaae5339 | 82 | 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]); |
higedura | 6:07f4aaae5339 | 83 | |
higedura | 6:07f4aaae5339 | 84 | 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); |
higedura | 5:12e37af16f2e | 85 | |
higedura | 5:12e37af16f2e | 86 | t += dt; |
higedura | 5:12e37af16f2e | 87 | wait(dt); |
higedura | 5:12e37af16f2e | 88 | |
higedura | 5:12e37af16f2e | 89 | } |
higedura | 5:12e37af16f2e | 90 | |
higedura | 5:12e37af16f2e | 91 | } |
higedura | 6:07f4aaae5339 | 92 | |
higedura | 6:07f4aaae5339 | 93 | void aile_up(){ t_aile.start(); } |
higedura | 6:07f4aaae5339 | 94 | void elev_up(){ t_elev.start(); } |
higedura | 6:07f4aaae5339 | 95 | void thro_up(){ t_thro.start(); } |
higedura | 6:07f4aaae5339 | 96 | void rudd_up(){ t_rudd.start(); } |
higedura | 6:07f4aaae5339 | 97 | void switch1_up(){ t_switch1.start(); } |
higedura | 6:07f4aaae5339 | 98 | void switch2_up(){ t_switch2.start(); } |
higedura | 6:07f4aaae5339 | 99 | |
higedura | 6:07f4aaae5339 | 100 | void aile_down(){ |
higedura | 6:07f4aaae5339 | 101 | t_aile.stop(); |
higedura | 6:07f4aaae5339 | 102 | pwm_pulse[0] = t_aile.read(); |
higedura | 6:07f4aaae5339 | 103 | t_aile.reset(); |
higedura | 6:07f4aaae5339 | 104 | } |
higedura | 6:07f4aaae5339 | 105 | |
higedura | 6:07f4aaae5339 | 106 | void elev_down(){ |
higedura | 6:07f4aaae5339 | 107 | t_elev.stop(); |
higedura | 6:07f4aaae5339 | 108 | pwm_pulse[1] = t_elev.read(); |
higedura | 6:07f4aaae5339 | 109 | t_elev.reset(); |
higedura | 6:07f4aaae5339 | 110 | } |
higedura | 6:07f4aaae5339 | 111 | |
higedura | 6:07f4aaae5339 | 112 | void thro_down(){ |
higedura | 6:07f4aaae5339 | 113 | t_thro.stop(); |
higedura | 6:07f4aaae5339 | 114 | pwm_pulse[2] = t_thro.read(); |
higedura | 6:07f4aaae5339 | 115 | t_thro.reset(); |
higedura | 6:07f4aaae5339 | 116 | } |
higedura | 6:07f4aaae5339 | 117 | |
higedura | 6:07f4aaae5339 | 118 | void rudd_down(){ |
higedura | 6:07f4aaae5339 | 119 | t_rudd.stop(); |
higedura | 6:07f4aaae5339 | 120 | pwm_pulse[3] = t_rudd.read(); |
higedura | 6:07f4aaae5339 | 121 | t_rudd.reset(); |
higedura | 6:07f4aaae5339 | 122 | } |
higedura | 6:07f4aaae5339 | 123 | |
higedura | 6:07f4aaae5339 | 124 | void switch1_down(){ |
higedura | 6:07f4aaae5339 | 125 | t_switch1.stop(); |
higedura | 6:07f4aaae5339 | 126 | pwm_pulse[4] = t_switch1.read(); |
higedura | 6:07f4aaae5339 | 127 | t_switch1.reset(); |
higedura | 6:07f4aaae5339 | 128 | } |
higedura | 6:07f4aaae5339 | 129 | |
higedura | 6:07f4aaae5339 | 130 | void switch2_down(){ |
higedura | 6:07f4aaae5339 | 131 | t_switch2.stop(); |
higedura | 6:07f4aaae5339 | 132 | pwm_pulse[5] = t_switch2.read(); |
higedura | 6:07f4aaae5339 | 133 | t_switch2.reset(); |
higedura | 6:07f4aaae5339 | 134 | } |