From 3d0ed004f29a81ec2a25ba455737569c6d64b04a Mon Sep 17 00:00:00 2001 From: crinq Date: Tue, 31 Mar 2015 19:54:22 +0200 Subject: [PATCH] +conf --- Makefile | 6 +- graphviz/test.hal | 390 +++++++++++++++++++++++++++++-------------- src/comps/cauto.comp | 5 +- src/comps/cur.comp | 49 +++--- src/comps/mux.comp | 79 +++++++++ src/comps/pid2.comp | 14 +- src/comps/rev.comp | 17 ++ src/comps/tune.comp | 86 ++++++++++ src/conf.h | 57 +++++++ src/hal.h | 4 +- src/link.c | 144 ++++++++++++++++ src/link.h | 3 + src/main.c | 234 ++++++++------------------ 13 files changed, 763 insertions(+), 325 deletions(-) create mode 100644 src/comps/mux.comp create mode 100644 src/comps/rev.comp create mode 100644 src/comps/tune.comp create mode 100644 src/conf.h create mode 100644 src/link.c create mode 100644 src/link.h diff --git a/Makefile b/Makefile index ffa7b7dd..e202a614 100644 --- a/Makefile +++ b/Makefile @@ -4,7 +4,7 @@ #ld gcollection #komische flags -SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c printf.c scanf.c setup.c hal.c misc.c +SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c printf.c scanf.c setup.c hal.c misc.c link.c #USB SRCS += ub_lib/stm32_ub_usb_cdc.c ub_lib/usb_cdc_lolevel/usb_core.c ub_lib/usb_cdc_lolevel/usb_dcd_int.c ub_lib/usb_cdc_lolevel/usbd_req.c ub_lib/usb_cdc_lolevel/usbd_cdc_core.c ub_lib/usb_cdc_lolevel/usbd_core.c ub_lib/usb_cdc_lolevel/usb_dcd.c ub_lib/usb_cdc_lolevel/usbd_cdc_vcp.c ub_lib/usb_cdc_lolevel/usbd_desc.c ub_lib/usb_cdc_lolevel/usbd_ioreq.c ub_lib/usb_cdc_lolevel/usb_bsp.c ub_lib/usb_cdc_lolevel/usbd_usr.c #SRCS = main.c system.c @@ -60,8 +60,8 @@ again: clean all burn: main.elf st-flash --reset write $(PROJ_NAME).bin 0x8000000 -btburn: main.bin - printf "bootloader\r" > /dev/cu.usbmodem14611 || true +btburn: main.elf + printf "bootloader\r" > /dev/cu.usbmodem1411 || true sleep 1 dfu-util -a 0 -d 0483:df11 -s 0x08000000:leave -D main.bin diff --git a/graphviz/test.hal b/graphviz/test.hal index 83bb4771..6389262a 100644 --- a/graphviz/test.hal +++ b/graphviz/test.hal @@ -1,137 +1,281 @@ -pos_minus0.in0 <= net0.cmd = 0.000000 -pos_minus0.in1 <= net0.fb = 0.000000 -pos_minus0.out <= pos_minus0.out = 0.000000 -p2uvw0.pwm <= cauto0.pwm_out = 0.000000 -p2uvw0.magpos <= cauto0.mag_pos_out = 1.570796 -p2uvw0.volt <= p2uvw0.volt = 130.000000 -p2uvw0.poles <= p2uvw0.poles = 1.000000 -p2uvw0.pwm_max <= p2uvw0.pwm_max = 0.900000 -p2uvw0.u <= p2uvw0.u = 65.000000 -p2uvw0.v <= p2uvw0.v = 65.000000 -p2uvw0.w <= p2uvw0.w = 65.000000 -p2uvw0.abpwm <= p2uvw0.abpwm = 0.000000 -pwm2uart0.u <= p2uvw0.u = 65.000000 -pwm2uart0.v <= p2uvw0.v = 65.000000 -pwm2uart0.w <= p2uvw0.w = 65.000000 +enc0.res0 <= enc0.res0 = 2048.000000 +enc0.pos0 <= enc0.pos0 = 0.000000 +enc0.reverse0 <= enc0.reverse0 = 0.000000 +enc0.res1 <= enc0.res1 = 2048.000000 +enc0.pos1 <= enc0.pos1 = 0.785398 +enc0.reverse1 <= enc0.reverse1 = 0.000000 +enc0.ireverse1 <= enc0.ireverse1 = 0.000000 +enc0.ipos1 <= enc0.ipos1 = 0.050061 +enc0.iposd1 <= enc0.iposd1 = -0.286610 +enc0.s <= enc0.s = -15790.000047 +enc0.c <= enc0.c = -4656.000007 +res0.pos <= res0.pos = -1.840562 +res0.amp <= res0.amp = 234852000.777777 +res0.s <= res0.s = -14774.898627 +res0.c <= res0.c = -4094.418027 +res0.reverse <= res0.reverse = 0.000000 +res0.enable <= res0.enable = 0.000000 +res0.error <= res0.error = 0.000000 +encm0.reverse <= encm0.reverse = 0.000000 +encm0.pos <= encm0.pos = 0.000000 +encm0.error <= encm0.error = 117631.000677 +sim0.amp <= sim0.amp = 0.100000 +sim0.freq <= sim0.freq = 0.159154 +sim0.sin <= sim0.sin = -0.056990 +sim0.sin2 <= sim0.sin2 = -0.057010 +sim0.phase <= sim0.phase = 0.000000 +sim0.cos <= sim0.cos = -0.082110 +sim0.offset <= sim0.offset = 0.000000 +sim0.msin <= sim0.msin = -0.057120 +sim0.mcos <= sim0.mcos = -0.082050 +sim0.square <= sim0.square = -0.099999 +sim0.msquare <= sim0.msquare = -0.099999 +sim0.vel <= sim0.vel = -2.532305 +sim0.res <= sim0.res = 100000.000077 +mux0.in0 <= mux0.in0 = 0.000000 +mux0.in1 <= enc0.pos0 = 0.000000 +mux0.in2 <= enc0.pos1 = -0.760854 +mux0.in3 <= res0.pos = -1.841197 +mux0.in4 <= mux0.in4 = 0.000000 +mux0.in5 <= encm0.pos = 0.000000 +mux0.in6 <= enc0.ipos1 = -1.741628 +mux0.in7 <= enc0.pos0 = 0.000000 +mux0.in8 <= enc0.pos1 = -2.233476 +mux0.in9 <= sim0.sin = -0.057510 +mux0.in10 <= sim0.vel = -2.528485 +mux0.in11 <= sim0.square = -0.099999 +mux0.in12 <= mux0.in12 = 0.000000 +mux0.in13 <= mux0.in13 = 0.000000 +mux0.in14 <= mux0.in14 = 0.000000 +mux0.in15 <= mux0.in15 = 0.000000 +mux0.out <= mux0.out = 0.000000 +mux0.select <= conf0.ccmd = 1.000000 +mux1.in0 <= mux1.in0 = 0.000000 +mux1.in1 <= enc0.pos0 = 0.000000 +mux1.in2 <= enc0.pos1 = 1.104466 +mux1.in3 <= res0.pos = -1.841310 +mux1.in4 <= mux1.in4 = 0.000000 +mux1.in5 <= encm0.pos = 0.000000 +mux1.in6 <= enc0.ipos1 = 0.123692 +mux1.in7 <= enc0.pos0 = 0.000000 +mux1.in8 <= enc0.pos1 = -0.368155 +mux1.in9 <= sim0.sin = -0.057980 +mux1.in10 <= sim0.vel = -2.522745 +mux1.in11 <= sim0.square = -0.099999 +mux1.in12 <= mux1.in12 = 0.000000 +mux1.in13 <= mux1.in13 = 0.000000 +mux1.in14 <= mux1.in14 = 0.000000 +mux1.in15 <= mux1.in15 = 0.000000 +mux1.out <= mux1.out = -1.841108 +mux1.select <= conf0.cfb = 3.000000 +rev0.in <= mux0.out = 0.000000 +rev0.out <= rev0.out = 0.000000 +rev0.rev <= conf0.cmd_rev = 0.000000 +rev1.in <= mux1.out = -1.841378 +rev1.out <= rev1.out = 1.841284 +rev1.rev <= conf0.fb_rev = 1.000000 +cauto0.i_q <= cauto0.i_q = 0.000000 +cauto0.i_d <= cauto0.i_d = 0.000000 +cauto0.magpos <= cauto0.magpos = -0.002320 +cauto0.start <= cauto0.start = 1.000000 +cauto0.ready <= cauto0.ready = 1.000000 +cauto0.start_offset <= cauto0.start_offset = 0.000000 +cauto0.offset <= cauto0.offset = 1.841564 +cauto0.fb_in <= rev1.out = 1.841200 +cauto0.fb_out <= cauto0.fb_out = -0.000545 +cauto0.pole_count <= conf0.pole_count = 4.000000 +cauto0.cur <= cauto0.cur = 1.000000 +cauto0.time <= cauto0.time = 0.500000 +pderiv0.in <= net0.cmd = 0.000000 +pderiv0.in_lp <= pderiv0.in_lp = 1.000000 +pderiv0.out <= pderiv0.out = 0.000000 +pderiv0.out_lp <= pderiv0.out_lp = 1.000000 +pderiv0.vel_max <= conf0.max_vel = 104.719752 +pderiv0.acc_max <= conf0.max_acc = 0.000000 +pderiv0.res <= pderiv0.res = 100000.000077 +pderiv1.in <= net0.fb = -0.000428 +pderiv1.in_lp <= pderiv1.in_lp = 1.000000 +pderiv1.out <= pderiv1.out = 1.650000 +pderiv1.out_lp <= pderiv1.out_lp = 1.000000 +pderiv1.vel_max <= conf0.max_vel = 104.719752 +pderiv1.acc_max <= conf0.max_acc = 0.000000 +pderiv1.res <= pderiv1.res = 100000.000077 +pid0.pos_ext_cmd <= net0.cmd = 0.000000 +pid0.pos_fb <= net0.fb = -0.000310 +pid0.pos_error <= pid0.pos_error = 0.000725 +pid0.vel_ext_cmd <= net0.cmd_d = 0.000000 +pid0.vel_fb <= net0.fb_d = 0.149999 +pid0.vel_cmd <= pid0.vel_cmd = 0.030233 +pid0.vel_error <= pid0.vel_error = -1.482322 +pid0.acc_ext_cmd <= pid0.acc_ext_cmd = 0.000000 +pid0.acc_cmd <= pid0.acc_cmd = 0.000000 +pid0.force_ext_cmd <= pid0.force_ext_cmd = 0.000000 +pid0.force_cmd <= pid0.force_cmd = -0.000936 +pid0.cur_ext_cmd <= cauto0.i_q = 0.000000 +pid0.cur_cmd <= pid0.cur_cmd = 0.032537 +pid0.enable <= cauto0.ready = 1.000000 +pid0.pos_en <= pid0.pos_en = 1.000000 +pid0.vel_en <= pid0.vel_en = 1.000000 +pid0.acc_en <= pid0.acc_en = 1.000000 +pid0.force_en <= cauto0.ready = 1.000000 +pid0.cur_en <= pid0.cur_en = 1.000000 +pid0.time <= pid0.time = 0.000199 +pid0.mot_r <= conf0.r = 15.000000 +pid0.mot_j <= conf0.j = 0.000141 +pid0.mot_km <= conf0.km = 0.272700 +pid0.mot_fr <= pid0.mot_fr = 30.000000 +pid0.mot_fd <= pid0.mot_fd = 30.000000 +pid0.mot_fl <= pid0.mot_fl = 30.000000 +pid0.pos_p <= conf0.pos_p = 60.000000 +pid0.pos_lp <= pid0.pos_lp = 4000.000007 +pid0.vel_p <= pid0.vel_p = 1.000000 +pid0.vel_lp <= pid0.vel_lp = 4000.000007 +pid0.vel_ff <= pid0.vel_ff = 1.000000 +pid0.vel_fb_lp <= pid0.vel_fb_lp = 4000.000007 +pid0.acc_p <= conf0.acc_p = 0.150000 +pid0.acc_lp <= pid0.acc_lp = 4000.000007 +pid0.acc_pi <= conf0.acc_pi = 100.000000 +pid0.acc_ff <= pid0.acc_ff = 1.000000 +pid0.force_p <= pid0.force_p = 1.000000 +pid0.force_lp <= pid0.force_lp = 4000.000007 +pid0.force_ff <= pid0.force_ff = 1.000000 +pid0.cur_p <= pid0.cur_p = 1.000000 +pid0.cur_ff <= pid0.cur_ff = 1.000000 +pid0.volt <= net0.vlt = 0.417564 +pid0.induction <= pid0.induction = 0.627210 +pid0.vel_limit <= conf0.max_vel = 104.719752 +pid0.acc_limit <= conf0.max_acc = 0.000000 +pid0.force_limit <= conf0.max_force = 1.500000 +pid0.cur_limit <= conf0.max_cur = 5.300000 +pid0.volt_limit <= pid0.volt_limit = 400.000000 +pid0.pwm_limit <= pid0.pwm_limit = 0.950000 +pid0.max_vel <= conf0.max_vel = 104.719752 +pid0.max_acc <= conf0.max_acc = 0.000000 +pid0.max_force <= conf0.max_force = 1.500000 +pid0.max_cur <= conf0.max_cur = 5.300000 +pid0.max_volt <= pid0.max_volt = 400.000000 +pid0.max_pwm <= pid0.max_pwm = 0.950000 +pid0.vel_min <= pid0.vel_min = -1.531221 +pid0.vel_max <= pid0.vel_max = 1.531221 +pid0.acc_min <= pid0.acc_min = 0.000000 +pid0.acc_max <= pid0.acc_max = 0.000000 +pid0.force_min <= pid0.force_min = -0.010953 +pid0.force_max <= pid0.force_max = 0.021860 +pid0.cur_min <= pid0.cur_min = -0.025621 +pid0.cur_max <= pid0.cur_max = 0.020167 +pid0.volt_min <= pid0.volt_min = 0.000000 +pid0.volt_max <= pid0.volt_max = 0.000000 +pid0.pwm_min <= pid0.pwm_min = 0.000000 +pid0.pwm_max <= pid0.pwm_max = 0.000000 +pid0.vel_sat <= pid0.vel_sat = 0.000000 +pid0.acc_sat <= pid0.acc_sat = -1.000000 +pid0.force_sat <= pid0.force_sat = 1.000000 +pid0.cur_sat <= pid0.cur_sat = 0.000000 +pid0.volt_sat <= pid0.volt_sat = 0.000000 +pid0.pwm_sat <= pid0.pwm_sat = 0.000000 +pid0.saturated <= pid0.saturated = 23.352348 +cur0.id <= cauto0.i_d = 0.000000 +cur0.iq <= pid0.cur_cmd = -0.014374 +cur0.ia <= cur0.ia = 0.000060 +cur0.ib <= cur0.ib = -0.043959 +cur0.iu <= cur0.iu = 0.000031 +cur0.iv <= cur0.iv = -0.018712 +cur0.iw <= cur0.iw = 0.000283 +cur0.i_u <= cur0.i_u = 0.000480 +cur0.i_v <= cur0.i_v = 0.218724 +cur0.i_w <= cur0.i_w = 0.053187 +cur0.u <= cur0.u = 0.209996 +cur0.v <= cur0.v = 0.407125 +cur0.w <= cur0.w = 0.010439 +cur0.magpos <= cauto0.magpos = -0.002319 +cur0.r <= conf0.r = 15.000000 +cur0.l <= conf0.l = 0.002000 +cur0.freq <= cur0.freq = 5000.000007 +cur0.volt <= net0.vlt = 0.417564 +cur0.induction <= pid0.induction = -0.818100 +cur0.ind_p <= cur0.ind_p = -1.000000 +cur0.pwm_max <= cur0.pwm_max = 0.950000 +cur0.cur_max <= conf0.max_cur = 5.300000 +cur0.p <= cur0.p = 1.500000 +cur0.ff <= cur0.ff = 1.000000 +cur0.lp <= conf0.cur_lp = 0.300000 +pwm2uart0.u <= cur0.u = 0.155425 +pwm2uart0.v <= cur0.v = 0.016980 +pwm2uart0.w <= cur0.w = 0.010439 pwm2uart0.uout <= pwm2uart0.uout = 0.000000 pwm2uart0.vout <= pwm2uart0.vout = 0.000000 pwm2uart0.wout <= pwm2uart0.wout = 0.000000 pwm2uart0.enable <= pwm2uart0.enable = 0.900000 -pwm2uart0.volt <= pwm2uart0.volt = 130.000000 +pwm2uart0.volt <= net0.vlt = 0.313173 pwm2uart0.pwm_max <= pwm2uart0.pwm_max = 0.900000 -enc0.res0 <= enc0.res0 = 4096.000007 -enc0.pos0 <= enc0.pos0 = 0.000000 -enc0.reverse0 <= enc0.reverse0 = 0.000000 -enc0.res1 <= enc0.res1 = 4096.000007 -enc0.pos1 <= enc0.pos1 = 0.000000 -enc0.reverse1 <= enc0.reverse1 = 0.000000 -enc0.ireverse1 <= enc0.ireverse1 = 0.000000 -enc0.ipos1 <= enc0.ipos1 = 0.000059 -enc0.iposd1 <= enc0.iposd1 = -0.780703 -enc0.s <= enc0.s = -105.000000 -enc0.c <= enc0.c = 111.000000 -res0.pos <= res0.pos = 0.721233 -res0.amp <= res0.amp = 5.584125 -res0.s <= res0.s = -0.675781 -res0.c <= res0.c = 0.652343 -res0.reverse <= res0.reverse = 0.000000 -res0.enable <= res0.enable = 1.000000 -res0.error <= res0.error = 1.000000 -pid0.pos_error <= pos_minus0.out = 0.000000 -pid0.vel_cmd <= pid0.vel_cmd = 0.000000 -pid0.vel_ext_cmd <= net0.cmd_d = 0.000000 -pid0.vel_fb <= net0.fb_d = 0.000000 -pid0.vel_error <= pid0.vel_error = 0.000000 -pid0.acc_cmd <= pid0.acc_cmd = 0.000000 -pid0.acc_ext_cmd <= pid0.acc_ext_cmd = 0.000000 -pid0.force_cmd <= pid0.force_cmd = 0.000000 -pid0.force_ext_cmd <= pid0.force_ext_cmd = 0.000000 -pid0.cur_cmd <= pid0.cur_cmd = 0.000000 -pid0.cur_ext_cmd <= pid0.cur_ext_cmd = 0.000000 -pid0.cur_fb <= pid0.cur_fb = 0.000000 -pid0.cur_error <= pid0.cur_error = 0.000000 -pid0.volt_cmd <= pid0.volt_cmd = 0.000000 -pid0.pwm_cmd <= pid0.pwm_cmd = 0.000000 -pid0.enable <= cauto0.ready = 1.000000 -pid0.pos_en <= pid0.pos_en = 1.000000 -pid0.pos_p <= pid0.pos_p = 35.000000 -pid0.pos_lp <= pid0.pos_lp = 1.000000 -pid0.vel_en <= pid0.vel_en = 1.000000 -pid0.vel_p <= pid0.vel_p = 1.000000 -pid0.vel_i <= pid0.vel_i = 50.000000 -pid0.vel_d <= pid0.vel_d = 0.000000 -pid0.vel_ff <= pid0.vel_ff = 1.000000 -pid0.vel_lp <= pid0.vel_lp = 1.000000 -pid0.acc_en <= pid0.acc_en = 1.000000 -pid0.acc_p <= pid0.acc_p = 0.100000 -pid0.acc_ff <= pid0.acc_ff = 0.000000 -pid0.force_en <= pid0.force_en = 1.000000 -pid0.force_p <= pid0.force_p = 1.000000 -pid0.cur_p <= pid0.cur_p = 0.000000 -pid0.cur_ff <= pid0.cur_ff = 28.000000 -pid0.cur_d <= pid0.cur_d = 0.000000 -pid0.cur_ind <= pid0.cur_ind = 2.340000 -pid0.cur_lp <= pid0.cur_lp = 1.000000 -pid0.volt <= pid0.volt = 60.000000 -pid0.vel_max <= pid0.vel_max = 1361.356968 -pid0.vel_e_max <= pid0.vel_e_max = 1.250000 -pid0.acc_max <= pid0.acc_max = 272271.402777 -pid0.force_max <= pid0.force_max = 100.000000 -pid0.cur_max <= pid0.cur_max = 6.000000 -pid0.volt_max <= pid0.volt_max = 130.000000 -pid0.pwm_max <= pid0.pwm_max = 0.900000 -pid0.saturated <= pid0.saturated = 0.000000 -term0.wave0 <= enc0.pos1 = 0.000000 -term0.gain0 <= term0.gain0 = 10000.000007 +term0.wave0 <= net0.cmd = 0.000000 +term0.gain0 <= term0.gain0 = 10.000000 term0.offset0 <= term0.offset0 = 0.000000 -term0.wave1 <= enc0.ipos1 = 0.000046 -term0.gain1 <= term0.gain1 = 10000.000007 +term0.wave1 <= net0.fb = -0.000303 +term0.gain1 <= term0.gain1 = 10.000000 term0.offset1 <= term0.offset1 = 0.000000 -term0.wave2 <= enc0.iposd1 = 0.728854 -term0.gain2 <= term0.gain2 = 10.000000 +term0.wave2 <= net0.cmd_d = 0.000000 +term0.gain2 <= term0.gain2 = 1.000000 term0.offset2 <= term0.offset2 = 0.000000 -term0.wave3 <= net0.fb_d = 0.000000 +term0.wave3 <= net0.fb_d = 0.249999 term0.gain3 <= term0.gain3 = 1.000000 term0.offset3 <= term0.offset3 = 0.000000 -term0.send_div <= term0.send_div = 10.000000 -term0.buf_use <= term0.buf_use = 1.000000 -sim0.amp <= sim0.amp = 0.100000 -sim0.freq <= sim0.freq = 0.159154 -sim0.sin <= sim0.sin = 0.099500 -sim0.cos <= sim0.cos = 0.009500 -sim0.square <= sim0.square = 0.100000 -sim0.vel <= sim0.vel = 1.472000 -sim0.res <= sim0.res = 2000.000000 -pderiv0.in <= net0.cmd = 0.000000 -pderiv0.in_lp <= pderiv0.in_lp = 1.000000 -pderiv0.out <= pderiv0.out = 0.000000 -pderiv0.out_lp <= pderiv0.out_lp = 0.500000 -pderiv0.vel_max <= pderiv0.vel_max = 1361.356968 -pderiv0.acc_max <= pderiv0.acc_max = 272271.402777 -pderiv0.res <= pderiv0.res = 100000.000077 -pderiv1.in <= net0.fb = 0.000000 -pderiv1.in_lp <= pderiv1.in_lp = 1.000000 -pderiv1.out <= pderiv1.out = 0.000000 -pderiv1.out_lp <= pderiv1.out_lp = 1.000000 -pderiv1.vel_max <= pderiv1.vel_max = 1361.356968 -pderiv1.acc_max <= pderiv1.acc_max = 272271.402777 -pderiv1.res <= pderiv1.res = 100000.000077 -cauto0.pwm_in <= pid0.pwm_cmd = 0.000000 -cauto0.pwm_out <= cauto0.pwm_out = 0.000000 -cauto0.mag_pos_out <= cauto0.mag_pos_out = 1.570796 -cauto0.start <= cauto0.start = 1.000000 -cauto0.ready <= cauto0.ready = 1.000000 -cauto0.offset <= cauto0.offset = 0.000000 -cauto0.fb_in <= enc0.pos1 = 0.000000 -cauto0.fb_out <= cauto0.fb_out = 0.000000 -cauto0.pole_count <= cauto0.pole_count = 3.000000 -cauto0.scale <= cauto0.scale = 0.600000 -cauto0.time <= cauto0.time = 0.500000 +term0.wave4 <= term0.wave4 = 0.000000 +term0.gain4 <= term0.gain4 = 10.000000 +term0.offset4 <= term0.offset4 = 0.000000 +term0.wave5 <= term0.wave5 = 0.000000 +term0.gain5 <= term0.gain5 = 10.000000 +term0.offset5 <= term0.offset5 = 0.000000 +term0.wave6 <= term0.wave6 = 0.000000 +term0.gain6 <= term0.gain6 = 10.000000 +term0.offset6 <= term0.offset6 = 0.000000 +term0.wave7 <= term0.wave7 = 0.000000 +term0.gain7 <= term0.gain7 = 10.000000 +term0.offset7 <= term0.offset7 = 0.000000 +term0.send_step <= term0.send_step = 10.000000 +term0.buf_use <= term0.buf_use = 2.000000 led0.r <= cauto0.start = 1.000000 led0.g <= cauto0.ready = 1.000000 -led0.y <= led0.y = 0.000000 +fan0.fan <= fan0.fan = 0.000000 +fan0.tacho <= fan0.tacho = 1.000000 +brake0.brake <= brake0.brake = 0.000000 +tune0.iq <= tune0.iq = 0.000000 +tune0.id <= tune0.id = 0.000000 +tune0.lp <= tune0.lp = 0.000000 +tune0.r <= tune0.r = 1.000000 +tune0.amp <= tune0.amp = 0.000000 +tune0.start <= tune0.start = 0.000000 +tune0.ready <= tune0.ready = 1.000000 +tune0.start_r <= tune0.start_r = 1.000000 +tune0.re <= tune0.re = 2147483647.777777 +tune0.l <= tune0.l = 0.000000 +tune0.time <= tune0.time = 0.100000 +tune0.freq <= tune0.freq = 1.000000 +tune0.cur <= tune0.cur = 1.000000 net0.enable <= net0.enable = 1.000000 -net0.cmd <= enc0.pos0 = 0.000000 -net0.fb <= cauto0.fb_out = 0.000000 +net0.cmd <= rev0.out = 0.000000 +net0.fb <= cauto0.fb_out = -0.000568 net0.cmd_d <= pderiv0.out = 0.000000 -net0.fb_d <= pderiv1.out = 0.000000 +net0.fb_d <= pderiv1.out = 2.949999 +net0.amp <= net0.amp = 0.071397 +net0.vlt <= net0.vlt = 0.417564 +net0.tmp <= net0.tmp = 27.387664 +conf0.r <= conf0.r = 15.000000 +conf0.l <= conf0.l = 0.002000 +conf0.j <= conf0.j = 0.000141 +conf0.km <= conf0.km = 0.272700 +conf0.pole_count <= conf0.pole_count = 4.000000 +conf0.fb_pole_count <= conf0.fb_pole_count = 0.000000 +conf0.fb_offset <= conf0.fb_offset = 0.000000 +conf0.pos_p <= conf0.pos_p = 60.000000 +conf0.acc_p <= conf0.acc_p = 0.150000 +conf0.acc_pi <= conf0.acc_pi = 100.000000 +conf0.cur_lp <= conf0.cur_lp = 0.300000 +conf0.max_vel <= conf0.max_vel = 104.719752 +conf0.max_acc <= conf0.max_acc = 0.000000 +conf0.max_force <= conf0.max_force = 1.500000 +conf0.max_cur <= conf0.max_cur = 5.300000 +conf0.cfb <= conf0.cfb = 3.000000 +conf0.ccmd <= conf0.ccmd = 1.000000 +conf0.fb_rev <= conf0.fb_rev = 1.000000 +conf0.cmd_rev <= conf0.cmd_rev = 0.000000 diff --git a/src/comps/cauto.comp b/src/comps/cauto.comp index 6598b969..37c9375d 100644 --- a/src/comps/cauto.comp +++ b/src/comps/cauto.comp @@ -1,6 +1,5 @@ COMP(cauto); -HAL_PIN(i_in) = 0.0; HAL_PIN(i_q) = 0.0; HAL_PIN(i_d) = 0.0; @@ -36,7 +35,7 @@ RT_CALC( HT( GOTO(0); STATE(0){ - iq = PIN(i_in); + iq = 0.0; id = 0.0; mpos = mod(minus(fbi, offset_) * PIN(pole_count)); if(PIN(start) > 0.0){ @@ -64,7 +63,7 @@ RT_CALC( } STATE(3){ PIN(ready) = 1.0; - iq = PIN(i_in); + iq = 0.0; id = 0.0; mpos = mod(minus(fbi, offset_) * PIN(pole_count)); if(PIN(start) == 0.0){ diff --git a/src/comps/cur.comp b/src/comps/cur.comp index 11943d9f..4fb773ca 100644 --- a/src/comps/cur.comp +++ b/src/comps/cur.comp @@ -10,6 +10,10 @@ HAL_PIN(iu) = 0.0; HAL_PIN(iv) = 0.0; HAL_PIN(iw) = 0.0; +HAL_PIN(i_u) = 0.0; +HAL_PIN(i_v) = 0.0; +HAL_PIN(i_w) = 0.0; + HAL_PIN(u) = 0.0; HAL_PIN(v) = 0.0; HAL_PIN(w) = 0.0; @@ -31,13 +35,13 @@ HAL_PIN(p) = 0.0; HAL_PIN(ff) = 1.0; HAL_PIN(lp) = 1.0; -MEM(float iuv_old) = 0.0; -MEM(float ivw_old) = 0.0; -MEM(float iwu_old) = 0.0; +MEM(float iu_old) = 0.0; +MEM(float iv_old) = 0.0; +MEM(float iw_old) = 0.0; RT_CALC( - float r_ = MAX(PIN(r) * 3.0 / 2.0, 0.1); - float l_ = MAX(PIN(l) * 3.0 / 2.0, 0.0005); + float r_ = MAX(PIN(r) / 2.0, 0.1); + float l_ = MAX(PIN(l) / 2.0, 0.001); float freq_ = MAX(PIN(freq), 1); float vlt = MAX(PIN(volt), 0.1); //float ib_ = PIN(i);// + PIN(induction) / r_; @@ -62,10 +66,6 @@ RT_CALC( float iv_ = - ia_ / 2.0 + ib_ / 2.0 * s3; float iw_ = - ia_ / 2.0 - ib_ / 2.0 * s3; - float iuv_ = iv_ - iu_; - float ivw_ = iw_ - iv_; - float iwu_ = iu_ - iw_; - float ud_ = 0.0; float uq_ = ind; @@ -77,10 +77,6 @@ RT_CALC( float uv_ = - ua_ / 2.0 + ub_ / 2.0 * s3; float uw_ = - ua_ / 2.0 - ub_ / 2.0 * s3; - float uuv_ = uv_ - uu_; - float uvw_ = uw_ - uv_; - float uwu_ = uu_ - uw_; - float ff_ = PIN(ff); float p_ = l_ * freq_ * PIN(lp); @@ -88,20 +84,15 @@ RT_CALC( float pwm_max_ = PIN(pwm_max); float vmax = vlt * pwm_max_; - float _uv = CLAMP(ff_ * r_ * iuv_ + p_ * (iuv_ - iuv_old) + uuv_ * ind_p_, -vmax , vmax); - float _vw = CLAMP(ff_ * r_ * ivw_ + p_ * (ivw_ - ivw_old) + uvw_ * ind_p_, -vmax , vmax); - float _wu = CLAMP(ff_ * r_ * iwu_ + p_ * (iwu_ - iwu_old) + uwu_ * ind_p_, -vmax , vmax); + float _u = CLAMP(ff_ * r_ * iu_ + p_ * (iu_ - iu_old) + uu_ * ind_p_, -vmax / 2.0, vmax / 2.0); + float _v = CLAMP(ff_ * r_ * iv_ + p_ * (iv_ - iv_old) + uv_ * ind_p_, -vmax / 2.0, vmax / 2.0); + float _w = CLAMP(ff_ * r_ * iw_ + p_ * (iw_ - iw_old) + uw_ * ind_p_, -vmax / 2.0, vmax / 2.0); - iuv_old += (_uv - iuv_old * r_ - uuv_) / l_ / freq_; - ivw_old += (_vw - ivw_old * r_ - uvw_) / l_ / freq_; - iwu_old += (_wu - iwu_old * r_ - uwu_) / l_ / freq_; + iu_old += (_u - iu_old * r_ - uu_) / l_ / freq_; + iv_old += (_v - iv_old * r_ - uv_) / l_ / freq_; + iw_old += (_w - iw_old * r_ - uw_) / l_ / freq_; - - float _u = 0.0; - float _v = 0.0; - float _w = 0.0; - - if(ABS(_uv) > ABS(_vw)){ +/* if(ABS(_uv) > ABS(_vw)){ if(ABS(_uv) > ABS(_wu)){ // _uv _u = (vlt - _uv) / 2.0; _v = (vlt + _uv) / 2.0; @@ -125,7 +116,11 @@ RT_CALC( _v = _u + _uv; } } +*/ + _u += vlt / 2.0; + _v += vlt / 2.0; + _w += vlt / 2.0; PIN(u) = _u; PIN(v) = _v; @@ -138,6 +133,10 @@ RT_CALC( PIN(iv) = iv_; PIN(iw) = iw_; + PIN(i_u) = iu_old; + PIN(i_v) = iv_old; + PIN(i_w) = iw_old; + PIN(p) = p_; ); diff --git a/src/comps/mux.comp b/src/comps/mux.comp new file mode 100644 index 00000000..f57ca5bf --- /dev/null +++ b/src/comps/mux.comp @@ -0,0 +1,79 @@ +COMP(mux); + +HAL_PIN(in0) = 0.0; +HAL_PIN(in1) = 0.0; +HAL_PIN(in2) = 0.0; +HAL_PIN(in3) = 0.0; +HAL_PIN(in4) = 0.0; +HAL_PIN(in5) = 0.0; +HAL_PIN(in6) = 0.0; +HAL_PIN(in7) = 0.0; +HAL_PIN(in8) = 0.0; +HAL_PIN(in9) = 0.0; +HAL_PIN(in10) = 0.0; +HAL_PIN(in11) = 0.0; +HAL_PIN(in12) = 0.0; +HAL_PIN(in13) = 0.0; +HAL_PIN(in14) = 0.0; +HAL_PIN(in15) = 0.0; + +HAL_PIN(out) = 0.0; + +HAL_PIN(select) = 0.0; + +RT_CALC( + switch((int)PIN(select)){ + case 0: + PIN(out) = PIN(in0); + break; + case 1: + PIN(out) = PIN(in1); + break; + case 2: + PIN(out) = PIN(in2); + break; + case 3: + PIN(out) = PIN(in3); + break; + case 4: + PIN(out) = PIN(in4); + break; + case 5: + PIN(out) = PIN(in5); + break; + case 6: + PIN(out) = PIN(in6); + break; + case 7: + PIN(out) = PIN(in7); + break; + case 8: + PIN(out) = PIN(in8); + break; + case 9: + PIN(out) = PIN(in9); + break; + case 10: + PIN(out) = PIN(in10); + break; + case 11: + PIN(out) = PIN(in11); + break; + case 12: + PIN(out) = PIN(in12); + break; + case 13: + PIN(out) = PIN(in13); + break; + case 14: + PIN(out) = PIN(in14); + break; + case 15: + PIN(out) = PIN(in15); + break; + default: + PIN(out) = 0.0; + } +); + +ENDCOMP; diff --git a/src/comps/pid2.comp b/src/comps/pid2.comp index 829094c2..c246b5a4 100644 --- a/src/comps/pid2.comp +++ b/src/comps/pid2.comp @@ -159,7 +159,7 @@ float curextcmd = PIN(cur_ext_cmd); //float curerr = 0.0; float cursat; -float r = PIN(mot_r); +float r = PIN(mot_r) / 2.0; if(r == 0.0){ r = 1.0; } @@ -230,16 +230,28 @@ if(PIN(enable) > 0.0){ posp = 0.0; } if(PIN(vel_en) == 0.0){ + posp = 0.0; velp = 0.0; } if(PIN(acc_en) == 0.0){ + posp = 0.0; + velp = 0.0; accp = 0.0; force_error_sum = 0.0; } if(PIN(force_en) == 0.0){ + posp = 0.0; + velp = 0.0; + accp = 0.0; + force_error_sum = 0.0; forcep = 0.0; } if(PIN(cur_en) == 0.0){ + posp = 0.0; + velp = 0.0; + accp = 0.0; + force_error_sum = 0.0; + forcep = 0.0; curp = 0.0; } diff --git a/src/comps/rev.comp b/src/comps/rev.comp new file mode 100644 index 00000000..629be027 --- /dev/null +++ b/src/comps/rev.comp @@ -0,0 +1,17 @@ +COMP(rev); + +HAL_PIN(in) = 0.0; +HAL_PIN(out) = 0.0; +HAL_PIN(rev) = 0.0; + +RT_CALC( + if(PIN(rev) > 0.0){ + PIN(out) = minus(0, PIN(in)); + } + else{ + PIN(out) = PIN(in); + } +); + + +ENDCOMP; diff --git a/src/comps/tune.comp b/src/comps/tune.comp new file mode 100644 index 00000000..655ab635 --- /dev/null +++ b/src/comps/tune.comp @@ -0,0 +1,86 @@ +COMP(tune); + +HAL_PIN(iq) = 0.0; +HAL_PIN(id) = 0.0; +HAL_PIN(lp) = 0.0; +HAL_PIN(r) = 1.0; + +HAL_PIN(amp) = 0.0; + +HAL_PIN(start) = 0.0; +HAL_PIN(ready) = 1.0; + +HAL_PIN(start_r) = 1.0; +HAL_PIN(re) = 0.0; +HAL_PIN(l) = 0.0; + +HAL_PIN(time) = 0.1; + +HAL_PIN(freq) = 1.0; +HAL_PIN(cur) = 1.0; + + +MEM(float c) = 0.0; +MEM(float i) = 0.0; + + +RT_CALC( + i = ABS(PIN(amp)) * 0.01 + i * 0.99; + float f = PIN(freq); + PIN(re) = PIN(cur) * PIN(start_r) / i; + + + HT( + GOTO(0); + STATE(0){ + PIN(ready) = 1.0; + if(PIN(start) > 0.0){ + c = PIN(cur) * PIN(start_r); + PIN(r) = PIN(start_r); + PIN(ready) = 0.0; + GOTO(1); + } + } + STATE(1){ + c = -c; + if(PIN(start) <= 0.0){ + c = 0.0; + GOTO(0); + } + SLEEP(0.5 / f); + } + ); + + /*HT( + GOTO(0); + STATE(0){ + PIN(iq) = 0.0; + PIN(id) = 0.0; + PIN(ready) = 1.0; + mpos = mod(minus(fbi, offset_) * PIN(pole_count)); + if(PIN(start) > 0.0){ + c = PIN(cur); + GOTO(1); + } + } + STATE(1){ + PIN(ready) = 0.0; + SLEEP(PIN(time) / 10.0); + GOTO(2); + } + STATE(2){ + PIN(ready) = 0.0; + SLEEP(PIN(time)); + GOTO(2); + } + );*/ + + PIN(id) = c; + + +); + + + + +ENDCOMP; diff --git a/src/conf.h b/src/conf.h new file mode 100644 index 00000000..5ba8c4b3 --- /dev/null +++ b/src/conf.h @@ -0,0 +1,57 @@ +#pragma once + +#define MAX_NAME 32 +typedef char NAME[MAX_NAME]; + +enum pos_iface{ + MISC = 0, + ENC0 = 1, + ENC1 = 2, + RES1 = 3, + MITSU0 = 4, + MITSU1 = 5, + SINCOS1 = 6, + STEPDIR0 = 7, + STEPDIR1 = 8, + SIN = 9, + VEL = 10, + SQUARE = 11 +}; + +enum mot_type{ + 1PH_DC = 1, + 1PH_SYNC = 2, + 1PH_ASYNC = 3, + 2PH_SYNC = 4, + 2PH_ASYNC = 5, + 3PH_SYNC = 6, + 3PH_ASYNC = 7 +}; + +typedef struct mot{ + NAME name; + NAME type; + pos_iface fb; + float fb_offset; + float fb_reverse; + float fb_pole_count; + float pos_p; + float acc_p; + float acc_pi; + float cur_lp; + float j; + float km; + float r; + float l; + float pole_count; + float max_vel; + float max_acc; + float max_force; + float max_cur; +}; + +typedef struct conf{ + pos_iface cmd; + float cmd_reverse; + mot motor; +}; diff --git a/src/hal.h b/src/hal.h index db6c40d2..af186221 100644 --- a/src/hal.h +++ b/src/hal.h @@ -24,7 +24,7 @@ #pragma once -#define MAX_HAL_PINS 256 +#define MAX_HAL_PINS 512 #define MAX_HPNAME 32 #define MAX_COMP_TYPES 32 #define MAX_COMP_FUNCS 32 @@ -134,7 +134,7 @@ int addf_nrt(void (*nrt)(float period)); (name.value) #define GLOBAL_HAL_PIN(name) \ - volatile struct hal_pin name; + volatile struct hal_pin name; #define MEM(var) static var diff --git a/src/link.c b/src/link.c new file mode 100644 index 00000000..2fddb2ad --- /dev/null +++ b/src/link.c @@ -0,0 +1,144 @@ +#include "link.h" +#include "hal.h" + + + +void link_pid(){ + // cmd + link_hal_pins("conf0.ccmd", "mux0.select"); + link_hal_pins("conf0.cmd_rev", "rev0.rev"); + + set_hal_pin("mux0.in0", 0.0); + link_hal_pins("enc0.pos0", "mux0.in1"); + link_hal_pins("enc0.pos1", "mux0.in2"); + link_hal_pins("res0.pos", "mux0.in3"); + set_hal_pin("mux0.in4", 0.0); + link_hal_pins("encm0.pos", "mux0.in5"); + link_hal_pins("enc0.ipos1", "mux0.in6"); + link_hal_pins("enc0.pos0", "mux0.in7"); + link_hal_pins("enc0.pos1", "mux0.in8"); + link_hal_pins("sim0.sin", "mux0.in9"); + link_hal_pins("sim0.vel", "mux0.in10"); + link_hal_pins("sim0.square", "mux0.in11"); + + link_hal_pins("mux0.out", "rev0.in"); + link_hal_pins("rev0.out", "net0.cmd"); + link_hal_pins("net0.cmd", "pderiv0.in"); + link_hal_pins("pderiv0.out", "net0.cmd_d"); + + link_hal_pins("conf0.max_vel", "pderiv0.vel_max"); + link_hal_pins("conf0.max_acc", "pderiv0.acc_max"); + + link_hal_pins("net0.cmd","pid0.pos_ext_cmd"); + link_hal_pins("net0.cmd_d", "pid0.vel_ext_cmd"); + + set_hal_pin("pderiv0.in_lp", 1.0); + set_hal_pin("pderiv0.out_lp", 1.0); + + + // fb + link_hal_pins("conf0.cfb", "mux1.select"); + link_hal_pins("conf0.fb_rev", "rev1.rev"); + + set_hal_pin("mux1.in0", 0.0); + link_hal_pins("enc0.pos0", "mux1.in1"); + link_hal_pins("enc0.pos1", "mux1.in2"); + link_hal_pins("res0.pos", "mux1.in3"); + set_hal_pin("mux1.in4", 0.0); + link_hal_pins("encm0.pos", "mux1.in5"); + link_hal_pins("enc0.ipos1", "mux1.in6"); + link_hal_pins("enc0.pos0", "mux1.in7"); + link_hal_pins("enc0.pos1", "mux1.in8"); + link_hal_pins("sim0.sin", "mux1.in9"); + link_hal_pins("sim0.vel", "mux1.in10"); + link_hal_pins("sim0.square", "mux1.in11"); + + link_hal_pins("mux1.out", "rev1.in"); + link_hal_pins("rev1.out", "cauto0.fb_in"); + link_hal_pins("cauto0.fb_out", "net0.fb"); + link_hal_pins("net0.fb", "pderiv1.in"); + link_hal_pins("pderiv1.out", "net0.fb_d"); + + link_hal_pins("conf0.max_vel", "pderiv1.vel_max"); + link_hal_pins("conf0.max_acc", "pderiv1.acc_max"); + + link_hal_pins("net0.fb","pid0.pos_fb"); + link_hal_pins("net0.fb_d", "pid0.vel_fb"); + + set_hal_pin("pderiv1.in_lp", 1.0); + set_hal_pin("pderiv1.out_lp", 1.0); + + + // cauto + link_hal_pins("conf0.pole_count", "cauto0.pole_count"); + link_hal_pins("cauto0.ready", "pid0.force_en"); + set_hal_pin("cauto0.time", 0.5); + set_hal_pin("cauto0.cur", 1.0); + + // pid + link_hal_pins("conf0.r", "pid0.mot_r"); + link_hal_pins("conf0.j", "pid0.mot_j"); + link_hal_pins("conf0.km", "pid0.mot_km"); + link_hal_pins("conf0.pos_p", "pid0.pos_p"); + link_hal_pins("conf0.acc_p", "pid0.acc_p"); + link_hal_pins("conf0.acc_pi", "pid0.acc_pi"); + link_hal_pins("conf0.max_vel", "pid0.max_vel"); + link_hal_pins("conf0.max_vel", "pid0.vel_limit"); + link_hal_pins("conf0.max_acc", "pid0.max_acc"); + link_hal_pins("conf0.max_acc", "pid0.acc_limit"); + link_hal_pins("conf0.max_force", "pid0.max_force"); + link_hal_pins("conf0.max_force", "pid0.force_limit"); + link_hal_pins("conf0.max_cur", "pid0.max_cur"); + link_hal_pins("conf0.max_cur", "pid0.cur_limit"); + set_hal_pin("pid0.max_volt", 400.0); + set_hal_pin("pid0.volt_limit", 400.0); + set_hal_pin("pid0.max_pwm", 0.95); + set_hal_pin("pid0.pwm_limit", 0.95); + set_hal_pin("pid0.pos_lp", 4000.0); + set_hal_pin("pid0.vel_p", 1.0); + set_hal_pin("pid0.vel_lp", 4000.0); + set_hal_pin("pid0.vel_fb_lp", 4000.0); + set_hal_pin("pid0.acc_lp", 4000.0); + set_hal_pin("pid0.force_p", 1.0); + set_hal_pin("pid0.force_lp", 4000.0); + set_hal_pin("pid0.cur_p", 1.0); + + link_hal_pins("pid0.enable", "net0.enable"); + set_hal_pin("net0.enable", 1.0); + link_hal_pins("net0.vlt", "pid0.volt"); + + + // cur + link_hal_pins("conf0.r", "cur0.r"); + link_hal_pins("conf0.l", "cur0.l"); + link_hal_pins("conf0.max_cur", "cur0.cur_max"); + link_hal_pins("conf0.cur_lp", "cur0.lp"); + link_hal_pins("pid0.induction", "cur0.induction"); + link_hal_pins("cauto0.magpos", "cur0.magpos"); + set_hal_pin("cur0.pwm_max", 0.95); + set_hal_pin("cur0.ind_p", -1.0); + link_hal_pins("net0.vlt", "cur0.volt"); + link_hal_pins("cur0.u", "pwm2uart0.u"); + link_hal_pins("cur0.v", "pwm2uart0.v"); + link_hal_pins("cur0.w", "pwm2uart0.w"); + link_hal_pins("pid0.cur_cmd", "cur0.iq"); + link_hal_pins("cauto0.i_q", "pid0.cur_ext_cmd"); + link_hal_pins("cauto0.i_d", "cur0.id"); + + + // pwm2uart + link_hal_pins("net0.vlt", "pwm2uart0.volt"); + set_hal_pin("pwm2uart0.enable", 0.9); + set_hal_pin("pwm2uart0.pwm_max", 0.9); + + + // 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); +} diff --git a/src/link.h b/src/link.h new file mode 100644 index 00000000..29fbbf6d --- /dev/null +++ b/src/link.h @@ -0,0 +1,3 @@ +#pragma once + +void link_pid(); diff --git a/src/main.c b/src/main.c index 07ddb150..d7e0ec46 100644 --- a/src/main.c +++ b/src/main.c @@ -23,6 +23,7 @@ #include "scanf.h" #include "hal.h" #include "setup.h" +#include "link.h" #include #ifdef USBTERM @@ -45,160 +46,33 @@ volatile uint16_t menc_buf[10]; #define NO 0 #define YES 1 -void link_pid(){ - link_hal_pins("cauto0.fb_out", "net0.fb"); - link_hal_pins("net0.cmd","pid0.pos_ext_cmd"); - link_hal_pins("net0.fb","pid0.pos_fb"); - - // 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.0); - set_hal_pin("pderiv0.out_lp", 1.0); - 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"); - link_hal_pins("net0.vlt", "pid0.volt"); - set_hal_pin("pderiv1.in_lp", 1.0); - set_hal_pin("pderiv1.out_lp", 1.0); - 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); - - //pwm over uart - link_hal_pins("net0.vlt", "pwm2uart0.volt"); - link_hal_pins("net0.vlt", "cur0.volt"); - link_hal_pins("pid0.induction", "cur0.induction"); - - link_hal_pins("cur0.u", "pwm2uart0.u"); - link_hal_pins("cur0.v", "pwm2uart0.v"); - link_hal_pins("cur0.w", "pwm2uart0.w"); - - // 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("cur0.pwm_max", 0.9); - set_hal_pin("pid0.enable", 1.0); - - - link_hal_pins("pid0.cur_cmd", "cauto0.i_in"); - link_hal_pins("cauto0.i_q", "cur0.iq"); - link_hal_pins("cauto0.i_d", "cur0.id"); - - - // magpos - link_hal_pins("cauto0.magpos", "cur0.magpos"); - link_hal_pins("cauto0.fb_out", "net0.fb"); -} - -void link_ac_sync_res(){ - link_hal_pins("enc0.pos0", "net0.cmd"); - link_hal_pins("res0.pos", "cauto0.fb_in"); - link_pid(); -} - -void link_ac_sync_enc(){ - link_hal_pins("enc0.pos0", "net0.cmd"); - link_hal_pins("enc0.pos1", "cauto0.fb_in"); - link_pid(); -} void set_bosch4(){ // achse 4 - link_ac_sync_res(); - // pole count - set_hal_pin("cauto0.pole_count", 4.0); + set_hal_pin("conf0.pole_count", 4.0); + set_hal_pin("conf0.max_vel", RPM(1000)); + set_hal_pin("conf0.max_acc", RPM(1000) / 0.002); + set_hal_pin("conf0.max_force", 1.5); + set_hal_pin("conf0.max_cur", 5.3); - // auto time - set_hal_pin("cauto0.time", 0.5); - - // auto scale - set_hal_pin("cauto0.cur", 1.0); - - set_hal_pin("pderiv0.in_lp", 1.0); - set_hal_pin("pderiv0.out_lp", 1.0); - set_hal_pin("pderiv0.vel_max", RPM(1000)); - set_hal_pin("pderiv0.acc_max", RPM(1000) / 0.002); - - - set_hal_pin("res0.enable", 1.0); - set_hal_pin("res0.reverse", 1.0); - set_hal_pin("pderiv1.in_lp", 1.0); - set_hal_pin("pderiv1.out_lp", 1.0); - set_hal_pin("pderiv1.vel_max", RPM(1000)); - set_hal_pin("pderiv1.acc_max", RPM(1000) / 0.002); - - //pid - set_hal_pin("pid0.mot_r", 15.0);//typenschild - set_hal_pin("pid0.mot_l", 0.002);//unknown - set_hal_pin("pid0.mot_j", 0.000141);//typenschild - set_hal_pin("pid0.mot_km", 0.2727);//typenschild - - - set_hal_pin("pid0.pos_p", 60.0); - set_hal_pin("pid0.pos_lp", 4000.0); - set_hal_pin("pid0.vel_p", 1.0); - set_hal_pin("pid0.vel_lp", 4000.0); - set_hal_pin("pid0.vel_fb_lp", 4000.0); - set_hal_pin("pid0.acc_p", 0.02); - set_hal_pin("pid0.acc_lp", 4000.0); - set_hal_pin("pid0.acc_pi", 70.0); - set_hal_pin("pid0.force_p", 1.0); - set_hal_pin("pid0.force_lp", 4000.0); - set_hal_pin("pid0.cur_p", 1.0); - set_hal_pin("pid0.cur_lp", 10000.0); - - set_hal_pin("pid0.max_vel", RPM(1000)); - set_hal_pin("pid0.max_acc", RPM(1000) / 0.002); - set_hal_pin("pid0.max_force", 1.5); - set_hal_pin("pid0.max_cur", 5.3); - - set_hal_pin("pid0.vel_limit", RPM(1000)); - set_hal_pin("pid0.acc_limit", RPM(1000) / 0.002); - set_hal_pin("pid0.force_limit", 1.5); - set_hal_pin("pid0.cur_limit", 5.3); - - - set_hal_pin("cur0.cur_max", 5.3); - set_hal_pin("cur0.l", 0.016); - set_hal_pin("cur0.r", 15.0); - set_hal_pin("cur0.lp", 0.3); - set_hal_pin("cauto0.start_offset", 1.311); + set_hal_pin("conf0.ccmd", 1.0); + set_hal_pin("conf0.cfb", 3.0); + set_hal_pin("conf0.fb_rev", 1.0); + set_hal_pin("conf0.r", 15.0);//typenschild + set_hal_pin("conf0.l", 0.002);//unknown + set_hal_pin("conf0.j", 0.000141);//typenschild + set_hal_pin("conf0.km", 0.2727);//typenschild + set_hal_pin("conf0.pos_p", 80.0); + set_hal_pin("conf0.acc_p", 0.15); + set_hal_pin("conf0.acc_pi", 50.0); + set_hal_pin("conf0.cur_lp", 0.4); } void set_bosch1(){ // achse 1 - link_ac_sync_res(); set_hal_pin("enc0.res0", 2000.0); - link_hal_pins("enc0.pos0", "lowpass0.in"); - link_hal_pins("lowpass0.out", "net0.cmd"); - set_hal_pin("lowpass0.gain", 0.002); - set_hal_pin("lowpass0.scale", 100.0); + link_hal_pins("enc0.pos0", "net0.cmd"); // pole count set_hal_pin("cauto0.pole_count", 4.0); @@ -260,7 +134,6 @@ void set_bosch1(){ // achse 1 } void set_kuka(){ - link_ac_sync_res(); // pole count set_hal_pin("cauto0.pole_count", 1.0); @@ -295,7 +168,6 @@ void set_kuka(){ } void set_festo(){ - link_pid(); link_hal_pins("enc0.pos0", "net0.cmd"); link_hal_pins("res0.pos", "cauto0.fb_in"); @@ -360,7 +232,6 @@ void set_festo(){ } void set_manutec(){ - link_ac_sync_enc(); set_hal_pin("enc0.res0", 2000.0); set_hal_pin("res0.enable", 1.0); @@ -401,7 +272,6 @@ void set_manutec(){ } void set_bergerlahr(){ - link_ac_sync_enc(); set_hal_pin("enc0.res0", 4096.0); set_hal_pin("res0.enable", 1.0); @@ -462,7 +332,6 @@ void set_bergerlahr(){ //mc101ns //100w 3000rpm 0.8A void set_sankyo(){ - link_ac_sync_enc(); set_hal_pin("enc0.res0", 4096.0); set_hal_pin("res0.enable", 1.0); @@ -522,7 +391,6 @@ void set_sankyo(){ //P50B08100DXS void set_sanyo(){ - link_ac_sync_enc(); set_hal_pin("enc0.res0", 4096.0); set_hal_pin("res0.enable", 1.0); @@ -575,7 +443,6 @@ void set_sanyo(){ //Mitsubishi HA-FF38-UE-S1 void set_mitsubishi(){ - link_pid(); link_hal_pins("enc0.pos0", "net0.cmd"); link_hal_pins("encm0.pos", "cauto0.fb_in"); set_hal_pin("encm0.reverse", 0.0); @@ -652,7 +519,6 @@ void set_mitsubishi(){ } void set_br(){ // achse 4 - link_ac_sync_res(); // pole count set_hal_pin("cauto0.pole_count", 3.0); @@ -822,30 +688,37 @@ int main(void) setup(); //ADC_SoftwareStartConv(ADC1); - //#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/pid2.comp" - #include "comps/term.comp" - #include "comps/sim.comp" - #include "comps/pderiv.comp" - #include "comps/pderiv.comp" - //#include "comps/autophase.comp" - #include "comps/lowpass.comp" - #include "comps/cauto.comp" #include "comps/encm.comp" + #include "comps/sim.comp" + + #include "comps/mux.comp" + #include "comps/mux.comp" + + #include "comps/rev.comp" + #include "comps/rev.comp" + + #include "comps/cauto.comp" + + #include "comps/pderiv.comp" + #include "comps/pderiv.comp" + + #include "comps/pid2.comp" + #include "comps/cur.comp" + + #include "comps/pwm2uart.comp" + + + + #include "comps/term.comp" #include "comps/led.comp" #include "comps/fan.comp" #include "comps/brake.comp" + #include "comps/tune.comp" + //#include "comps/vel_observer.comp" @@ -859,6 +732,28 @@ int main(void) HAL_PIN(vlt) = 0.0; HAL_PIN(tmp) = 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(cfb) = 0.0; + HAL_PIN(ccmd) = 0.0; + HAL_PIN(fb_rev) = 0.0; + HAL_PIN(cmd_rev) = 0.0; + + g_amp = map_hal_pin("net0.amp"); g_vlt = map_hal_pin("net0.vlt"); g_tmp = map_hal_pin("net0.tmp"); @@ -867,12 +762,15 @@ int main(void) hal.init[i](); } + link_pid(); + //set_bergerlahr();//pid2: ok //set_mitsubishi();//pid2: ok //set_festo(); //set_manutec(); //set_precise(); set_bosch4();//pid2: ok + //set_hal_pin("res0.reverse", 0.0); //set_bosch1();//pid2: ok //set_sanyo();//pid2: ok //set_br();