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);
}
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);
}
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]);
}
}
}
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);
}
