hige dura
/
yalHackVelFB
Velocity feedback for YAL RChack
main.cpp@0:72e0bf0dbe7c, 2014-11-06 (annotated)
- Committer:
- higedura
- Date:
- Thu Nov 06 10:13:27 2014 +0000
- Revision:
- 0:72e0bf0dbe7c
Velocity feedback for YAL RChack
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
higedura | 0:72e0bf0dbe7c | 1 | #include "mbed.h" |
higedura | 0:72e0bf0dbe7c | 2 | |
higedura | 0:72e0bf0dbe7c | 3 | #define N_CHAN 3 // the numbers of channels |
higedura | 0:72e0bf0dbe7c | 4 | #define SIZE_DATA 16 // size of vz data |
higedura | 0:72e0bf0dbe7c | 5 | |
higedura | 0:72e0bf0dbe7c | 6 | Serial pc(USBTX, USBRX); |
higedura | 0:72e0bf0dbe7c | 7 | |
higedura | 0:72e0bf0dbe7c | 8 | DigitalOut led1(LED1); |
higedura | 0:72e0bf0dbe7c | 9 | DigitalOut led2(LED2); |
higedura | 0:72e0bf0dbe7c | 10 | DigitalOut led3(LED3); |
higedura | 0:72e0bf0dbe7c | 11 | DigitalOut led4(LED4); |
higedura | 0:72e0bf0dbe7c | 12 | |
higedura | 0:72e0bf0dbe7c | 13 | //InterruptIn ch1_in(p5); // roll |
higedura | 0:72e0bf0dbe7c | 14 | InterruptIn ch2_in(p5); // pitch |
higedura | 0:72e0bf0dbe7c | 15 | InterruptIn ch3_in(p6); // throttle |
higedura | 0:72e0bf0dbe7c | 16 | //InterruptIn ch4_in(p7); // taw |
higedura | 0:72e0bf0dbe7c | 17 | InterruptIn ch5_in(p7); // aux1 |
higedura | 0:72e0bf0dbe7c | 18 | //InterruptIn ch6_in(p18); // aux2 |
higedura | 0:72e0bf0dbe7c | 19 | |
higedura | 0:72e0bf0dbe7c | 20 | DigitalIn button(p12); |
higedura | 0:72e0bf0dbe7c | 21 | |
higedura | 0:72e0bf0dbe7c | 22 | Serial xbee(p13, p14); // tx, rx |
higedura | 0:72e0bf0dbe7c | 23 | |
higedura | 0:72e0bf0dbe7c | 24 | //PwmOut ch1_out(p21); // rall |
higedura | 0:72e0bf0dbe7c | 25 | PwmOut ch2_out(p21); // pitch |
higedura | 0:72e0bf0dbe7c | 26 | PwmOut ch3_out(p22); // throttle |
higedura | 0:72e0bf0dbe7c | 27 | //PwmOut ch4_out(p24); // yaw |
higedura | 0:72e0bf0dbe7c | 28 | //PwmOut ch5_out(p25); // aux1 |
higedura | 0:72e0bf0dbe7c | 29 | //PwmOut ch6_out(p26); // aux2 |
higedura | 0:72e0bf0dbe7c | 30 | |
higedura | 0:72e0bf0dbe7c | 31 | ///////////////////// Function prototype //////////////////// |
higedura | 0:72e0bf0dbe7c | 32 | //void roll_up(void); |
higedura | 0:72e0bf0dbe7c | 33 | void pitch_up(void); |
higedura | 0:72e0bf0dbe7c | 34 | void thr_up(void); |
higedura | 0:72e0bf0dbe7c | 35 | //void yaw_up(void); |
higedura | 0:72e0bf0dbe7c | 36 | void aux1_up(void); |
higedura | 0:72e0bf0dbe7c | 37 | //void aux2_up(void); |
higedura | 0:72e0bf0dbe7c | 38 | |
higedura | 0:72e0bf0dbe7c | 39 | //void rall_down(void); |
higedura | 0:72e0bf0dbe7c | 40 | void pitch_down(void); |
higedura | 0:72e0bf0dbe7c | 41 | void thr_down(void); |
higedura | 0:72e0bf0dbe7c | 42 | //void yaw_down(void); |
higedura | 0:72e0bf0dbe7c | 43 | void aux1_down(void); |
higedura | 0:72e0bf0dbe7c | 44 | //void aux2_down(void); |
higedura | 0:72e0bf0dbe7c | 45 | |
higedura | 0:72e0bf0dbe7c | 46 | ///////////////////// Timers //////////////////// |
higedura | 0:72e0bf0dbe7c | 47 | //Timer t_roll; |
higedura | 0:72e0bf0dbe7c | 48 | Timer t_pitch; |
higedura | 0:72e0bf0dbe7c | 49 | Timer t_thr; |
higedura | 0:72e0bf0dbe7c | 50 | //Timer t_yaw; |
higedura | 0:72e0bf0dbe7c | 51 | Timer t_aux1; |
higedura | 0:72e0bf0dbe7c | 52 | //Timer t_aux2; |
higedura | 0:72e0bf0dbe7c | 53 | |
higedura | 0:72e0bf0dbe7c | 54 | ///////////////////// Global variable //////////////////// |
higedura | 0:72e0bf0dbe7c | 55 | int g_pwm_pulse[N_CHAN] = {0}; // pitch thr aux1 |
higedura | 0:72e0bf0dbe7c | 56 | |
higedura | 0:72e0bf0dbe7c | 57 | ///////////////////// Local file system //////////////////// |
higedura | 0:72e0bf0dbe7c | 58 | LocalFileSystem local("local"); |
higedura | 0:72e0bf0dbe7c | 59 | |
higedura | 0:72e0bf0dbe7c | 60 | ///////////////////// Main loop //////////////////// |
higedura | 0:72e0bf0dbe7c | 61 | int main() { |
higedura | 0:72e0bf0dbe7c | 62 | |
higedura | 0:72e0bf0dbe7c | 63 | pc.baud(921600); |
higedura | 0:72e0bf0dbe7c | 64 | xbee.baud(57600); |
higedura | 0:72e0bf0dbe7c | 65 | |
higedura | 0:72e0bf0dbe7c | 66 | double GAIN_VEL_X = 0.2; |
higedura | 0:72e0bf0dbe7c | 67 | //double GAIN_VEL_Z = 0; |
higedura | 0:72e0bf0dbe7c | 68 | |
higedura | 0:72e0bf0dbe7c | 69 | double VEL_X_TARGET = 500; |
higedura | 0:72e0bf0dbe7c | 70 | |
higedura | 0:72e0bf0dbe7c | 71 | double LIMIT_PULTH_WIDTH_CH2_OUT_LOW = 0.0013; |
higedura | 0:72e0bf0dbe7c | 72 | double LIMIT_PULTH_WIDTH_CH2_OUT_UP = 0.0017; |
higedura | 0:72e0bf0dbe7c | 73 | |
higedura | 0:72e0bf0dbe7c | 74 | int pos[2] = {0}; |
higedura | 0:72e0bf0dbe7c | 75 | int vel[2] = {0}; |
higedura | 0:72e0bf0dbe7c | 76 | |
higedura | 0:72e0bf0dbe7c | 77 | int PULTH_WIDTH_RC_TRIM_CH2 = 0; |
higedura | 0:72e0bf0dbe7c | 78 | int PULTH_WIDTH_RC_TRIM_CH3 = 0; |
higedura | 0:72e0bf0dbe7c | 79 | |
higedura | 0:72e0bf0dbe7c | 80 | double pulse_width_ch2_out = 0.0015; |
higedura | 0:72e0bf0dbe7c | 81 | double pulse_width_ch3_out = 0.001; |
higedura | 0:72e0bf0dbe7c | 82 | |
higedura | 0:72e0bf0dbe7c | 83 | ch2_out.period(0.02); |
higedura | 0:72e0bf0dbe7c | 84 | ch3_out.period(0.02); |
higedura | 0:72e0bf0dbe7c | 85 | |
higedura | 0:72e0bf0dbe7c | 86 | ch2_out.pulsewidth(0.0015); // pitch |
higedura | 0:72e0bf0dbe7c | 87 | ch3_out.pulsewidth(0.0011); // thr |
higedura | 0:72e0bf0dbe7c | 88 | |
higedura | 0:72e0bf0dbe7c | 89 | /////////////////// Interrupt for xbee //////////////////// |
higedura | 0:72e0bf0dbe7c | 90 | //xbee.attach(&get_vel); |
higedura | 0:72e0bf0dbe7c | 91 | |
higedura | 0:72e0bf0dbe7c | 92 | //////////////////// Interrupt for RC reciever //////////////////// |
higedura | 0:72e0bf0dbe7c | 93 | //ch1_in.rise(&roll_up); ch1_in.fall(&roll_down); |
higedura | 0:72e0bf0dbe7c | 94 | ch2_in.rise(&pitch_up); ch2_in.fall(&pitch_down); |
higedura | 0:72e0bf0dbe7c | 95 | ch3_in.rise(&thr_up); ch3_in.fall(&thr_down); |
higedura | 0:72e0bf0dbe7c | 96 | //ch4_in.rise(&yaw_up); ch4_in.fall(&yaw_down); |
higedura | 0:72e0bf0dbe7c | 97 | ch5_in.rise(&aux1_up); ch5_in.fall(&aux1_down); |
higedura | 0:72e0bf0dbe7c | 98 | //ch6_in.rise(&aux2_up); ch6_in.fall(&aux2_down); |
higedura | 0:72e0bf0dbe7c | 99 | |
higedura | 0:72e0bf0dbe7c | 100 | // pos初期値取得 |
higedura | 0:72e0bf0dbe7c | 101 | // if pos 初期値(0,0) -> noVzかも |
higedura | 0:72e0bf0dbe7c | 102 | |
higedura | 0:72e0bf0dbe7c | 103 | // Get RC trim |
higedura | 0:72e0bf0dbe7c | 104 | for(int i=0;i<10;i++){ |
higedura | 0:72e0bf0dbe7c | 105 | wait(.1); |
higedura | 0:72e0bf0dbe7c | 106 | PULTH_WIDTH_RC_TRIM_CH2 += g_pwm_pulse[0]; |
higedura | 0:72e0bf0dbe7c | 107 | PULTH_WIDTH_RC_TRIM_CH3 += g_pwm_pulse[1]; |
higedura | 0:72e0bf0dbe7c | 108 | } |
higedura | 0:72e0bf0dbe7c | 109 | PULTH_WIDTH_RC_TRIM_CH2 = PULTH_WIDTH_RC_TRIM_CH2 / 10; |
higedura | 0:72e0bf0dbe7c | 110 | PULTH_WIDTH_RC_TRIM_CH3 = PULTH_WIDTH_RC_TRIM_CH3 / 10; |
higedura | 0:72e0bf0dbe7c | 111 | |
higedura | 0:72e0bf0dbe7c | 112 | // Check RC trim |
higedura | 0:72e0bf0dbe7c | 113 | if(PULTH_WIDTH_RC_TRIM_CH2 < 450 || 550 < PULTH_WIDTH_RC_TRIM_CH2){ |
higedura | 0:72e0bf0dbe7c | 114 | //pc.printf("%d\r\n", PULTH_WIDTH_RC_TRIM_CH2); |
higedura | 0:72e0bf0dbe7c | 115 | while(1){ |
higedura | 0:72e0bf0dbe7c | 116 | led2 = 1; |
higedura | 0:72e0bf0dbe7c | 117 | led3 = 0; |
higedura | 0:72e0bf0dbe7c | 118 | wait(0.5); |
higedura | 0:72e0bf0dbe7c | 119 | led2 = 0; |
higedura | 0:72e0bf0dbe7c | 120 | led3 = 1; |
higedura | 0:72e0bf0dbe7c | 121 | wait(0.5); |
higedura | 0:72e0bf0dbe7c | 122 | } |
higedura | 0:72e0bf0dbe7c | 123 | } |
higedura | 0:72e0bf0dbe7c | 124 | |
higedura | 0:72e0bf0dbe7c | 125 | //////////////////// Stand by //////////////////// |
higedura | 0:72e0bf0dbe7c | 126 | |
higedura | 0:72e0bf0dbe7c | 127 | ch2_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 128 | ch3_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 129 | ch5_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 130 | |
higedura | 0:72e0bf0dbe7c | 131 | while(button < 0.5){ |
higedura | 0:72e0bf0dbe7c | 132 | led1 = 1; |
higedura | 0:72e0bf0dbe7c | 133 | led4 = 1; |
higedura | 0:72e0bf0dbe7c | 134 | wait(0.5); |
higedura | 0:72e0bf0dbe7c | 135 | led1 = 0; |
higedura | 0:72e0bf0dbe7c | 136 | led4 = 0; |
higedura | 0:72e0bf0dbe7c | 137 | wait(0.5); |
higedura | 0:72e0bf0dbe7c | 138 | } |
higedura | 0:72e0bf0dbe7c | 139 | |
higedura | 0:72e0bf0dbe7c | 140 | wait(3); |
higedura | 0:72e0bf0dbe7c | 141 | |
higedura | 0:72e0bf0dbe7c | 142 | FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing |
higedura | 0:72e0bf0dbe7c | 143 | |
higedura | 0:72e0bf0dbe7c | 144 | led1 = 1; |
higedura | 0:72e0bf0dbe7c | 145 | led4 = 1; |
higedura | 0:72e0bf0dbe7c | 146 | |
higedura | 0:72e0bf0dbe7c | 147 | while(button < 0.5){ |
higedura | 0:72e0bf0dbe7c | 148 | //while(1){ |
higedura | 0:72e0bf0dbe7c | 149 | |
higedura | 0:72e0bf0dbe7c | 150 | for(int i=0;i<5;i++){ |
higedura | 0:72e0bf0dbe7c | 151 | |
higedura | 0:72e0bf0dbe7c | 152 | //////////////////// Get PWM //////////////////// |
higedura | 0:72e0bf0dbe7c | 153 | ch2_in.enable_irq(); |
higedura | 0:72e0bf0dbe7c | 154 | ch3_in.enable_irq(); |
higedura | 0:72e0bf0dbe7c | 155 | ch5_in.enable_irq(); |
higedura | 0:72e0bf0dbe7c | 156 | |
higedura | 0:72e0bf0dbe7c | 157 | wait(.02); // Interval to get pwm |
higedura | 0:72e0bf0dbe7c | 158 | |
higedura | 0:72e0bf0dbe7c | 159 | ch2_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 160 | ch3_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 161 | ch5_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 162 | |
higedura | 0:72e0bf0dbe7c | 163 | //////////////////// Get pos and vel //////////////////// |
higedura | 0:72e0bf0dbe7c | 164 | if(xbee.readable()){ |
higedura | 0:72e0bf0dbe7c | 165 | // get vz data |
higedura | 0:72e0bf0dbe7c | 166 | int buf_char[SIZE_DATA] = {0}; |
higedura | 0:72e0bf0dbe7c | 167 | |
higedura | 0:72e0bf0dbe7c | 168 | // decode vz data |
higedura | 0:72e0bf0dbe7c | 169 | for(int j=0;j<SIZE_DATA;j++){ buf_char[j] = (int)xbee.getc() - 48; } |
higedura | 0:72e0bf0dbe7c | 170 | |
higedura | 0:72e0bf0dbe7c | 171 | //for(int i=0;i<SIZE_DATA;i++){ pc.printf("%5d ", buf_char[i]); } |
higedura | 0:72e0bf0dbe7c | 172 | //pc.printf("\r\n"); |
higedura | 0:72e0bf0dbe7c | 173 | |
higedura | 0:72e0bf0dbe7c | 174 | pos[1] = 1000*buf_char[0] + 100*buf_char[1] + 10*buf_char[2] + buf_char[3] - 5000; |
higedura | 0:72e0bf0dbe7c | 175 | pos[0] = 1000*buf_char[4] + 100*buf_char[5] + 10*buf_char[6] + buf_char[7] - 5000; |
higedura | 0:72e0bf0dbe7c | 176 | |
higedura | 0:72e0bf0dbe7c | 177 | vel[1] = 1000* buf_char[8] + 100* buf_char[9] + 10*buf_char[10] + buf_char[11] - 5000; |
higedura | 0:72e0bf0dbe7c | 178 | vel[0] = 1000*buf_char[12] + 100*buf_char[13] + 10*buf_char[14] + buf_char[15] - 5000; |
higedura | 0:72e0bf0dbe7c | 179 | }else{ |
higedura | 0:72e0bf0dbe7c | 180 | |
higedura | 0:72e0bf0dbe7c | 181 | // しばらくこなかったらautoにならないフラグとか |
higedura | 0:72e0bf0dbe7c | 182 | |
higedura | 0:72e0bf0dbe7c | 183 | } |
higedura | 0:72e0bf0dbe7c | 184 | |
higedura | 0:72e0bf0dbe7c | 185 | //////////////////// Control law //////////////////// |
higedura | 0:72e0bf0dbe7c | 186 | if(500 < g_pwm_pulse[2]){ |
higedura | 0:72e0bf0dbe7c | 187 | |
higedura | 0:72e0bf0dbe7c | 188 | // Constant velocity F/B |
higedura | 0:72e0bf0dbe7c | 189 | |
higedura | 0:72e0bf0dbe7c | 190 | //pulse_width_ch2_out = 0.001 + 0.000001 * g_pwm_pulse[0]; |
higedura | 0:72e0bf0dbe7c | 191 | pulse_width_ch2_out = 0.001 + 0.000001 * PULTH_WIDTH_RC_TRIM_CH2 + (-1) * 0.000001 * GAIN_VEL_X * (VEL_X_TARGET - vel[0]); |
higedura | 0:72e0bf0dbe7c | 192 | |
higedura | 0:72e0bf0dbe7c | 193 | pulse_width_ch3_out = 0.001 + 0.000001 * g_pwm_pulse[1]; |
higedura | 0:72e0bf0dbe7c | 194 | //pulse_width_ch3_out = 0.001 + PULTH_WIDTH_RC_TRIM_CH3 + GAIN_VEL_Z * (-vel[1]); |
higedura | 0:72e0bf0dbe7c | 195 | |
higedura | 0:72e0bf0dbe7c | 196 | }else{ |
higedura | 0:72e0bf0dbe7c | 197 | |
higedura | 0:72e0bf0dbe7c | 198 | // without RC-hack |
higedura | 0:72e0bf0dbe7c | 199 | pulse_width_ch2_out = 0.001 + 0.000001 * g_pwm_pulse[0]; |
higedura | 0:72e0bf0dbe7c | 200 | pulse_width_ch3_out = 0.001 + 0.000001 * g_pwm_pulse[1]; |
higedura | 0:72e0bf0dbe7c | 201 | |
higedura | 0:72e0bf0dbe7c | 202 | } |
higedura | 0:72e0bf0dbe7c | 203 | |
higedura | 0:72e0bf0dbe7c | 204 | // Limmiter for RC |
higedura | 0:72e0bf0dbe7c | 205 | if(pulse_width_ch2_out < LIMIT_PULTH_WIDTH_CH2_OUT_LOW){ pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_LOW; } |
higedura | 0:72e0bf0dbe7c | 206 | if(LIMIT_PULTH_WIDTH_CH2_OUT_UP < pulse_width_ch2_out) { pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_UP; } |
higedura | 0:72e0bf0dbe7c | 207 | |
higedura | 0:72e0bf0dbe7c | 208 | ch2_out.pulsewidth(pulse_width_ch2_out); |
higedura | 0:72e0bf0dbe7c | 209 | ch3_out.pulsewidth(pulse_width_ch3_out); |
higedura | 0:72e0bf0dbe7c | 210 | |
higedura | 0:72e0bf0dbe7c | 211 | } |
higedura | 0:72e0bf0dbe7c | 212 | |
higedura | 0:72e0bf0dbe7c | 213 | |
higedura | 0:72e0bf0dbe7c | 214 | //for(int i=0;i<N_CHAN;i++){ pc.printf("%5d ", g_pwm_pulse[i]); } |
higedura | 0:72e0bf0dbe7c | 215 | /* |
higedura | 0:72e0bf0dbe7c | 216 | pc.printf(" "); |
higedura | 0:72e0bf0dbe7c | 217 | for(int i=0;i<2;i++){ pc.printf("%5d ", pos[i]); } |
higedura | 0:72e0bf0dbe7c | 218 | for(int i=0;i<2;i++){ pc.printf("%5d ", vel[i]); } |
higedura | 0:72e0bf0dbe7c | 219 | pc.printf("\r\n"); |
higedura | 0:72e0bf0dbe7c | 220 | */ |
higedura | 0:72e0bf0dbe7c | 221 | fprintf(fp, "%f %f %5d %5d %5d %5d %5d \r\n", pulse_width_ch2_out, pulse_width_ch3_out, g_pwm_pulse[2], pos[0], pos[1], vel[0], vel[1]); |
higedura | 0:72e0bf0dbe7c | 222 | |
higedura | 0:72e0bf0dbe7c | 223 | } |
higedura | 0:72e0bf0dbe7c | 224 | |
higedura | 0:72e0bf0dbe7c | 225 | ch2_out.pulsewidth(0.0015); |
higedura | 0:72e0bf0dbe7c | 226 | ch3_out.pulsewidth(0.0011); |
higedura | 0:72e0bf0dbe7c | 227 | |
higedura | 0:72e0bf0dbe7c | 228 | ch2_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 229 | ch3_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 230 | ch5_in.disable_irq(); |
higedura | 0:72e0bf0dbe7c | 231 | |
higedura | 0:72e0bf0dbe7c | 232 | fclose(fp); |
higedura | 0:72e0bf0dbe7c | 233 | |
higedura | 0:72e0bf0dbe7c | 234 | led1 = 0; |
higedura | 0:72e0bf0dbe7c | 235 | led4 = 0; |
higedura | 0:72e0bf0dbe7c | 236 | led2 = 1; |
higedura | 0:72e0bf0dbe7c | 237 | led3 = 1; |
higedura | 0:72e0bf0dbe7c | 238 | } |
higedura | 0:72e0bf0dbe7c | 239 | |
higedura | 0:72e0bf0dbe7c | 240 | |
higedura | 0:72e0bf0dbe7c | 241 | //////////////////// RC interrupt (up) //////////////////// |
higedura | 0:72e0bf0dbe7c | 242 | //void roll_up(){ t_roll.start(); } |
higedura | 0:72e0bf0dbe7c | 243 | void pitch_up(){ t_pitch.start(); } |
higedura | 0:72e0bf0dbe7c | 244 | void thr_up(){ t_thr.start(); } |
higedura | 0:72e0bf0dbe7c | 245 | //void yaw_up(){ t_yaw.start(); } |
higedura | 0:72e0bf0dbe7c | 246 | void aux1_up(){ t_aux1.start(); } |
higedura | 0:72e0bf0dbe7c | 247 | //void aux2_up(){ t_aux2.start(); } |
higedura | 0:72e0bf0dbe7c | 248 | |
higedura | 0:72e0bf0dbe7c | 249 | |
higedura | 0:72e0bf0dbe7c | 250 | //////////////////// RC interrupt (down) //////////////////// |
higedura | 0:72e0bf0dbe7c | 251 | /* |
higedura | 0:72e0bf0dbe7c | 252 | void roll_down(){ |
higedura | 0:72e0bf0dbe7c | 253 | t_roll.stop(); |
higedura | 0:72e0bf0dbe7c | 254 | g_pwm_pulse[] = (int)(t_roll.read()*1000000-1000); |
higedura | 0:72e0bf0dbe7c | 255 | t_roll.reset(); |
higedura | 0:72e0bf0dbe7c | 256 | } |
higedura | 0:72e0bf0dbe7c | 257 | */ |
higedura | 0:72e0bf0dbe7c | 258 | void pitch_down(){ |
higedura | 0:72e0bf0dbe7c | 259 | t_pitch.stop(); |
higedura | 0:72e0bf0dbe7c | 260 | g_pwm_pulse[0] = (int)(t_pitch.read()*1000000-1000); |
higedura | 0:72e0bf0dbe7c | 261 | t_pitch.reset(); |
higedura | 0:72e0bf0dbe7c | 262 | } |
higedura | 0:72e0bf0dbe7c | 263 | |
higedura | 0:72e0bf0dbe7c | 264 | void thr_down(){ |
higedura | 0:72e0bf0dbe7c | 265 | t_thr.stop(); |
higedura | 0:72e0bf0dbe7c | 266 | g_pwm_pulse[1] = (int)(t_thr.read()*1000000-1000); |
higedura | 0:72e0bf0dbe7c | 267 | t_thr.reset(); |
higedura | 0:72e0bf0dbe7c | 268 | } |
higedura | 0:72e0bf0dbe7c | 269 | /* |
higedura | 0:72e0bf0dbe7c | 270 | void yaw_down(){ |
higedura | 0:72e0bf0dbe7c | 271 | t_yaw.stop(); |
higedura | 0:72e0bf0dbe7c | 272 | g_pwm_pulse[] = (int)(t_yaw.read()*1000000-1000); |
higedura | 0:72e0bf0dbe7c | 273 | t_yaw.reset(); |
higedura | 0:72e0bf0dbe7c | 274 | } |
higedura | 0:72e0bf0dbe7c | 275 | */ |
higedura | 0:72e0bf0dbe7c | 276 | void aux1_down(){ |
higedura | 0:72e0bf0dbe7c | 277 | t_aux1.stop(); |
higedura | 0:72e0bf0dbe7c | 278 | g_pwm_pulse[2] = (int)(t_aux1.read()*1000000-1000); |
higedura | 0:72e0bf0dbe7c | 279 | t_aux1.reset(); |
higedura | 0:72e0bf0dbe7c | 280 | } |
higedura | 0:72e0bf0dbe7c | 281 | /* |
higedura | 0:72e0bf0dbe7c | 282 | void aux2_down(){ |
higedura | 0:72e0bf0dbe7c | 283 | t_aux2.stop(); |
higedura | 0:72e0bf0dbe7c | 284 | g_pwm_pulse[] = (int)(t_aux2.read()*1000000-1000); |
higedura | 0:72e0bf0dbe7c | 285 | t_aux2.reset(); |
higedura | 0:72e0bf0dbe7c | 286 | } |
higedura | 0:72e0bf0dbe7c | 287 | */ |