hige dura / Mbed 2 deprecated RK4_euler

RK4_euler

Dependencies:   FatFileSystem mbed

Fork of RK4_euler by hige dura

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?

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