void link_ac_sync_enc(){//berger lahr // cmd link_hal_pins("res0.pos", "net0.cmd"); set_hal_pin("res0.enable", 1.0); set_hal_pin("pderiv0.in_lp", 1); set_hal_pin("pderiv0.out_lp", 1); set_hal_pin("pderiv0.vel_max", 13000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pderiv0.acc_max", 13000.0 / 60.0 * 2.0 * M_PI / 0.005); // fb link_hal_pins("enc0.pos", "net0.fb"); set_hal_pin("enc0.res", 4096.0); set_hal_pin("res0.enable", 1.0); set_hal_pin("pderiv1.in_lp", 1); set_hal_pin("pderiv1.out_lp", 1); set_hal_pin("pderiv1.vel_max", 13000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pderiv1.acc_max", 13000.0 / 60.0 * 2.0 * M_PI / 0.005); // res_offset set_hal_pin("ap0.fb_offset_in", 0.0); // pole count set_hal_pin("ap0.pole_count", 3.0); // pid set_hal_pin("pid0.pos_p", 80.0); set_hal_pin("pid0.pos_lp", 1.0); set_hal_pin("pid0.vel_lp", 0.6); set_hal_pin("pid0.vel_max", 13000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pid0.acc_max", 13000.0 / 60.0 * 2.0 * M_PI / 0.005); }
void link_ac_sync_res(){//bosch // cmd link_hal_pins("enc0.pos", "net0.cmd"); set_hal_pin("pderiv0.in_lp", 1); set_hal_pin("pderiv0.out_lp", 1); set_hal_pin("pderiv0.vel_max", 1000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pderiv0.acc_max", 1000.0 / 60.0 * 2.0 * M_PI / 0.005); // fb link_hal_pins("res0.pos", "net0.fb"); set_hal_pin("res0.enable", 1.0); set_hal_pin("pderiv1.in_lp", 1); set_hal_pin("pderiv1.out_lp", 0.2); set_hal_pin("pderiv1.vel_max", 1000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pderiv1.acc_max", 1000.0 / 60.0 * 2.0 * M_PI / 0.005); // res_offset set_hal_pin("ap0.fb_offset_in", -0.64); // pole count set_hal_pin("ap0.pole_count", 4.0); // pid set_hal_pin("pid0.pos_p", 100.0); set_hal_pin("pid0.pos_lp", 1.0); set_hal_pin("pid0.vel_lp", 0.4); set_hal_pin("pid0.cur_lp", 0.5); set_hal_pin("pid0.vel_max", 1000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pid0.acc_max", 1000.0 / 60.0 * 2.0 * M_PI / 0.005); }
int main(void) { float period = 0.0; float lasttime = 0.0; setup(); #include "comps/adc.comp" #include "comps/fault.comp" #include "comps/enc_cmd.comp" #include "comps/enc_fb.comp" //#include "comps/res.comp" #include "comps/encm.comp" #include "comps/sim.comp" #include "comps/rev.comp" #include "comps/rev.comp" #include "comps/cauto.comp" #include "comps/pderiv.comp" #include "comps/pderiv.comp" #include "comps/pid.comp" #include "comps/rev.comp" #include "comps/cur.comp" #include "comps/pwm2uart.comp" #include "comps/absavg.comp" #include "comps/term.comp" #include "comps/led.comp" #include "comps/fan.comp" #include "comps/brake.comp" set_comp_type("net"); HAL_PIN(enable) = 0.0; HAL_PIN(cmd) = 0.0; HAL_PIN(fb) = 0.0; HAL_PIN(fb_error) = 0.0; HAL_PIN(cmd_d) = 0.0; HAL_PIN(fb_d) = 0.0; HAL_PIN(amp) = 0.0; HAL_PIN(vlt) = 0.0; HAL_PIN(tmp) = 0.0; HAL_PIN(rt_calc_time) = 0.0; set_comp_type("conf"); HAL_PIN(r) = 0.0; HAL_PIN(l) = 0.0; HAL_PIN(j) = 0.0; HAL_PIN(km) = 0.0; HAL_PIN(pole_count) = 0.0; HAL_PIN(fb_pole_count) = 0.0; HAL_PIN(fb_offset) = 0.0; HAL_PIN(pos_p) = 0.0; HAL_PIN(acc_p) = 0.0; HAL_PIN(acc_pi) = 0.0; HAL_PIN(cur_lp) = 0.0; HAL_PIN(max_vel) = 0.0; HAL_PIN(max_acc) = 0.0; HAL_PIN(max_force) = 0.0; HAL_PIN(max_cur) = 0.0; HAL_PIN(fb_type) = 0.0; HAL_PIN(cmd_type) = 0.0; HAL_PIN(fb_rev) = 0.0; HAL_PIN(cmd_rev) = 0.0; HAL_PIN(out_rev) = 0.0; HAL_PIN(fb_res) = 1.0; HAL_PIN(cmd_res) = 2000.0; HAL_PIN(sin_offset) = -17600.0; HAL_PIN(cos_offset) = -17661.0; HAL_PIN(sin_gain) = 0.0001515; HAL_PIN(cos_gain) = 0.00015; HAL_PIN(max_volt) = 370.0; HAL_PIN(max_temp) = 100.0; HAL_PIN(max_pos_error) = M_PI / 2.0; HAL_PIN(high_volt) = 350.0; HAL_PIN(low_volt) = 12.0; HAL_PIN(high_temp) = 80.0; HAL_PIN(fan_temp) = 40.0; HAL_PIN(autophase) = 1.0; HAL_PIN(max_sat) = 0.2; g_amp_hal_pin = map_hal_pin("net0.amp"); g_vlt_hal_pin = map_hal_pin("net0.vlt"); g_tmp_hal_pin = map_hal_pin("net0.tmp"); rt_time_hal_pin = map_hal_pin("net0.rt_calc_time"); for(int i = 0; i < hal.init_func_count; i++){ hal.init[i](); } link_pid(); //set_e240(); //set_bergerlahr();//pid2: ok //set_mitsubishi();//pid2: ok //set_festo(); //set_manutec(); set_rexroth();//pid2: ok //link_hal_pins("enc10.ipos", "rev1.in"); //set_hal_pin("res0.reverse", 0.0); //set_bosch1();//pid2: ok //set_bosch4();//pid2: ok //set_sanyo();//pid2: ok //set_br(); set_cmd_enc(); link_hal_pins("conf0.max_cur", "fault0.max_cur"); link_hal_pins("conf0.max_volt", "fault0.max_volt"); link_hal_pins("conf0.max_temp", "fault0.max_temp"); link_hal_pins("conf0.max_pos_error", "fault0.max_pos_error"); link_hal_pins("conf0.high_volt", "fault0.high_volt"); link_hal_pins("conf0.high_temp", "fault0.high_temp"); link_hal_pins("conf0.low_volt", "fault0.low_volt"); link_hal_pins("conf0.fan_temp", "fault0.fan_temp"); link_hal_pins("conf0.autophase", "fault0.phase_on_start"); link_hal_pins("conf0.max_sat", "fault0.max_sat"); set_hal_pin("fault0.reset", 0.0); link_hal_pins("fault0.phase_start", "cauto0.start"); link_hal_pins("cauto0.ready", "fault0.phase_ready"); link_hal_pins("pid0.pos_error", "fault0.pos_error"); link_hal_pins("pid0.saturated", "fault0.sat"); link_hal_pins("net0.vlt", "fault0.volt"); link_hal_pins("net0.tmp", "fault0.temp"); link_hal_pins("net0.amp", "fault0.amp"); link_hal_pins("net0.fb_error", "fault0.fb_error"); link_hal_pins("net0.cmd", "fault0.cmd"); link_hal_pins("rev1.out", "fault0.fb"); link_hal_pins("fault0.start_offset", "cauto0.start_offset"); link_hal_pins("fault0.cur", "pid0.max_cur"); link_hal_pins("fault0.cur", "cur0.cur_max"); link_hal_pins("fault0.brake", "brake0.brake"); link_hal_pins("fault0.fan", "fan0.fan"); link_hal_pins("fault0.enable_out", "pwm2uart0.enable"); link_hal_pins("fault0.enable_pid", "pid0.enable"); link_hal_pins("net0.enable", "fault0.enable"); link_hal_pins("fault0.led_green", "led0.g"); link_hal_pins("fault0.led_red", "led0.r"); link_hal_pins("pid0.pos_error", "avg0.in"); set_hal_pin("avg0.ac", 0.0001); TIM_Cmd(TIM8, ENABLE);//int UB_USB_CDC_Init(); while(1) // Do not exit { Wait(2); period = systime/1000.0 + (1.0 - SysTick->VAL/RCC_Clocks.HCLK_Frequency)/1000.0 - lasttime; lasttime = systime/1000.0 + (1.0 - SysTick->VAL/RCC_Clocks.HCLK_Frequency)/1000.0; for(int i = 0; i < hal.nrt_func_count; i++){ hal.nrt[i](period); } } }
void link_pid(){ // pos link_hal_pins("net0.cmd", "pos_minus0.in0"); link_hal_pins("net0.fb", "pos_minus0.in1"); link_hal_pins("pos_minus0.out", "pid0.pos_error"); link_hal_pins("net0.fb", "ap0.fb_in"); // vel link_hal_pins("net0.cmd", "pderiv0.in"); link_hal_pins("pderiv0.out", "net0.cmd_d"); link_hal_pins("net0.cmd_d", "pid0.vel_ext_cmd"); set_hal_pin("pderiv0.in_lp", 1); set_hal_pin("pderiv0.out_lp", 1); set_hal_pin("pderiv0.vel_max", 1000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pderiv0.acc_max", 1000.0 / 60.0 * 2.0 * M_PI / 0.005); link_hal_pins("net0.fb", "pderiv1.in"); link_hal_pins("pderiv1.out", "net0.fb_d"); link_hal_pins("net0.fb_d", "pid0.vel_fb"); set_hal_pin("pderiv1.in_lp", 1); set_hal_pin("pderiv1.out_lp", 1); set_hal_pin("pderiv1.vel_max", 1000.0 / 60.0 * 2.0 * M_PI); set_hal_pin("pderiv1.acc_max", 1000.0 / 60.0 * 2.0 * M_PI / 0.005); // timer pwm link_hal_pins("pid0.pwm_cmd", "ap0.pwm_in"); link_hal_pins("ap0.pwm_out", "p2uvw0.pwm"); link_hal_pins("p2uvw0.u", "pwmout0.u"); link_hal_pins("p2uvw0.v", "pwmout0.v"); link_hal_pins("p2uvw0.w", "pwmout0.w"); //pwm over uart link_hal_pins("p2uvw0.u", "pwm2uart0.u"); link_hal_pins("p2uvw0.v", "pwm2uart0.v"); link_hal_pins("p2uvw0.w", "pwm2uart0.w"); // magpos link_hal_pins("ap0.mag_pos_out", "p2uvw0.magpos"); // term link_hal_pins("net0.cmd", "term0.wave0"); link_hal_pins("net0.fb", "term0.wave1"); link_hal_pins("net0.cmd_d", "term0.wave2"); link_hal_pins("net0.fb_d", "term0.wave3"); set_hal_pin("term0.gain0", 10.0); set_hal_pin("term0.gain1", 10.0); set_hal_pin("term0.gain2", 1.0); set_hal_pin("term0.gain3", 1.0); // enable link_hal_pins("pid0.enable", "net0.enable"); set_hal_pin("net0.enable", 1.0); // misc set_hal_pin("pwmout0.enable", 0.9); set_hal_pin("pwmout0.volt", 130.0); set_hal_pin("pwmout0.pwm_max", 0.9); set_hal_pin("pwm2uart0.enable", 0.9); set_hal_pin("pwm2uart0.volt", 130.0); set_hal_pin("pwm2uart0.pwm_max", 0.9); set_hal_pin("p2uvw0.volt", 130.0); set_hal_pin("p2uvw0.pwm_max", 0.9); set_hal_pin("pid0.volt", 130.0); set_hal_pin("p2uvw0.poles", 1.0); set_hal_pin("pid0.enable", 1.0); }
int main(void) { float period = 0.0; float lasttime = 0.0; setup(); #include "comps/frt.comp" #include "comps/rt.comp" #include "comps/nrt.comp" #include "comps/pos_minus.comp" #include "comps/pwm2uvw.comp" #include "comps/pwmout.comp" #include "comps/pwm2uart.comp" #include "comps/enc.comp" #include "comps/res.comp" //#include "comps/pid.comp" #include "comps/term.comp" #include "comps/sim.comp" #include "comps/pderiv.comp" #include "comps/pderiv.comp" #include "comps/autophase.comp" #include "comps/vel_observer.comp" set_comp_type("net"); HAL_PIN(enable) = 0.0; HAL_PIN(cmd) = 0.0; HAL_PIN(fb) = 0.0; HAL_PIN(cmd_d) = 0.0; HAL_PIN(fb_d) = 0.0; HAL_PIN(u) = 0.0; HAL_PIN(v) = 0.0; HAL_PIN(w) = 0.0; for(int i = 0; i < hal.init_func_count; i++){ hal.init[i](); } TIM_Cmd(TIM2, ENABLE);//int TIM_Cmd(TIM4, ENABLE);//PWM TIM_Cmd(TIM5, ENABLE);//PID link_pid(); link_ac_sync_res(); set_hal_pin("ap0.start", 1.0); link_hal_pins("sim0.sin", "net0.cmd"); link_hal_pins("net0.cmd", "vel_ob0.pos_in"); link_hal_pins("net0.cmd", "term0.wave0"); link_hal_pins("net0.cmd_d", "term0.wave1"); link_hal_pins("vel_ob0.pos_out", "term0.wave2"); link_hal_pins("vel_ob0.vel_out", "term0.wave3"); link_hal_pins("vel_ob0.trg", "rt0.trg0"); set_hal_pin("term0.gain0", 10.0); set_hal_pin("term0.gain1", 10.0); set_hal_pin("term0.gain2", 10.0); set_hal_pin("term0.gain3", 10.0); set_hal_pin("sim0.amp", 1.0); set_hal_pin("sim0.freq", 0.5); set_hal_pin("vel_ob0.alpha", 1.0); set_hal_pin("vel_ob0.beta", 0.1); link_hal_pins("pwm2uart0.uout", "net0.u"); link_hal_pins("pwm2uart0.vout", "net0.v"); link_hal_pins("pwm2uart0.wout", "net0.w"); data_t data; while(1) // Do not exit { Wait(1); period = time/1000.0 + (1.0 - SysTick->VAL/RCC_Clocks.HCLK_Frequency)/1000.0 - lasttime; lasttime = time/1000.0 + (1.0 - SysTick->VAL/RCC_Clocks.HCLK_Frequency)/1000.0; for(int i = 0; i < hal.nrt_func_count; i++){ hal.nrt[i](period); } data.data[0] = (uint16_t)PIN(u); data.data[1] = (uint16_t)PIN(v); data.data[2] = (uint16_t)PIN(w); while(USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET); USART_SendData(USART3, 0x155); for(int i = 0; i < DATALENGTH * 2; i++){ while(USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET); USART_SendData(USART3, data.byte[i]); } } }