yal kaiyo / Mbed 2 deprecated hikorobo_2015_quad_2

2015飛行ロボコンマルチコプタ部門

Dependencies:   mbed

main.cpp@0:70d405bfdc8b, 2015-10-18 (annotated)

Committer:
yal_kaiyo
Date:
Sun Oct 18 09:58:59 2015 +0000
Revision:
0:70d405bfdc8b
2015??????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yal_kaiyo 0:70d405bfdc8b 1 #include "mbed.h"
yal_kaiyo 0:70d405bfdc8b 2 #include "MPU6050.h"
yal_kaiyo 0:70d405bfdc8b 3 #include "ledControl.h"
yal_kaiyo 0:70d405bfdc8b 4
yal_kaiyo 0:70d405bfdc8b 5 #define chan 5 // the numbers of channels
yal_kaiyo 0:70d405bfdc8b 6
yal_kaiyo 0:70d405bfdc8b 7 Serial pc(USBTX,USBRX); // default baud rate: 9600
yal_kaiyo 0:70d405bfdc8b 8
yal_kaiyo 0:70d405bfdc8b 9 DigitalOut ex_led(p29);
yal_kaiyo 0:70d405bfdc8b 10
yal_kaiyo 0:70d405bfdc8b 11 Ticker toggler1;
yal_kaiyo 0:70d405bfdc8b 12
yal_kaiyo 0:70d405bfdc8b 13 PwmOut esc1(p21);
yal_kaiyo 0:70d405bfdc8b 14 PwmOut esc2(p22);
yal_kaiyo 0:70d405bfdc8b 15 PwmOut esc3(p23);
yal_kaiyo 0:70d405bfdc8b 16 PwmOut esc4(p24);
yal_kaiyo 0:70d405bfdc8b 17
yal_kaiyo 0:70d405bfdc8b 18 void toggle_led1();
yal_kaiyo 0:70d405bfdc8b 19 void toggle_led2();
yal_kaiyo 0:70d405bfdc8b 20
yal_kaiyo 0:70d405bfdc8b 21 Timer t_main;
yal_kaiyo 0:70d405bfdc8b 22
yal_kaiyo 0:70d405bfdc8b 23 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 24 // MPU6050
yal_kaiyo 0:70d405bfdc8b 25 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 26
yal_kaiyo 0:70d405bfdc8b 27 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
yal_kaiyo 0:70d405bfdc8b 28
yal_kaiyo 0:70d405bfdc8b 29 void compFilter();
yal_kaiyo 0:70d405bfdc8b 30
yal_kaiyo 0:70d405bfdc8b 31 double pitch = 0;
yal_kaiyo 0:70d405bfdc8b 32 double roll = 0;
yal_kaiyo 0:70d405bfdc8b 33
yal_kaiyo 0:70d405bfdc8b 34 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 35 // Read reciever
yal_kaiyo 0:70d405bfdc8b 36 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 37
yal_kaiyo 0:70d405bfdc8b 38 InterruptIn ch1(p15); // yaw
yal_kaiyo 0:70d405bfdc8b 39 InterruptIn ch2(p16); // pitch
yal_kaiyo 0:70d405bfdc8b 40 InterruptIn ch3(p17); // throttle
yal_kaiyo 0:70d405bfdc8b 41 InterruptIn ch4(p18); // roll
yal_kaiyo 0:70d405bfdc8b 42 InterruptIn ch5(p13); // aux1
yal_kaiyo 0:70d405bfdc8b 43 //InterruptIn ch6(p15); // aux2
yal_kaiyo 0:70d405bfdc8b 44
yal_kaiyo 0:70d405bfdc8b 45 void ch1_rise(); void ch1_fall();
yal_kaiyo 0:70d405bfdc8b 46 void ch2_rise(); void ch2_fall();
yal_kaiyo 0:70d405bfdc8b 47 void ch3_rise(); void ch3_fall();
yal_kaiyo 0:70d405bfdc8b 48 void ch4_rise(); void ch4_fall();
yal_kaiyo 0:70d405bfdc8b 49 void ch5_rise(); void ch5_fall();
yal_kaiyo 0:70d405bfdc8b 50 //void ch6_rise(); void ch6_fall();
yal_kaiyo 0:70d405bfdc8b 51
yal_kaiyo 0:70d405bfdc8b 52 Timer t_ch1;
yal_kaiyo 0:70d405bfdc8b 53 Timer t_ch2;
yal_kaiyo 0:70d405bfdc8b 54 Timer t_ch3;
yal_kaiyo 0:70d405bfdc8b 55 Timer t_ch4;
yal_kaiyo 0:70d405bfdc8b 56 Timer t_ch5;
yal_kaiyo 0:70d405bfdc8b 57 //Timer t_ch6;
yal_kaiyo 0:70d405bfdc8b 58
yal_kaiyo 0:70d405bfdc8b 59 double pwm_in_ch1 = 0;
yal_kaiyo 0:70d405bfdc8b 60 double pwm_in_ch2 = 0;
yal_kaiyo 0:70d405bfdc8b 61 double pwm_in_ch3 = 0;
yal_kaiyo 0:70d405bfdc8b 62 double pwm_in_ch4 = 0;
yal_kaiyo 0:70d405bfdc8b 63 double pwm_in_ch5 = 0;
yal_kaiyo 0:70d405bfdc8b 64 //double pwm_in_ch6 = 0;
yal_kaiyo 0:70d405bfdc8b 65
yal_kaiyo 0:70d405bfdc8b 66 int main()
yal_kaiyo 0:70d405bfdc8b 67 {
yal_kaiyo 0:70d405bfdc8b 68
yal_kaiyo 0:70d405bfdc8b 69 int flag_mode = 0; // 0: disarmed, 1: armed, 2: auto
yal_kaiyo 0:70d405bfdc8b 70
yal_kaiyo 0:70d405bfdc8b 71 int count_print = 0;
yal_kaiyo 0:70d405bfdc8b 72
yal_kaiyo 0:70d405bfdc8b 73 double pwm_in_mod_ch1 = 0;
yal_kaiyo 0:70d405bfdc8b 74 double pwm_in_mod_ch2 = 0;
yal_kaiyo 0:70d405bfdc8b 75 double pwm_in_mod_ch3 = 0;
yal_kaiyo 0:70d405bfdc8b 76 double pwm_in_mod_ch4 = 0;
yal_kaiyo 0:70d405bfdc8b 77
yal_kaiyo 0:70d405bfdc8b 78 //double pwm_in_cen_ch1 = 0;
yal_kaiyo 0:70d405bfdc8b 79 //double pwm_in_cen_ch2 = 0;
yal_kaiyo 0:70d405bfdc8b 80 double pwm_in_low_ch3 = 0;
yal_kaiyo 0:70d405bfdc8b 81 //double pwm_in_cen_ch4 = 0;
yal_kaiyo 0:70d405bfdc8b 82
yal_kaiyo 0:70d405bfdc8b 83 //double pwm_in_trim_ch1;
yal_kaiyo 0:70d405bfdc8b 84 //double pwm_in_trim_ch2;
yal_kaiyo 0:70d405bfdc8b 85 double pwm_in_trim_ch3;
yal_kaiyo 0:70d405bfdc8b 86 //double pwm_in_trim_ch4;
yal_kaiyo 0:70d405bfdc8b 87
yal_kaiyo 0:70d405bfdc8b 88 double input_phi = 0;
yal_kaiyo 0:70d405bfdc8b 89 double input_the = 0;
yal_kaiyo 0:70d405bfdc8b 90 double input_ome = 0;
yal_kaiyo 0:70d405bfdc8b 91
yal_kaiyo 0:70d405bfdc8b 92 // Gains
yal_kaiyo 0:70d405bfdc8b 93 double GAIN_P = 0.0000003;
yal_kaiyo 0:70d405bfdc8b 94 double GAIN_Q = 0.0000003;
yal_kaiyo 0:70d405bfdc8b 95 double GAIN_R = 0.0000026;
yal_kaiyo 0:70d405bfdc8b 96
yal_kaiyo 0:70d405bfdc8b 97 double GAIN_PHI = 0.00000213;
yal_kaiyo 0:70d405bfdc8b 98 double GAIN_THE = 0.00000213;
yal_kaiyo 0:70d405bfdc8b 99
yal_kaiyo 0:70d405bfdc8b 100 double GAIN_PWM2ANGLE = 25 / 0.0004; // Max input -> 30 [deg] (0.0011~0.0019 [s], center: 0.0015 [s])
yal_kaiyo 0:70d405bfdc8b 101
yal_kaiyo 0:70d405bfdc8b 102 double GAIN_OME = 1.2;
yal_kaiyo 0:70d405bfdc8b 103
yal_kaiyo 0:70d405bfdc8b 104 //
yal_kaiyo 0:70d405bfdc8b 105 double PWM_OUT_MIN = 0.0011;
yal_kaiyo 0:70d405bfdc8b 106 double pwm_out_ch1 = PWM_OUT_MIN;
yal_kaiyo 0:70d405bfdc8b 107 double pwm_out_ch2 = PWM_OUT_MIN;
yal_kaiyo 0:70d405bfdc8b 108 double pwm_out_ch3 = PWM_OUT_MIN;
yal_kaiyo 0:70d405bfdc8b 109 double pwm_out_ch4 = PWM_OUT_MIN;
yal_kaiyo 0:70d405bfdc8b 110
yal_kaiyo 0:70d405bfdc8b 111 // Rocking wings
yal_kaiyo 0:70d405bfdc8b 112 double pwm_in_mod_ch3_prev = pwm_out_ch3;
yal_kaiyo 0:70d405bfdc8b 113
yal_kaiyo 0:70d405bfdc8b 114 int count_rw = 0;
yal_kaiyo 0:70d405bfdc8b 115 int flag_rw_on = 0;
yal_kaiyo 0:70d405bfdc8b 116 int flag_rw_off = 0;
yal_kaiyo 0:70d405bfdc8b 117
yal_kaiyo 0:70d405bfdc8b 118 double phi_target_rw = 0;
yal_kaiyo 0:70d405bfdc8b 119 double roll_target_rw = 0;
yal_kaiyo 0:70d405bfdc8b 120
yal_kaiyo 0:70d405bfdc8b 121 double DT_RW_1 = 0.3; // For 1st tilt
yal_kaiyo 0:70d405bfdc8b 122 double DT_RW_2 = 0.3; // For stable
yal_kaiyo 0:70d405bfdc8b 123 double DT_RW_3 = 0.3; // For 2nd tilt
yal_kaiyo 0:70d405bfdc8b 124
yal_kaiyo 0:70d405bfdc8b 125 int COUNT_1_RW = DT_RW_1 * 200; // 200 Hz
yal_kaiyo 0:70d405bfdc8b 126 int COUNT_2_RW = COUNT_1_RW + DT_RW_2 * 200;
yal_kaiyo 0:70d405bfdc8b 127 int COUNT_3_RW = COUNT_2_RW + DT_RW_3 * 200;
yal_kaiyo 0:70d405bfdc8b 128 int COUNT_4_RW = COUNT_3_RW + DT_RW_2 * 200;
yal_kaiyo 0:70d405bfdc8b 129
yal_kaiyo 0:70d405bfdc8b 130 double TARGET_ANGLE_RW_1 = 15; // For 1st tilt
yal_kaiyo 0:70d405bfdc8b 131 double TARGET_ANGLE_RW_2 = 5; // for 2nd tilt
yal_kaiyo 0:70d405bfdc8b 132
yal_kaiyo 0:70d405bfdc8b 133 // Set baud rate for PC
yal_kaiyo 0:70d405bfdc8b 134 pc.baud(115200); // baud rate: 115200
yal_kaiyo 0:70d405bfdc8b 135
yal_kaiyo 0:70d405bfdc8b 136 // Set reciever function
yal_kaiyo 0:70d405bfdc8b 137 ch1.rise(&ch1_rise); ch1.fall(&ch1_fall);
yal_kaiyo 0:70d405bfdc8b 138 ch2.rise(&ch2_rise); ch2.fall(&ch2_fall);
yal_kaiyo 0:70d405bfdc8b 139 ch3.rise(&ch3_rise); ch3.fall(&ch3_fall);
yal_kaiyo 0:70d405bfdc8b 140 ch4.rise(&ch4_rise); ch4.fall(&ch4_fall);
yal_kaiyo 0:70d405bfdc8b 141 ch5.rise(&ch5_rise); ch5.fall(&ch5_fall);
yal_kaiyo 0:70d405bfdc8b 142 //ch6.rise(&ch6_rise); ch6.fall(&ch6_fall);
yal_kaiyo 0:70d405bfdc8b 143
yal_kaiyo 0:70d405bfdc8b 144 // Set ESC period
yal_kaiyo 0:70d405bfdc8b 145 esc1.period(0.020); // servo requires a 20ms period
yal_kaiyo 0:70d405bfdc8b 146 esc2.period(0.020); // servo requires a 20ms period
yal_kaiyo 0:70d405bfdc8b 147 esc3.period(0.020); // servo requires a 20ms period
yal_kaiyo 0:70d405bfdc8b 148 esc4.period(0.020); // servo requires a 20ms period
yal_kaiyo 0:70d405bfdc8b 149 wait(1);
yal_kaiyo 0:70d405bfdc8b 150
yal_kaiyo 0:70d405bfdc8b 151 led4 = 1;
yal_kaiyo 0:70d405bfdc8b 152 esc1.pulsewidth(0);
yal_kaiyo 0:70d405bfdc8b 153 esc2.pulsewidth(0);
yal_kaiyo 0:70d405bfdc8b 154 esc3.pulsewidth(0);
yal_kaiyo 0:70d405bfdc8b 155 esc4.pulsewidth(0);
yal_kaiyo 0:70d405bfdc8b 156 wait(3);
yal_kaiyo 0:70d405bfdc8b 157 led4 = 0;
yal_kaiyo 0:70d405bfdc8b 158
yal_kaiyo 0:70d405bfdc8b 159 // Set MPU6050
yal_kaiyo 0:70d405bfdc8b 160 i2c.frequency(400000); // fast i2c: 400 kHz
yal_kaiyo 0:70d405bfdc8b 161 wait(1);
yal_kaiyo 0:70d405bfdc8b 162 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
yal_kaiyo 0:70d405bfdc8b 163 pc.printf("\r\nCalibration is completed. \r\n");
yal_kaiyo 0:70d405bfdc8b 164 wait(0.5);
yal_kaiyo 0:70d405bfdc8b 165 mpu6050.init(); // Initialize the sensor
yal_kaiyo 0:70d405bfdc8b 166 wait(1);
yal_kaiyo 0:70d405bfdc8b 167 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
yal_kaiyo 0:70d405bfdc8b 168 wait_ms(500);
yal_kaiyo 0:70d405bfdc8b 169
yal_kaiyo 0:70d405bfdc8b 170 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 171 // Preprocessing
yal_kaiyo 0:70d405bfdc8b 172 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 173 // Get trim reciever
yal_kaiyo 0:70d405bfdc8b 174 for(int i_trim_rc=0; i_trim_rc<10; i_trim_rc++)
yal_kaiyo 0:70d405bfdc8b 175 {
yal_kaiyo 0:70d405bfdc8b 176 wait(.1);
yal_kaiyo 0:70d405bfdc8b 177 //pwm_in_cen_ch1 += pwm_in_ch1;
yal_kaiyo 0:70d405bfdc8b 178 //pwm_in_cen_ch2 += pwm_in_ch2;
yal_kaiyo 0:70d405bfdc8b 179 pwm_in_low_ch3 += pwm_in_ch3;
yal_kaiyo 0:70d405bfdc8b 180 //pwm_in_cen_ch4 += pwm_in_ch4;
yal_kaiyo 0:70d405bfdc8b 181 //pc.printf("%f %f %f %f\r\n", pwm_in_ch1, pwm_in_ch2, pwm_in_ch3, pwm_in_ch4);
yal_kaiyo 0:70d405bfdc8b 182 }
yal_kaiyo 0:70d405bfdc8b 183 //pwm_in_cen_ch1 = pwm_in_cen_ch1 / 10;
yal_kaiyo 0:70d405bfdc8b 184 //pwm_in_cen_ch2 = pwm_in_cen_ch2 / 10;
yal_kaiyo 0:70d405bfdc8b 185 pwm_in_low_ch3 = pwm_in_low_ch3 / 10;
yal_kaiyo 0:70d405bfdc8b 186 //pwm_in_cen_ch4 = pwm_in_cen_ch4 / 10;
yal_kaiyo 0:70d405bfdc8b 187
yal_kaiyo 0:70d405bfdc8b 188 //pwm_in_trim_ch1 = pwm_in_cen_ch1 - 0.0015;
yal_kaiyo 0:70d405bfdc8b 189 //pwm_in_trim_ch2 = pwm_in_cen_ch2 - 0.0015;
yal_kaiyo 0:70d405bfdc8b 190 pwm_in_trim_ch3 = pwm_in_low_ch3 - 0.0011;
yal_kaiyo 0:70d405bfdc8b 191 //pwm_in_trim_ch4 = pwm_in_cen_ch4 - 0.0015;
yal_kaiyo 0:70d405bfdc8b 192
yal_kaiyo 0:70d405bfdc8b 193 //pc.printf("%f %f %f %f\r\n", pwm_in_cen_ch1, pwm_in_cen_ch2, pwm_in_low_ch3, pwm_in_cen_ch4);
yal_kaiyo 0:70d405bfdc8b 194
yal_kaiyo 0:70d405bfdc8b 195 // Prepare ESC
yal_kaiyo 0:70d405bfdc8b 196 led3 = 1;
yal_kaiyo 0:70d405bfdc8b 197 esc1.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 198 esc2.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 199 esc3.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 200 esc4.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 201 wait(5);
yal_kaiyo 0:70d405bfdc8b 202 led3 = 0;
yal_kaiyo 0:70d405bfdc8b 203
yal_kaiyo 0:70d405bfdc8b 204 // LED for auto
yal_kaiyo 0:70d405bfdc8b 205 ex_led = 0;
yal_kaiyo 0:70d405bfdc8b 206
yal_kaiyo 0:70d405bfdc8b 207 //double dt_check = 0;
yal_kaiyo 0:70d405bfdc8b 208 //t_main.start();
yal_kaiyo 0:70d405bfdc8b 209
yal_kaiyo 0:70d405bfdc8b 210 // Main loop
yal_kaiyo 0:70d405bfdc8b 211 while(1)
yal_kaiyo 0:70d405bfdc8b 212 {
yal_kaiyo 0:70d405bfdc8b 213
yal_kaiyo 0:70d405bfdc8b 214 if((0.00165<pwm_in_ch5 || flag_rw_on==1) && flag_rw_off==0)
yal_kaiyo 0:70d405bfdc8b 215 {
yal_kaiyo 0:70d405bfdc8b 216 flag_mode = 2;
yal_kaiyo 0:70d405bfdc8b 217 flag_rw_on = 1;
yal_kaiyo 0:70d405bfdc8b 218 ex_led = 1;
yal_kaiyo 0:70d405bfdc8b 219 }
yal_kaiyo 0:70d405bfdc8b 220 else if(0.00165<pwm_in_ch5 && flag_rw_off==1) { flag_mode = 1; }
yal_kaiyo 0:70d405bfdc8b 221 else if(0.00135<pwm_in_ch5 && pwm_in_ch5<0.00165)
yal_kaiyo 0:70d405bfdc8b 222 {
yal_kaiyo 0:70d405bfdc8b 223 flag_mode = 1;
yal_kaiyo 0:70d405bfdc8b 224 flag_rw_off = 0;
yal_kaiyo 0:70d405bfdc8b 225 }
yal_kaiyo 0:70d405bfdc8b 226 else { flag_mode = 0; }
yal_kaiyo 0:70d405bfdc8b 227
yal_kaiyo 0:70d405bfdc8b 228 // Update sensors
yal_kaiyo 0:70d405bfdc8b 229 mpu6050.complementaryFilter(&pitch, &roll);
yal_kaiyo 0:70d405bfdc8b 230
yal_kaiyo 0:70d405bfdc8b 231 //
yal_kaiyo 0:70d405bfdc8b 232 pwm_in_mod_ch1 = pwm_in_ch1;
yal_kaiyo 0:70d405bfdc8b 233 pwm_in_mod_ch2 = pwm_in_ch2;
yal_kaiyo 0:70d405bfdc8b 234 pwm_in_mod_ch3 = pwm_in_ch3 - pwm_in_trim_ch3;
yal_kaiyo 0:70d405bfdc8b 235 pwm_in_mod_ch4 = pwm_in_ch4;
yal_kaiyo 0:70d405bfdc8b 236
yal_kaiyo 0:70d405bfdc8b 237 switch(flag_mode)
yal_kaiyo 0:70d405bfdc8b 238 {
yal_kaiyo 0:70d405bfdc8b 239 case 0:
yal_kaiyo 0:70d405bfdc8b 240
yal_kaiyo 0:70d405bfdc8b 241 esc1.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 242 esc2.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 243 esc3.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 244 esc4.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 245
yal_kaiyo 0:70d405bfdc8b 246 wait(0.004468);
yal_kaiyo 0:70d405bfdc8b 247
yal_kaiyo 0:70d405bfdc8b 248 break;
yal_kaiyo 0:70d405bfdc8b 249
yal_kaiyo 0:70d405bfdc8b 250 case 1:
yal_kaiyo 0:70d405bfdc8b 251
yal_kaiyo 0:70d405bfdc8b 252 // Control law (ch1: yaw, ch2: pitch, ch3: thr, ch4: roll)
yal_kaiyo 0:70d405bfdc8b 253 input_phi = - GAIN_P * gx + GAIN_PHI * (GAIN_PWM2ANGLE*(pwm_in_mod_ch2-0.0015) - pitch);
yal_kaiyo 0:70d405bfdc8b 254 input_the = - GAIN_Q * gy + GAIN_THE * (GAIN_PWM2ANGLE*(pwm_in_mod_ch4-0.0015) + roll);
yal_kaiyo 0:70d405bfdc8b 255 input_ome = - GAIN_R * gz + GAIN_OME * (pwm_in_mod_ch1-0.0015);
yal_kaiyo 0:70d405bfdc8b 256
yal_kaiyo 0:70d405bfdc8b 257 // ch1: left_front, ch3: right_front, ch4: left_rear, ch2: right_rear
yal_kaiyo 0:70d405bfdc8b 258 pwm_out_ch1 = pwm_in_mod_ch3 + input_phi + input_the + input_ome;
yal_kaiyo 0:70d405bfdc8b 259 pwm_out_ch3 = pwm_in_mod_ch3 + input_phi - input_the - input_ome;
yal_kaiyo 0:70d405bfdc8b 260 pwm_out_ch4 = pwm_in_mod_ch3 - input_phi + input_the - input_ome;
yal_kaiyo 0:70d405bfdc8b 261 pwm_out_ch2 = pwm_in_mod_ch3 - input_phi - input_the + input_ome;
yal_kaiyo 0:70d405bfdc8b 262
yal_kaiyo 0:70d405bfdc8b 263 // Limit pwm out
yal_kaiyo 0:70d405bfdc8b 264 if(pwm_out_ch1<0.00121){ pwm_out_ch1 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 265 if(pwm_out_ch2<0.00121){ pwm_out_ch2 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 266 if(pwm_out_ch3<0.00121){ pwm_out_ch3 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 267 if(pwm_out_ch4<0.00121){ pwm_out_ch4 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 268
yal_kaiyo 0:70d405bfdc8b 269 if(0.0019<pwm_out_ch1){ pwm_out_ch1 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 270 if(0.0019<pwm_out_ch2){ pwm_out_ch2 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 271 if(0.0019<pwm_out_ch3){ pwm_out_ch3 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 272 if(0.0019<pwm_out_ch4){ pwm_out_ch4 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 273
yal_kaiyo 0:70d405bfdc8b 274 // Set pwm out (1-2ms)
yal_kaiyo 0:70d405bfdc8b 275 esc1.pulsewidth(pwm_out_ch1);
yal_kaiyo 0:70d405bfdc8b 276 esc2.pulsewidth(pwm_out_ch2);
yal_kaiyo 0:70d405bfdc8b 277 esc3.pulsewidth(pwm_out_ch3);
yal_kaiyo 0:70d405bfdc8b 278 esc4.pulsewidth(pwm_out_ch4);
yal_kaiyo 0:70d405bfdc8b 279
yal_kaiyo 0:70d405bfdc8b 280 wait(0.004436);
yal_kaiyo 0:70d405bfdc8b 281
yal_kaiyo 0:70d405bfdc8b 282 break;
yal_kaiyo 0:70d405bfdc8b 283
yal_kaiyo 0:70d405bfdc8b 284 case 2:
yal_kaiyo 0:70d405bfdc8b 285
yal_kaiyo 0:70d405bfdc8b 286 // Inputs
yal_kaiyo 0:70d405bfdc8b 287 if(count_rw<COUNT_1_RW) { roll_target_rw = - TARGET_ANGLE_RW_1; }
yal_kaiyo 0:70d405bfdc8b 288 else if(COUNT_1_RW<count_rw && count_rw<COUNT_2_RW) { roll_target_rw = 0; }
yal_kaiyo 0:70d405bfdc8b 289 else if(COUNT_2_RW<=count_rw && count_rw<COUNT_3_RW) { roll_target_rw = TARGET_ANGLE_RW_1; }
yal_kaiyo 0:70d405bfdc8b 290 else if(COUNT_3_RW<=count_rw && count_rw<COUNT_4_RW) { roll_target_rw = -TARGET_ANGLE_RW_2; }
yal_kaiyo 0:70d405bfdc8b 291
yal_kaiyo 0:70d405bfdc8b 292 // Control law (ch1: yaw, ch2: pitch, ch3: thr, ch4: roll)
yal_kaiyo 0:70d405bfdc8b 293 input_phi = - GAIN_P * gx + GAIN_PHI * (phi_target_rw - pitch);
yal_kaiyo 0:70d405bfdc8b 294 input_the = - GAIN_Q * gy + GAIN_THE * (roll_target_rw + roll);
yal_kaiyo 0:70d405bfdc8b 295 input_ome = - GAIN_R * gz + GAIN_OME * (pwm_in_mod_ch1-0.0015);
yal_kaiyo 0:70d405bfdc8b 296
yal_kaiyo 0:70d405bfdc8b 297 // ch1: left_front, ch3: right_front, ch4: left_rear, ch2: right_rear
yal_kaiyo 0:70d405bfdc8b 298 pwm_out_ch1 = pwm_in_mod_ch3_prev + input_phi + input_the + input_ome;
yal_kaiyo 0:70d405bfdc8b 299 pwm_out_ch3 = pwm_in_mod_ch3_prev + input_phi - input_the - input_ome;
yal_kaiyo 0:70d405bfdc8b 300 pwm_out_ch4 = pwm_in_mod_ch3_prev - input_phi + input_the - input_ome;
yal_kaiyo 0:70d405bfdc8b 301 pwm_out_ch2 = pwm_in_mod_ch3_prev - input_phi - input_the + input_ome;
yal_kaiyo 0:70d405bfdc8b 302
yal_kaiyo 0:70d405bfdc8b 303 // Limit pwm out
yal_kaiyo 0:70d405bfdc8b 304 if(pwm_out_ch1<0.00121){ pwm_out_ch1 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 305 if(pwm_out_ch2<0.00121){ pwm_out_ch2 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 306 if(pwm_out_ch3<0.00121){ pwm_out_ch3 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 307 if(pwm_out_ch4<0.00121){ pwm_out_ch4 = 0.00121; }
yal_kaiyo 0:70d405bfdc8b 308
yal_kaiyo 0:70d405bfdc8b 309 if(0.0019<pwm_out_ch1){ pwm_out_ch1 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 310 if(0.0019<pwm_out_ch2){ pwm_out_ch2 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 311 if(0.0019<pwm_out_ch3){ pwm_out_ch3 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 312 if(0.0019<pwm_out_ch4){ pwm_out_ch4 = 0.0019; }
yal_kaiyo 0:70d405bfdc8b 313
yal_kaiyo 0:70d405bfdc8b 314 // Set pwm out (1-2ms)
yal_kaiyo 0:70d405bfdc8b 315 esc1.pulsewidth(pwm_out_ch1);
yal_kaiyo 0:70d405bfdc8b 316 esc2.pulsewidth(pwm_out_ch2);
yal_kaiyo 0:70d405bfdc8b 317 esc3.pulsewidth(pwm_out_ch3);
yal_kaiyo 0:70d405bfdc8b 318 esc4.pulsewidth(pwm_out_ch4);
yal_kaiyo 0:70d405bfdc8b 319
yal_kaiyo 0:70d405bfdc8b 320 count_rw++;
yal_kaiyo 0:70d405bfdc8b 321
yal_kaiyo 0:70d405bfdc8b 322 wait(0.004445);
yal_kaiyo 0:70d405bfdc8b 323
yal_kaiyo 0:70d405bfdc8b 324 if(COUNT_3_RW<=count_rw)
yal_kaiyo 0:70d405bfdc8b 325 {
yal_kaiyo 0:70d405bfdc8b 326 flag_rw_on = 0;
yal_kaiyo 0:70d405bfdc8b 327 flag_rw_off = 1;
yal_kaiyo 0:70d405bfdc8b 328 count_rw = 0;
yal_kaiyo 0:70d405bfdc8b 329 ex_led = 0;
yal_kaiyo 0:70d405bfdc8b 330 }
yal_kaiyo 0:70d405bfdc8b 331
yal_kaiyo 0:70d405bfdc8b 332 break;
yal_kaiyo 0:70d405bfdc8b 333
yal_kaiyo 0:70d405bfdc8b 334 default:
yal_kaiyo 0:70d405bfdc8b 335
yal_kaiyo 0:70d405bfdc8b 336 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
yal_kaiyo 0:70d405bfdc8b 337 esc1.pulsewidth(PWM_OUT_MIN); esc2.pulsewidth(PWM_OUT_MIN); esc3.pulsewidth(PWM_OUT_MIN); esc4.pulsewidth(PWM_OUT_MIN);
yal_kaiyo 0:70d405bfdc8b 338
yal_kaiyo 0:70d405bfdc8b 339 }
yal_kaiyo 0:70d405bfdc8b 340
yal_kaiyo 0:70d405bfdc8b 341 pwm_in_mod_ch3_prev = pwm_in_mod_ch3;
yal_kaiyo 0:70d405bfdc8b 342 /*
yal_kaiyo 0:70d405bfdc8b 343 if(200<count_print)
yal_kaiyo 0:70d405bfdc8b 344 {
yal_kaiyo 0:70d405bfdc8b 345 //pc.printf("%9.6f, %9.6f, %9.6f, %9.6f\r\n", pwm_in_mod_ch1, pwm_in_mod_ch2, pwm_in_mod_ch3, pwm_in_mod_ch4);
yal_kaiyo 0:70d405bfdc8b 346 //pc.printf("%7.3f, %7.3f, %7.3f, %7.3f, %7.3f\r\n", gx, gy, gz, pitch, roll);
yal_kaiyo 0:70d405bfdc8b 347 //pc.printf("%d, %3.0f, %7.3f, %7.3f, %13.10f, %13.10f\r\n", flag_mode, roll_target_rw, pitch, roll, input_phi, input_the);
yal_kaiyo 0:70d405bfdc8b 348 pc.printf("%d, %3.0f, %4.0f, %4.0f, %4.0f, %4.0f\r\n", flag_mode, roll_target_rw, pwm_out_ch1*1000000, pwm_out_ch2*1000000, pwm_out_ch3*1000000, pwm_out_ch4*1000000);
yal_kaiyo 0:70d405bfdc8b 349
yal_kaiyo 0:70d405bfdc8b 350 //t_main.stop();
yal_kaiyo 0:70d405bfdc8b 351 //dt_check = t_main.read();
yal_kaiyo 0:70d405bfdc8b 352 //pc.printf("%8.6f\r\n", dt_check);
yal_kaiyo 0:70d405bfdc8b 353 //t_main.reset();
yal_kaiyo 0:70d405bfdc8b 354 //t_main.start();
yal_kaiyo 0:70d405bfdc8b 355
yal_kaiyo 0:70d405bfdc8b 356 count_print = 0;
yal_kaiyo 0:70d405bfdc8b 357 }
yal_kaiyo 0:70d405bfdc8b 358 count_print++;
yal_kaiyo 0:70d405bfdc8b 359 */
yal_kaiyo 0:70d405bfdc8b 360 }
yal_kaiyo 0:70d405bfdc8b 361 }
yal_kaiyo 0:70d405bfdc8b 362
yal_kaiyo 0:70d405bfdc8b 363 void toggle_led1() {ledToggle(1);}
yal_kaiyo 0:70d405bfdc8b 364 void toggle_led2() {ledToggle(2);}
yal_kaiyo 0:70d405bfdc8b 365
yal_kaiyo 0:70d405bfdc8b 366 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 367 // Functions for read ESC
yal_kaiyo 0:70d405bfdc8b 368 //////////////////////////////////////////////////
yal_kaiyo 0:70d405bfdc8b 369
yal_kaiyo 0:70d405bfdc8b 370 void ch1_rise() { t_ch1.start(); }
yal_kaiyo 0:70d405bfdc8b 371 void ch2_rise() { t_ch2.start(); }
yal_kaiyo 0:70d405bfdc8b 372 void ch3_rise() { t_ch3.start(); }
yal_kaiyo 0:70d405bfdc8b 373 void ch4_rise() { t_ch4.start(); }
yal_kaiyo 0:70d405bfdc8b 374 void ch5_rise() { t_ch5.start(); }
yal_kaiyo 0:70d405bfdc8b 375 //void ch6_rise() { t_ch6.start(); }
yal_kaiyo 0:70d405bfdc8b 376
yal_kaiyo 0:70d405bfdc8b 377 void ch1_fall(){
yal_kaiyo 0:70d405bfdc8b 378 t_ch1.stop();
yal_kaiyo 0:70d405bfdc8b 379 pwm_in_ch1 = t_ch1.read();
yal_kaiyo 0:70d405bfdc8b 380 t_ch1.reset();
yal_kaiyo 0:70d405bfdc8b 381 }
yal_kaiyo 0:70d405bfdc8b 382
yal_kaiyo 0:70d405bfdc8b 383 void ch2_fall(){
yal_kaiyo 0:70d405bfdc8b 384 t_ch2.stop();
yal_kaiyo 0:70d405bfdc8b 385 pwm_in_ch2 = t_ch2.read();
yal_kaiyo 0:70d405bfdc8b 386 t_ch2.reset();
yal_kaiyo 0:70d405bfdc8b 387 }
yal_kaiyo 0:70d405bfdc8b 388
yal_kaiyo 0:70d405bfdc8b 389 void ch3_fall(){
yal_kaiyo 0:70d405bfdc8b 390 t_ch3.stop();
yal_kaiyo 0:70d405bfdc8b 391 pwm_in_ch3 = t_ch3.read();
yal_kaiyo 0:70d405bfdc8b 392 t_ch3.reset();
yal_kaiyo 0:70d405bfdc8b 393 //pc.printf("%f\r\n", pwm_in_ch3);
yal_kaiyo 0:70d405bfdc8b 394 }
yal_kaiyo 0:70d405bfdc8b 395
yal_kaiyo 0:70d405bfdc8b 396 void ch4_fall(){
yal_kaiyo 0:70d405bfdc8b 397 t_ch4.stop();
yal_kaiyo 0:70d405bfdc8b 398 pwm_in_ch4 = t_ch4.read();
yal_kaiyo 0:70d405bfdc8b 399 t_ch4.reset();
yal_kaiyo 0:70d405bfdc8b 400 }
yal_kaiyo 0:70d405bfdc8b 401
yal_kaiyo 0:70d405bfdc8b 402 void ch5_fall()
yal_kaiyo 0:70d405bfdc8b 403 {
yal_kaiyo 0:70d405bfdc8b 404 t_ch5.stop();
yal_kaiyo 0:70d405bfdc8b 405 pwm_in_ch5 = t_ch5.read();
yal_kaiyo 0:70d405bfdc8b 406 t_ch5.reset();
yal_kaiyo 0:70d405bfdc8b 407 }
yal_kaiyo 0:70d405bfdc8b 408 /*
yal_kaiyo 0:70d405bfdc8b 409 void ch6_fall()
yal_kaiyo 0:70d405bfdc8b 410 {
yal_kaiyo 0:70d405bfdc8b 411 t_ch6.stop();
yal_kaiyo 0:70d405bfdc8b 412 pwm_in_ch6 = t_ch6.read();
yal_kaiyo 0:70d405bfdc8b 413 t_ch6.reset();
yal_kaiyo 0:70d405bfdc8b 414 }
yal_kaiyo 0:70d405bfdc8b 415 */