hige dura
/
yalHackVelFB
Velocity feedback for YAL RChack
Revision 0:72e0bf0dbe7c, committed 2014-11-06
- Comitter:
- higedura
- Date:
- Thu Nov 06 10:13:27 2014 +0000
- Commit message:
- Velocity feedback for YAL RChack
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 72e0bf0dbe7c main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Nov 06 10:13:27 2014 +0000 @@ -0,0 +1,287 @@ +#include "mbed.h" + +#define N_CHAN 3 // the numbers of channels +#define SIZE_DATA 16 // size of vz data + +Serial pc(USBTX, USBRX); + +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); + +//InterruptIn ch1_in(p5); // roll +InterruptIn ch2_in(p5); // pitch +InterruptIn ch3_in(p6); // throttle +//InterruptIn ch4_in(p7); // taw +InterruptIn ch5_in(p7); // aux1 +//InterruptIn ch6_in(p18); // aux2 + +DigitalIn button(p12); + +Serial xbee(p13, p14); // tx, rx + +//PwmOut ch1_out(p21); // rall +PwmOut ch2_out(p21); // pitch +PwmOut ch3_out(p22); // throttle +//PwmOut ch4_out(p24); // yaw +//PwmOut ch5_out(p25); // aux1 +//PwmOut ch6_out(p26); // aux2 + +///////////////////// Function prototype //////////////////// +//void roll_up(void); +void pitch_up(void); +void thr_up(void); +//void yaw_up(void); +void aux1_up(void); +//void aux2_up(void); + +//void rall_down(void); +void pitch_down(void); +void thr_down(void); +//void yaw_down(void); +void aux1_down(void); +//void aux2_down(void); + +///////////////////// Timers //////////////////// +//Timer t_roll; +Timer t_pitch; +Timer t_thr; +//Timer t_yaw; +Timer t_aux1; +//Timer t_aux2; + +///////////////////// Global variable //////////////////// +int g_pwm_pulse[N_CHAN] = {0}; // pitch thr aux1 + +///////////////////// Local file system //////////////////// +LocalFileSystem local("local"); + +///////////////////// Main loop //////////////////// +int main() { + + pc.baud(921600); + xbee.baud(57600); + + double GAIN_VEL_X = 0.2; + //double GAIN_VEL_Z = 0; + + double VEL_X_TARGET = 500; + + double LIMIT_PULTH_WIDTH_CH2_OUT_LOW = 0.0013; + double LIMIT_PULTH_WIDTH_CH2_OUT_UP = 0.0017; + + int pos[2] = {0}; + int vel[2] = {0}; + + int PULTH_WIDTH_RC_TRIM_CH2 = 0; + int PULTH_WIDTH_RC_TRIM_CH3 = 0; + + double pulse_width_ch2_out = 0.0015; + double pulse_width_ch3_out = 0.001; + + ch2_out.period(0.02); + ch3_out.period(0.02); + + ch2_out.pulsewidth(0.0015); // pitch + ch3_out.pulsewidth(0.0011); // thr + + /////////////////// Interrupt for xbee //////////////////// + //xbee.attach(&get_vel); + + //////////////////// Interrupt for RC reciever //////////////////// + //ch1_in.rise(&roll_up); ch1_in.fall(&roll_down); + ch2_in.rise(&pitch_up); ch2_in.fall(&pitch_down); + ch3_in.rise(&thr_up); ch3_in.fall(&thr_down); + //ch4_in.rise(&yaw_up); ch4_in.fall(&yaw_down); + ch5_in.rise(&aux1_up); ch5_in.fall(&aux1_down); + //ch6_in.rise(&aux2_up); ch6_in.fall(&aux2_down); + + // pos初期値取得 + // if pos 初期値(0,0) -> noVzかも + + // Get RC trim + for(int i=0;i<10;i++){ + wait(.1); + PULTH_WIDTH_RC_TRIM_CH2 += g_pwm_pulse[0]; + PULTH_WIDTH_RC_TRIM_CH3 += g_pwm_pulse[1]; + } + PULTH_WIDTH_RC_TRIM_CH2 = PULTH_WIDTH_RC_TRIM_CH2 / 10; + PULTH_WIDTH_RC_TRIM_CH3 = PULTH_WIDTH_RC_TRIM_CH3 / 10; + + // Check RC trim + if(PULTH_WIDTH_RC_TRIM_CH2 < 450 || 550 < PULTH_WIDTH_RC_TRIM_CH2){ + //pc.printf("%d\r\n", PULTH_WIDTH_RC_TRIM_CH2); + while(1){ + led2 = 1; + led3 = 0; + wait(0.5); + led2 = 0; + led3 = 1; + wait(0.5); + } + } + + //////////////////// Stand by //////////////////// + + ch2_in.disable_irq(); + ch3_in.disable_irq(); + ch5_in.disable_irq(); + + while(button < 0.5){ + led1 = 1; + led4 = 1; + wait(0.5); + led1 = 0; + led4 = 0; + wait(0.5); + } + + wait(3); + + FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing + + led1 = 1; + led4 = 1; + + while(button < 0.5){ + //while(1){ + + for(int i=0;i<5;i++){ + + //////////////////// Get PWM //////////////////// + ch2_in.enable_irq(); + ch3_in.enable_irq(); + ch5_in.enable_irq(); + + wait(.02); // Interval to get pwm + + ch2_in.disable_irq(); + ch3_in.disable_irq(); + ch5_in.disable_irq(); + + //////////////////// Get pos and vel //////////////////// + if(xbee.readable()){ + // get vz data + int buf_char[SIZE_DATA] = {0}; + + // decode vz data + for(int j=0;j<SIZE_DATA;j++){ buf_char[j] = (int)xbee.getc() - 48; } + + //for(int i=0;i<SIZE_DATA;i++){ pc.printf("%5d ", buf_char[i]); } + //pc.printf("\r\n"); + + pos[1] = 1000*buf_char[0] + 100*buf_char[1] + 10*buf_char[2] + buf_char[3] - 5000; + pos[0] = 1000*buf_char[4] + 100*buf_char[5] + 10*buf_char[6] + buf_char[7] - 5000; + + vel[1] = 1000* buf_char[8] + 100* buf_char[9] + 10*buf_char[10] + buf_char[11] - 5000; + vel[0] = 1000*buf_char[12] + 100*buf_char[13] + 10*buf_char[14] + buf_char[15] - 5000; + }else{ + + // しばらくこなかったらautoにならないフラグとか + + } + + //////////////////// Control law //////////////////// + if(500 < g_pwm_pulse[2]){ + + // Constant velocity F/B + + //pulse_width_ch2_out = 0.001 + 0.000001 * g_pwm_pulse[0]; + pulse_width_ch2_out = 0.001 + 0.000001 * PULTH_WIDTH_RC_TRIM_CH2 + (-1) * 0.000001 * GAIN_VEL_X * (VEL_X_TARGET - vel[0]); + + pulse_width_ch3_out = 0.001 + 0.000001 * g_pwm_pulse[1]; + //pulse_width_ch3_out = 0.001 + PULTH_WIDTH_RC_TRIM_CH3 + GAIN_VEL_Z * (-vel[1]); + + }else{ + + // without RC-hack + pulse_width_ch2_out = 0.001 + 0.000001 * g_pwm_pulse[0]; + pulse_width_ch3_out = 0.001 + 0.000001 * g_pwm_pulse[1]; + + } + + // Limmiter for RC + if(pulse_width_ch2_out < LIMIT_PULTH_WIDTH_CH2_OUT_LOW){ pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_LOW; } + if(LIMIT_PULTH_WIDTH_CH2_OUT_UP < pulse_width_ch2_out) { pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_UP; } + + ch2_out.pulsewidth(pulse_width_ch2_out); + ch3_out.pulsewidth(pulse_width_ch3_out); + + } + + + //for(int i=0;i<N_CHAN;i++){ pc.printf("%5d ", g_pwm_pulse[i]); } + /* + pc.printf(" "); + for(int i=0;i<2;i++){ pc.printf("%5d ", pos[i]); } + for(int i=0;i<2;i++){ pc.printf("%5d ", vel[i]); } + pc.printf("\r\n"); + */ + 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]); + + } + + ch2_out.pulsewidth(0.0015); + ch3_out.pulsewidth(0.0011); + + ch2_in.disable_irq(); + ch3_in.disable_irq(); + ch5_in.disable_irq(); + + fclose(fp); + + led1 = 0; + led4 = 0; + led2 = 1; + led3 = 1; +} + + +//////////////////// RC interrupt (up) //////////////////// +//void roll_up(){ t_roll.start(); } +void pitch_up(){ t_pitch.start(); } +void thr_up(){ t_thr.start(); } +//void yaw_up(){ t_yaw.start(); } +void aux1_up(){ t_aux1.start(); } +//void aux2_up(){ t_aux2.start(); } + + +//////////////////// RC interrupt (down) //////////////////// +/* +void roll_down(){ + t_roll.stop(); + g_pwm_pulse[] = (int)(t_roll.read()*1000000-1000); + t_roll.reset(); +} +*/ +void pitch_down(){ + t_pitch.stop(); + g_pwm_pulse[0] = (int)(t_pitch.read()*1000000-1000); + t_pitch.reset(); +} + +void thr_down(){ + t_thr.stop(); + g_pwm_pulse[1] = (int)(t_thr.read()*1000000-1000); + t_thr.reset(); +} +/* +void yaw_down(){ + t_yaw.stop(); + g_pwm_pulse[] = (int)(t_yaw.read()*1000000-1000); + t_yaw.reset(); +} +*/ +void aux1_down(){ + t_aux1.stop(); + g_pwm_pulse[2] = (int)(t_aux1.read()*1000000-1000); + t_aux1.reset(); +} +/* +void aux2_down(){ + t_aux2.stop(); + g_pwm_pulse[] = (int)(t_aux2.read()*1000000-1000); + t_aux2.reset(); +} +*/
diff -r 000000000000 -r 72e0bf0dbe7c mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Nov 06 10:13:27 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file