Merge branch 'no_volatile'

Makefile

@@ -30,6 +30,10 @@ HWVERSION = v4
 ifeq ($(HWVERSION),v3)
 	COMPS += src/comps/hw/io3.c
 	COMPS += src/comps/hvf1.c
+	COMPS += shared/comps/pmsm.c
+	COMPS += shared/comps/curpid.c
+	COMPS += shared/comps/dq.c
+	COMPS += shared/comps/idq.c
 	SOURCES += shared/common_f1.c
 	CFLAGS += -DV3
 else
@@ -42,6 +46,7 @@ else
 	COMPS += src/comps/yaskawa.c
 	COMPS += src/comps/encs.c
 	COMPS += src/comps/encf.c
+	COMPS += src/comps/endat.c
 	CFLAGS += -DV4
 endif
 
@@ -55,20 +60,17 @@ COMPS += src/comps/hx711.c
 
 COMPS += shared/comps/sim.c
 COMPS += shared/comps/term.c
-COMPS += shared/comps/curpid.c
 COMPS += shared/comps/svm.c
-COMPS += shared/comps/dq.c
-COMPS += shared/comps/idq.c
+
 COMPS += shared/comps/vel.c
 COMPS += shared/comps/rev.c
 COMPS += shared/comps/hal_test.c
-COMPS += shared/comps/dc.c
+# COMPS += shared/comps/dc.c
 COMPS += shared/comps/ypid.c
 COMPS += shared/comps/fault.c
 COMPS += shared/comps/pid.c
 COMPS += shared/comps/spid.c
 COMPS += shared/comps/pe.c
-COMPS += shared/comps/pmsm.c
 COMPS += shared/comps/pmsm_limits.c
 COMPS += shared/comps/pmsm_ttc.c
 COMPS += shared/comps/dc_limits.c
@@ -98,9 +100,20 @@ COMPS += shared/comps/velbuf.c
 COMPS += shared/comps/avg.c
 COMPS += shared/comps/mux.c
 COMPS += shared/comps/veltopos.c
-COMPS += shared/comps/wobl.c
+# COMPS += shared/comps/wobl.c
 COMPS += shared/comps/debounce.c
 COMPS += shared/comps/pos_filter.c
+COMPS += shared/comps/rl.c
+COMPS += shared/comps/mad.c
+COMPS += shared/comps/sensorless.c
+COMPS += shared/comps/field.c
+COMPS += shared/comps/gain.c
+COMPS += shared/comps/rlpsij.c
+COMPS += shared/comps/veltime.c
+COMPS += shared/comps/mpid.c
+COMPS += shared/comps/fmove.c
+COMPS += shared/comps/home.c
+COMPS += shared/comps/en.c
 
 SOURCES += $(COMPS)
 
@@ -111,6 +124,7 @@ SOURCES += src/syscalls.c
 
 SOURCES += shared/crc8.c
 SOURCES += shared/yaskawa_crc16.c
+SOURCES += shared/endat.c
 SOURCES += shared/angle.c
 SOURCES += shared/hal.c
 SOURCES += shared/commands.c

conf/haas_spindle_slip.txt

@@ -0,0 +1,45 @@
+link pid
+link acim
+link enc_fb0
+link misc
+
+link sserial
+linrev0.scale = 1
+
+conf0.r = 0.5
+conf0.l = 0.005
+conf0.j = 0.015
+conf0.max_force = 30
+conf0.max_ac_cur = 25
+conf0.polecount = 2
+conf0.vel_p = 100
+conf0.vel_i = 5
+conf0.max_sat = 1
+conf0.max_pos_error = 0
+conf0.max_vel = 840
+conf0.max_acc = 2000
+conf0.mot_fb_res = 2000
+conf0.max_dc_volt = 390
+conf0.high_dc_volt = 375
+conf0.mot_fb_rev = 1
+
+iit0.cur_boost = 1.3
+iit0.max_time = 60
+
+acim_ttc0.vel_n = 182.7
+acim_ttc0.torque_n = 23.0
+acim_ttc0.cur_n = 17.0
+acim_ttc0.freq_n = 60.0
+acim_ttc0.u_n = 80.0
+acim_ttc0.u_boost = 7.0
+acim_ttc0.t_boost = 1.3
+
+load ramp
+ramp0.rt_prio = 1.95
+ramp0.vel_ext_cmd = rev0.out_d
+ramp0.en = sserial0.en
+ramp0.max_vel = 838
+ramp0.max_acc = 500
+
+fault0.en = ramp0.en_out
+pid0.pos_en = 0

conf/haas_spindle_slip_uf.txt

@@ -0,0 +1,68 @@
+link pid
+link acim
+link enc_fb0
+link misc
+
+link sserial_uf
+load idx_home
+load linrev
+linrev0.rt_prio = 2.1
+idx_home0.rt_prio = 2.3
+linrev0.fb_in = idx_home0.pos_out
+linrev0.fb_d_in = vel1.vel
+idx_home0.fb = enc_fb0.pos
+idx_home0.fb_abs = enc_fb0.abs_pos
+idx_home0.mot_state = enc_fb0.state
+enc_fb0.amp = 5
+
+idx_home0.index_en = sserial0.index_out
+idx_home0.mot_state = fb_switch0.mot_state
+sserial0.index_clear = idx_home0.index_clear
+linrev0.rev_clear = sserial0.index_clear
+sserial0.pos_fb = linrev0.fb_out
+sserial0.vel_fb = linrev0.fb_d_out
+linrev0.scale = -1
+
+conf0.r = 1.4
+conf0.l = 0.0013
+conf0.j = 0.015
+conf0.max_force = 50
+conf0.max_ac_cur = 25
+acim_ttc0.cur_n = 20
+acim_ttc0.torque_n = 19
+acim_ttc0.freq_n = 50
+acim_ttc0.slip_n = 1.8
+acim_ttc0.u_n = 230
+acim_ttc0.u_boost = 5
+conf0.polecount = 2
+conf0.vel_p = 500
+conf0.vel_i = 1
+conf0.max_sat = 0.5
+iit0.cur_boost = 2
+iit0.max_time = 60
+conf0.max_pos_error = 0
+conf0.max_vel = 900
+conf0.max_acc = 9000
+conf0.mot_fb_res = 2000
+conf0.max_dc_volt = 390
+conf0.high_dc_volt = 375
+
+pid0.max_vel = conf0.max_vel
+pid0.max_torque = 50
+pid0.min_torque = -50
+
+load ramp
+ramp0.rt_prio = 1.95
+ramp0.vel_ext_cmd = sserial0.pos_cmd_d
+ramp0.en = sserial0.out3
+ramp0.max_vel = 837
+ramp0.max_acc = 837
+
+load not
+not0.rt_prio = 2
+not0.in = sserial0.out3
+
+pid0.vel_ext_cmd = ramp0.vel_cmd
+pid0.pos_en = not0.out
+
+acim_ttc0.mode = 2

conf/move.txt

@@ -0,0 +1,60 @@
+link pid
+link pmsm
+link enc_fb0
+enc_fb0.en_index = 0
+link jog_cmd
+link misc
+conf0.r = 2
+conf0.l = 0.007
+conf0.j = 0.000067
+conf0.j_load = 0.00005
+conf0.polecount = 5
+conf0.max_ac_cur = 5
+conf0.mot_fb_res = 20000
+
+load hx
+hx0.rt_prio = 9
+hx0.gain = 1
+
+load en
+en0.rt_prio = 2
+en0.en_in = 1
+fault0.en = en0.en_out0
+en0.fault = fault0.fault
+en0.time = 3
+conf0.phase_cur = 3
+conf0.phase_time = 1
+
+load home
+home0.rt_prio = 9
+home0.en_in = en0.en_out1
+rev0.in = home0.pos_out
+home0.home_in = io0.ind0n
+home0.home_vel = -12
+
+load fmove
+fmove0.rt_prio = 10
+fmove0.en = home0.en_out
+fmove0.force_in = hx0.out
+home0.pos_in = fmove0.mpos
+fmove0.max_usr_vel = 0.1
+fmove0.max_usr_acc = 0.1
+term0.wave0 = fmove0.mpos
+hx0.gain = 870
+hx0.offset = -5.5
+
+fmove0.damping = 0
+fmove0.real_mass = 0
+fmove0.virtual_mass = 5
+fmove0.friction = 0.5
+fmove0.min_pos = 0
+fmove0.max_pos = 1
+fmove0.scale = 340
+fmove0.max_vel = 0.025
+fmove0.max_acc = 0.15
+fmove0.max_usr_vel = 0.35
+fmove0.max_usr_acc = 5
+fmove0.force_th = 2.5
+term0.send_step = 0
+fmove0.print_freq = 10
+fmove0.force_offset_lpf = 0.05

conf/sensorless_perske.txt

@@ -0,0 +1,38 @@
+link pid
+link acim
+link jog_cmd
+link misc
+conf0.r = 4.3
+conf0.l = 0.008
+conf0.max_ac_cur = 2
+load sensorless
+sensorless0.rt_prio = 5
+sensorless0.r = conf0.r
+sensorless0.l = conf0.l
+sensorless0.ud = hv0.ud_fb
+sensorless0.uq = hv0.uq_fb
+sensorless0.id = hv0.id_fb
+sensorless0.iq = hv0.iq_fb
+term0.wave0 = sensorless0.udd
+term0.wave1 = sensorless0.udd
+term0.wave2 = sensorless0.ed
+term0.wave3 = sensorless0.eq
+term0.wave4 = sensorless0.vel
+term0.gain0 = 10
+term0.gain1 = 10
+term0.gain2 = 10
+term0.gain3 = 10
+term0.gain4 = 0.5
+term0.gain5 = 0.5
+term0.gain6 = 0.5
+#link rl
+hv0.d_cmd = 0.55
+hv0.q_cmd = 0
+hv0.pos = sensorless0.pos
+sensorless0.drop = 1.5
+sensorless0.ki = 0.5
+sensorless0.min_vel = 20
+sensorless0.vel_boost = 0.2
+conf0.r = 4
+conf0.max_ac_cur = 6
+sensorless0.kb = 1

conf/spindle_slip_uf.txt

@@ -0,0 +1,81 @@
+link pid
+link acim
+link res_fb0
+link enc_cmd
+link misc
+conf0.r = 2
+conf0.l = 0.016
+conf0.j = 0.0003
+conf0.max_force = 20
+conf0.max_ac_cur = 12
+acim_ttc0.cur_n = 3
+acim_ttc0.torque_n = 1.9
+acim_ttc0.freq_n = 100
+acim_ttc0.u_n = 160
+acim_ttc0.u_boost = 5
+conf0.polecount = 2
+conf0.mot_fb_rev = 1
+conf0.vel_p = 300
+conf0.vel_i = 2
+conf0.max_sat = 0.2
+iit0.cur_boost = 4
+iit0.max_time = 60
+pid0.max_vel = conf0.max_vel
+pid0.max_torque = 20
+pid0.min_torque = -20
+conf0.max_pos_error = 0
+conf0.max_vel = 1100
+conf0.mot_fb_res = 8000
+
+load mad
+mad0.rt_prio = 1.9
+mad0.in = io0.in1
+mad0.mult = 84
+
+load ramp
+ramp0.rt_prio = 1.95
+ramp0.vel_ext_cmd = mad0.out
+ramp0.en = io0.ind0
+ramp0.max_vel = 1050
+ramp0.max_acc = 500
+
+load mux
+mux0.rt_prio = 2
+pid0.pos_en = mux0.out0
+mux0.in0 = 1
+mux0.in10 = 0
+
+pid0.vel_ext_cmd = mux0.out1
+mux0.in1 = vel0.vel
+mux0.in11 = ramp0.vel_cmd
+
+acim_ttc0.mode = mux0.out2
+mux0.in2 = 0
+mux0.in12 = 2
+
+acim_ttc0.slip_n = mux0.out3
+mux0.in3 = 1
+mux0.in13 = 2
+
+fault0.max_sat = mux0.out4
+mux0.in4 = conf0.max_sat
+mux0.in14 = 2
+
+mux0.mux = io0.ind0
+
+res0.freq = 5000
+res0.phase = 0.89
+
+enc_cmd0.mode = 1
+enc_cmd0.remap = 3
+
+load not
+not0.rt_prio = 2
+not0.in = io0.C12
+fault0.en = not0.out
+
+enc_cmd0.fault = fault0.fault
+term0.wave2 = mux0.out1
+term0.gain2 = 0.1
+term0.gain3 = 0.1
+conf0.max_dc_volt = 390

conf/template/acim.txt

@@ -1,25 +1,15 @@
-load pmsm_limits
 load acim_ttc
-pmsm_limits0.rt_prio = 7
 acim_ttc0.rt_prio = 9
-pmsm_limits0.r = conf0.r
-pmsm_limits0.ld = conf0.l
-pmsm_limits0.lq = conf0.l
-pmsm_limits0.psi = conf0.psi
-pmsm_limits0.j = conf0.j
-pmsm_limits0.polecount = conf0.polecount
-pmsm_limits0.ac_volt = hv0.pwm_volt
-pmsm_limits0.iq = hv0.q_fb
-pmsm_limits0.id = hv0.d_fb
-pid0.max_torque = pmsm_limits0.max_torque
-pid0.min_torque = pmsm_limits0.min_torque
-pid0.max_vel = pmsm_limits0.abs_max_vel
+pid0.max_torque = acim_ttc0.t_max
+pid0.min_torque = acim_ttc0.t_min
+pid0.max_vel = conf0.max_vel
 acim_ttc0.torque = pid0.torque_cor_cmd
 acim_ttc0.polecount = conf0.polecount
-acim_ttc0.vel = vel1.vel
+acim_ttc0.vel_m = vel1.vel
 acim_ttc0.mode = 0
-hv0.q_cmd = acim_ttc0.iq
-hv0.d_cmd = acim_ttc0.id
-hv0.cmd_mode = 1
+acim_ttc0.sensorless = 0
+hv0.q_cmd = acim_ttc0.q_cmd
+hv0.d_cmd = acim_ttc0.d_cmd
+hv0.cmd_mode = acim_ttc0.cmd_mode
 hv0.phase_mode = 2
 hv0.pos = acim_ttc0.pos

conf/template/conf.txt

@@ -35,5 +35,5 @@ conf0.vel_p = 2000
 conf0.vel_i = 5
 conf0.vel_g = 1
 conf0.cur_p = 0.2
-conf0.cur_i = 0.0001
+conf0.cur_i = 1.5
 conf0.max_sat = 0.2

conf/template/dc.txt

@@ -8,7 +8,7 @@ dc_limits0.lq = conf0.l
 dc_limits0.psi = conf0.psi
 dc_ttc0.psi = conf0.psi
 dc_limits0.ac_volt = hv0.pwm_volt
-dc_limits0.iq = hv0.q_fb
+dc_limits0.iq = hv0.iq_fb
 pid0.max_torque = dc_limits0.max_torque
 pid0.min_torque = dc_limits0.min_torque
 pid0.max_vel = dc_limits0.abs_max_vel

conf/template/enc_fb0.txt

@@ -13,3 +13,4 @@ io0.fb0y = enc_fb0.error
 io0.fb0g = 1
 enc_fb0.vel = vel1.vel
 enc_fb0.amp = adc0.amp0
+enc_fb0.en_index = 1

conf/template/enc_ws_fb0.txt

@@ -0,0 +1,30 @@
+load enc_fb
+enc_fb0.rt_prio = 2
+enc_fb0.res = conf0.mot_fb_res
+enc_fb0.sin = adc0.sin0l
+enc_fb0.cos = adc0.cos0l
+enc_fb0.quad = adc0.quad
+fb_switch0.mot_pos = enc_fb0.pos
+fb_switch0.mot_abs_pos = enc_fb0.abs_pos
+fb_switch0.mot_polecount = 1
+fb_switch0.mot_state = enc_fb0.state
+fault0.mot_fb_error = enc_fb0.error
+io0.fb0y = enc_fb0.error
+io0.fb0g = 1
+enc_fb0.vel = vel1.vel
+enc_fb0.amp = adc0.amp0
+
+load uvw
+uvw0.rt_prio = 2
+uvw0.u = io0.fb0a
+uvw0.v = io0.fb0b
+uvw0.w = io0.fb0z
+fb_switch0.com_pos = uvw0.pos
+fb_switch0.com_abs_pos = uvw0.pos
+fb_switch0.com_state = uvw0.state
+fb_switch0.com_polecount = conf0.com_fb_polecount
+conf0.com_fb_polecount = conf0.polecount
+uvw0.amp = adc0.amp0
+fault0.com_fb_error = uvw0.error
+enc_fb0.en_index = uvw0.state
+uvw0.mode = 1

conf/template/linkv3.txt

@@ -34,8 +34,7 @@ curpid0.id_cmd = fb_switch0.id
 curpid0.iq_cmd = pmsm_ttc0.cur
 curpid0.dc_volt = hv0.dc_volt
 curpid0.ac_volt = hv0.ac_volt
-curpid0.rd = conf0.r
-curpid0.rq = conf0.r
+curpid0.r = conf0.r
 curpid0.ld = conf0.l
 curpid0.lq = conf0.l
 curpid0.ff = conf0.cur_ff
@@ -57,15 +56,3 @@ fb_switch0.phase_time = conf0.phase_time
 
 pid0.min_torque = pmsm_limits0.next_min_torque
 pid0.max_torque = pmsm_limits0.next_max_torque
-
-
-
-curpid0.max_cur = 5
-
-
-pid0.max_vel = conf0.
-pid0.max_torque = 5.000000
-pid0.min_torque = -5.000000
-pmsm0.vel = 
-curpid0.cmd_mode = 1 ? CURRENT_MODE
-

conf/template/mpid.txt

@@ -0,0 +1,118 @@
+load adc
+load reslimit
+load rev
+load fb_switch
+load veltime
+load veltime
+load vel
+load mpid
+load fault
+load hv
+load sim
+load io
+load iit
+link conf
+hv0.rt_prio = 0.9
+hv0.frt_prio = 1
+adc0.rt_prio = 1
+reslimit0.rt_prio = 3
+rev0.rt_prio = 4
+fb_switch0.rt_prio = 5
+veltime0.frt_prio = 6
+veltime1.frt_prio = 6
+vel0.rt_prio = 6
+io0.rt_prio = 7
+mpid0.rt_prio = 8
+fault0.rt_prio = 10
+iit0.rt_prio = 13
+sim0.rt_prio = 13
+term0.rt_prio = 16
+rev0.rev = conf0.cmd_rev
+fb_switch0.mot_rev = conf0.mot_fb_rev
+fb_switch0.com_rev = conf0.com_fb_rev
+fb_switch0.joint_rev = conf0.joint_fb_rev
+reslimit0.res = conf0.mot_fb_res
+mpid0.j = conf0.j
+mpid0.pos_p = conf0.pos_p
+mpid0.vel_p = conf0.vel_p
+mpid0.vel_i = conf0.vel_i
+mpid0.max_vel = conf0.max_vel
+mpid0.max_acc = conf0.max_acc
+mpid0.max_torque = conf0.max_force
+fault0.max_sat = conf0.max_sat
+fault0.max_pos_error = conf0.max_pos_error
+fault0.max_vel_error = conf0.max_vel_error
+fault0.max_dc_cur = conf0.max_dc_cur
+fault0.max_ac_cur = conf0.max_ac_cur
+fault0.max_dc_volt = conf0.max_dc_volt
+fault0.high_dc_volt = conf0.max_dc_volt
+fault0.min_dc_volt = conf0.min_dc_volt
+fault0.max_hv_temp = conf0.max_hv_temp
+fault0.max_mot_temp = conf0.max_mot_temp
+fault0.high_hv_temp = conf0.high_hv_temp
+fault0.high_mot_temp = conf0.high_mot_temp
+fault0.fan_hv_temp = conf0.fan_hv_temp
+fault0.fan_mot_temp = conf0.fan_mot_temp
+fault0.ac_cur = hv0.abs_cur
+hv0.rev = conf0.out_rev
+hv0.r = conf0.r
+hv0.l = conf0.l
+hv0.psi = conf0.psi
+hv0.cur_p = conf0.cur_p
+hv0.cur_i = conf0.cur_i
+hv0.cur_ff = conf0.cur_ff
+hv0.cur_ind = conf0.cur_ind
+hv0.max_cur = conf0.max_ac_cur
+reslimit0.pos_in = rev0.out
+fb_switch0.cmd_pos = reslimit0.pos_out
+fb_switch0.polecount = conf0.polecount
+fb_switch0.en = fault0.en_fb
+fb_switch0.mot_polecount = conf0.mot_fb_polecount
+fb_switch0.joint_polecount = conf0.joint_fb_polecount
+fb_switch0.com_polecount = conf0.com_fb_polecount
+fb_switch0.mot_offset = conf0.mot_fb_offset
+fb_switch0.joint_offset = conf0.joint_fb_offset
+fb_switch0.com_offset = conf0.com_fb_offset
+fb_switch0.phase_time = conf0.phase_time
+fb_switch0.phase_cur = conf0.phase_cur
+veltime0.pos = rev0.out
+veltime1.pos = fb_switch0.vel_fb
+vel0.pos_in = fb_switch0.com_fb
+vel0.en = fault0.en_fb
+vel0.torque = mpid0.torque_cor_cmd
+mpid0.en = fault0.en_pid
+mpid0.pos_ext_cmd = reslimit0.pos_out
+mpid0.vel_ext_cmd = veltime0.vel_lp
+mpid0.pos_fb = fb_switch0.pos_fb
+mpid0.vel_fb = veltime1.vel_lp
+rev0.in_d = veltime0.vel_lp
+fault0.pos_error = mpid0.pos_error
+fault0.vel_error = mpid0.vel_error
+fault0.dc_volt = hv0.dc_volt
+fault0.hv_temp = hv0.hv_temp
+fault0.dc_cur = hv0.dc_cur
+fault0.hv_error = hv0.fault
+fault0.fb_ready = fb_switch0.state
+hv0.en = fault0.en_out
+hv0.pos = vel2.pos_out
+hv0.vel = vel2.vel
+hv0.scale = fault0.scale
+io0.fan = fault0.hv_fan
+io0.fault = fault0.fault
+io0.state = fault0.state
+pe0.udc = hv0.dc_volt
+pe0.idc = 0
+pe0.ud = 0
+pe0.uq = 0
+pe0.id = hv0.id_fb
+pe0.iq = hv0.iq_fb
+pe0.torque = mpid0.torque_cmd
+pe0.vel = vel1.vel
+pe0.r = conf0.r
+pe0.j = conf0.j
+pe0.cap = 0.00054
+iit0.max_temp = conf0.max_mot_temp
+iit0.high_temp = conf0.high_mot_temp
+iit0.max_cur = conf0.max_ac_cur
+fault0.mot_temp = iit0.temp
+iit0.cur = hv0.abs_cur

conf/template/mpmsm.txt

@@ -0,0 +1,11 @@
+load pmsm_ttc
+pmsm_ttc0.rt_prio = 9
+pmsm_ttc0.psi = conf0.psi
+pmsm_ttc0.polecount = conf0.polecount
+pmsm_ttc0.pos_in = vel0.pos_out
+pmsm_ttc0.torque = mpid0.torque_cmd
+hv0.d_cmd = fb_switch0.id
+hv0.q_cmd = pmsm_ttc0.cur
+hv0.cmd_mode = 1
+hv0.phase_mode = 2
+hv0.pos = pmsm_ttc0.pos_out

conf/template/pid.txt

@@ -115,8 +115,8 @@ pe0.udc = hv0.dc_volt
 pe0.idc = 0
 pe0.ud = 0
 pe0.uq = 0
-pe0.id = hv0.d_fb
-pe0.iq = hv0.q_fb
+pe0.id = hv0.id_fb
+pe0.iq = hv0.iq_fb
 pe0.torque = pid0.torque_cor_cmd
 pe0.vel = vel1.vel
 pe0.r = conf0.r

conf/template/pmsm.txt

@@ -11,8 +11,8 @@ pmsm_limits0.polecount = conf0.polecount
 pmsm_ttc0.psi = conf0.psi
 pmsm_ttc0.polecount = conf0.polecount
 pmsm_limits0.ac_volt = hv0.pwm_volt
-pmsm_limits0.iq = hv0.q_fb
-pmsm_limits0.id = hv0.d_fb
+pmsm_limits0.iq = hv0.iq_fb
+pmsm_limits0.id = hv0.id_fb
 pmsm_ttc0.pos_in = vel2.pos_out
 pid0.max_torque = pmsm_limits0.max_torque
 pid0.min_torque = pmsm_limits0.min_torque

conf/template/rl.txt

@@ -0,0 +1,22 @@
+link pid
+link pmsm
+conf0.max_ac_cur = 15
+load rl
+rl0.rt_prio = 6
+hv0.en = rl0.en_out
+hv0.d_cmd = rl0.ud_cmd
+hv0.q_cmd = rl0.uq_cmd
+rl0.id_fb = hv0.id_fb
+rl0.iq_fb = hv0.iq_fb
+rl0.ud_fb = hv0.ud_fb
+rl0.uq_fb = hv0.uq_fb
+hv0.cmd_mode = 0
+hv0.pos = 0
+term0.wave0 = rl0.ud_cmd
+term0.wave1 = hv0.id_fb
+term0.wave2 = rl0.r
+term0.wave3 = rl0.ld
+term0.gain0 = 5
+term0.gain1 = 5
+term0.gain2 = 30
+term0.gain3 = 3000

conf/template/sserial.txt

@@ -40,5 +40,5 @@ io0.out1 = sserial0.out1
 io0.out2 = sserial0.out2
 fault0.brake_release = sserial0.out3
 term0.wave2 = linrev0.cmd_d_out
-sserial0.current = hv0.q_fb
+sserial0.current = hv0.iq_fb
 linrev0.scale = sserial0.scale

conf/template/uvw_fb1.txt

@@ -10,3 +10,5 @@ fb_switch0.com_polecount = conf0.com_fb_polecount
 io0.fb1g = 1
 io0.fb1y = uvw0.led
 conf0.com_fb_polecount = conf0.polecount
+uvw0.amp = adc0.amp1
+fault0.com_fb_error = uvw0.error

conf/uf.txt

@@ -11,7 +11,7 @@ hv0.pos = uf0.pos
 hv0.d_cmd = uf0.ud
 hv0.q_cmd = 0
 term0.wave0 = hv0.d_cmd
-term0.wave1 = hv0.d_fb
+term0.wave1 = hv0.id_fb
 term0.wave2 = uf0.f_cmd
 term0.wave3 = uf0.f_fb
 term0.gain0 = 1

inc/hw/hw.h

@@ -135,18 +135,6 @@
 #define ADC_TIMER_FREQ 84000000
 #define RES_TIMER_FREQ 20000
 
-#define FB0_SIN_ADC ADC1
-#define FB0_SIN_ADC_RCC RCC_APB2Periph_ADC1
-#define FB0_SIN_ADC_CHAN ADC_Channel_6
-#define FB0_SIN_PIN GPIO_Pin_6
-#define FB0_SIN_PORT GPIOA
-
-#define FB0_COS_ADC ADC2
-#define FB0_COS_ADC_RCC RCC_APB2Periph_ADC2
-#define FB0_COS_ADC_CHAN ADC_Channel_7
-#define FB0_COS_PIN GPIO_Pin_7
-#define FB0_COS_PORT GPIOA
-
 #define FB1
 
 #define FB1_SIN_ADC ADC1
@@ -170,7 +158,66 @@
 #define OP_R_OUT_HIGH 22.0     //opamp out high
 #define OP_REF 1.83            //opamp reference voltage
 
-//FB UART
+// FB0 ADC
+#define FB0_SIN_ADC ADC1
+#define FB0_SIN_ADC_RCC RCC_APB2Periph_ADC1
+#define FB0_SIN_ADC_CHAN ADC_Channel_6
+#define FB0_SIN_PIN GPIO_Pin_6
+#define FB0_SIN_PORT GPIOA
+
+#define FB0_COS_ADC ADC2
+#define FB0_COS_ADC_RCC RCC_APB2Periph_ADC2
+#define FB0_COS_ADC_CHAN ADC_Channel_7
+#define FB0_COS_PIN GPIO_Pin_7
+#define FB0_COS_PORT GPIOA
+
+// FB0 TX EN
+#define FB0_A_EN_PIN GPIO_Pin_11
+#define FB0_A_EN_PORT GPIOD
+
+#define FB0_B_EN_PIN GPIO_Pin_10
+#define FB0_B_EN_PORT GPIOD
+
+#define FB0_Z_TXEN_PIN GPIO_Pin_15
+#define FB0_Z_TXEN_PORT GPIOD
+
+// FB0 TIM4
+#define FB0_A_PIN GPIO_Pin_12
+#define FB0_A_PIN_SOURCE GPIO_PinSource12
+#define FB0_A_PORT GPIOD
+
+#define FB0_B_PIN GPIO_Pin_13
+#define FB0_B_PIN_SOURCE GPIO_PinSource13
+#define FB0_B_PORT GPIOD
+
+#define FB0_Z_PIN GPIO_Pin_14
+#define FB0_Z_PIN_SOURCE GPIO_PinSource14
+#define FB0_Z_PORT GPIOD
+
+#define FB0_RES_REF_PIN GPIO_Pin_14
+#define FB0_RES_REF_PIN_SOURCE GPIO_PinSource14
+#define FB0_RES_REF_PORT GPIOD
+
+#define FB0_RES_REF_TIM_AF GPIO_AF_TIM4
+#define FB0_RES_REF_TIM TIM4
+
+#define FB0_ENC_TIM TIM4
+#define FB0_ENC_TIM_AF GPIO_AF_TIM4
+#define FB0_ENC_TIM_RCC RCC_APB1Periph_TIM4
+
+#define FB0_TIM_A_DMA DMA1_Stream0
+#define FB0_TIM_A_DMA_CHAN DMA_Channel_2
+#define FB0_TIM_A_DMA_TCIF DMA_FLAG_TCIF0
+
+#define FB0_TIM_B_DMA DMA1_Stream3
+#define FB0_TIM_B_DMA_CHAN DMA_Channel_2
+#define FB0_TIM_B_DMA_TCIF DMA_FLAG_TCIF3
+
+#define FB0_TIM_Z_DMA DMA1_Stream7
+#define FB0_TIM_Z_DMA_CHAN DMA_Channel_2
+#define FB0_TIM_Z_DMA_TCIF DMA_FLAG_TCIF7
+
+// FB0 UART
 #define FB0_UART USART6
 #define FB0_UART_RCC RCC_APB2Periph_USART6
 #define FB0_UART_CLOCK_COMMAND RCC_APB2PeriphClockCmd
@@ -183,17 +230,50 @@
 #define FB0_UART_TX_DMA_CHAN DMA_Channel_5
 #define FB0_UART_TX_DMA_TCIF DMA_FLAG_TCIF6
 
-//v4 does not have dedicated rx pin, use pc6 as rx and tx.
-//#define FB0_UART_RX_PIN GPIO_Pin_11
-//#define FB0_UART_RX_PORT GPIOB
-//#define FB0_UART_RX_PIN_SOURCE GPIO_PinSource11
-//#define FB0_UART_RX_AF_SOURCE GPIO_AF_USART3
+#define FB0_UART_RX_PIN GPIO_Pin_7
+#define FB0_UART_RX_PORT GPIOC
+#define FB0_UART_RX_PIN_SOURCE GPIO_PinSource7
+#define FB0_UART_RX_AF_SOURCE GPIO_AF_USART6
 
 #define FB0_UART_TX_PIN GPIO_Pin_6
 #define FB0_UART_TX_PORT GPIOC
 #define FB0_UART_TX_PIN_SOURCE GPIO_PinSource6
 #define FB0_UART_TX_AF_SOURCE GPIO_AF_USART6
 
+#define FB0_UART_CLK_PIN GPIO_Pin_8
+#define FB0_UART_CLK_PORT GPIOC
+#define FB0_UART_CLK_PIN_SOURCE GPIO_PinSource8
+#define FB0_UART_CLK_AF_SOURCE GPIO_AF_USART6
+
+// FB0 SPI
+#define FB0_SPI SPI3
+#define FB0_SPI_RCC RCC_APB2Perip_SPI3
+#define FB0_SPI_CLOCK_COMMAND RCC_APB1PeriphClockCmd
+
+#define FB0_SPI_RX_DMA DMA1_Stream2
+#define FB0_SPI_RX_DMA_CHAN DMA_Channel_0
+#define FB0_SPI_RX_DMA_TCIF DMA_FLAG_TCIF2
+
+#define FB0_SPI_TX_DMA DMA1_Stream7
+#define FB0_SPI_TX_DMA_CHAN DMA_Channel_0
+#define FB0_SPI_TX_DMA_TCIF DMA_FLAG_TCIF7
+
+#define FB0_SPI_RX_PIN GPIO_Pin_11
+#define FB0_SPI_RX_PORT GPIOC
+#define FB0_SPI_RX_PIN_SOURCE GPIO_PinSource11
+#define FB0_SPI_RX_AF_SOURCE GPIO_AF_SPI3
+
+#define FB0_SPI_TX_PIN GPIO_Pin_12
+#define FB0_SPI_TX_PORT GPIOC
+#define FB0_SPI_TX_PIN_SOURCE GPIO_PinSource12
+#define FB0_SPI_TX_AF_SOURCE GPIO_AF_SPI3
+
+#define FB0_SPI_CLK_PIN GPIO_Pin_10
+#define FB0_SPI_CLK_PORT GPIOC
+#define FB0_SPI_CLK_PIN_SOURCE GPIO_PinSource10
+#define FB0_SPI_CLK_AF_SOURCE GPIO_AF_SPI3
+
+
 //HV UART
 #define UART_DRV USART2
 #define UART_DRV_RCC RCC_APB1Periph_USART2
@@ -254,41 +334,6 @@
 #define CMD_D_EN_PIN_SOURCE GPIO_PinSource2
 #define CMD_D_EN_PORT GPIOB
 
-//FB0 TIM4
-#define FB0_A_PIN GPIO_Pin_12
-#define FB0_A_PIN_SOURCE GPIO_PinSource12
-#define FB0_A_PORT GPIOD
-
-#define FB0_B_PIN GPIO_Pin_13
-#define FB0_B_PIN_SOURCE GPIO_PinSource13
-#define FB0_B_PORT GPIOD
-
-#define FB0_Z_PIN GPIO_Pin_14
-#define FB0_Z_PIN_SOURCE GPIO_PinSource14
-#define FB0_Z_PORT GPIOD
-
-#define FB0_RES_REF_PIN GPIO_Pin_14
-#define FB0_RES_REF_PIN_SOURCE GPIO_PinSource14
-#define FB0_RES_REF_PORT GPIOD
-
-#define FB0_RES_REF_TIM_AF GPIO_AF_TIM4
-#define FB0_RES_REF_TIM TIM4
-
-#define FB0_A_EN_PIN GPIO_Pin_11
-#define FB0_A_EN_PIN_SOURCE GPIO_PinSource11
-#define FB0_A_EN_PORT GPIOD
-
-#define FB0_B_EN_PIN GPIO_Pin_10
-#define FB0_B_EN_PIN_SOURCE GPIO_PinSource10
-#define FB0_B_EN_PORT GPIOD
-
-#define FB0_Z_TXEN_PIN GPIO_Pin_15
-#define FB0_Z_TXEN_PORT GPIOD
-
-#define FB0_ENC_TIM TIM4
-#define FB0_ENC_TIM_AF GPIO_AF_TIM4
-#define FB0_ENC_TIM_RCC RCC_APB1Periph_TIM4
-
 //FB1, TIM1
 #define FB1_A_PIN GPIO_Pin_9
 #define FB1_A_PIN_SOURCE GPIO_PinSource9

shared/angle.c

@@ -114,6 +114,10 @@ inline float mod(float a) {
   }
 }
 
+inline int modi(int v, int m){
+  return(SIGN(v) * (ABS(v) % m));
+}
+
 int quadrant(float a) {
   if(a >= 0) {
     if(a < M_PI / 2.0) {

shared/angle.h

@@ -2,6 +2,7 @@
 
 float minus(float a, float b);
 float mod(float a);
+int modi(int v, int m);
 int quadrant(float a);
 void sincos_fast(float x, float *sin, float *cos);
 float err_filter(float *ctx, float max, float dens, float err);

shared/common.h

@@ -51,13 +51,15 @@ _Static_assert(sizeof(packet_bootloader_t) == 24, "packet_bootloader_t size erro
 //process data from f3 to f4
 typedef struct {
   stmbl_talk_header_t header;
-  float d_fb;
-  float q_fb;
+  float id_fb;
+  float iq_fb;
+  float ud_fb;
+  float uq_fb;
   uint8_t fault;
   uint8_t buf;
   uint16_t padding;
 } packet_from_hv_t;
-_Static_assert(sizeof(packet_from_hv_t) == 24, "packet_from_hv_t size error");
+_Static_assert(sizeof(packet_from_hv_t) == 32, "packet_from_hv_t size error");
 
 //process data from f4 to f3
 typedef struct {

shared/comps/ac.c

@@ -10,7 +10,7 @@ struct ac_ctx_t {
   float avg;
 };
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct ac_ctx_t *ctx      = (struct ac_ctx_t *)ctx_ptr;
   struct ac_pin_ctx_t *pins = (struct ac_pin_ctx_t *)pin_ptr;
 

shared/comps/acim_ttc.c

@@ -6,67 +6,167 @@
 
 HAL_COMP(acim_ttc);
 
-HAL_PIN(mode);  // 0 = u/f, 1 = mtpa
+HAL_PIN(mode);  // 0 = slip, 1 = mtpa, 2 = u/f
+HAL_PIN(sensorless);
 
 // motor values
 HAL_PIN(torque_n);
 HAL_PIN(cur_n);
 HAL_PIN(slip_n);
 HAL_PIN(polecount);
+HAL_PIN(freq_n);
+HAL_PIN(vel_n);
+HAL_PIN(u_n);
+HAL_PIN(u_boost);
+HAL_PIN(t_boost);
+HAL_PIN(s_boost);
 
 // torque cmd in
 HAL_PIN(torque);
-HAL_PIN(vel);
+HAL_PIN(vel_m);
 
 // cur cmd out
-HAL_PIN(id);
-HAL_PIN(iq);
-HAL_PIN(freq);
-HAL_PIN(slip)
+HAL_PIN(d_cmd);
+HAL_PIN(q_cmd);
+HAL_PIN(cmd_mode);
 HAL_PIN(pos);
+HAL_PIN(vel_e);
+HAL_PIN(slip);
+HAL_PIN(t_min);
+HAL_PIN(t_max);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+HAL_PIN(scale);
+HAL_PIN(ki);
+HAL_PIN(duty);
+HAL_PIN(duty_setpoint);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  struct acim_ttc_pin_ctx_t *pins = (struct acim_ttc_pin_ctx_t *)pin_ptr;
+  PIN(polecount) = 2;
+  PIN(vel_n) = 1745.0 / 60.0 * 2.0 * M_PI;
+  PIN(torque_n) = 23.0;
+  PIN(cur_n) = 17.0;
+  PIN(freq_n) = 60.0;
+  PIN(u_n) = 80.0;
+  PIN(u_boost) = 7.0;
+  PIN(t_boost) = 1.3;
+  PIN(s_boost) = 2.5;
+  PIN(mode) = 0;
+  PIN(sensorless) = 0;
+  PIN(scale) = 1.0;
+  PIN(ki) = 50.0;
+  PIN(duty_setpoint) = 0.9;
+}
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct acim_ttc_ctx_t * ctx = (struct acim_ttc_ctx_t *)ctx_ptr;
   struct acim_ttc_pin_ctx_t *pins = (struct acim_ttc_pin_ctx_t *)pin_ptr;
 
   float poles    = MAX(PIN(polecount), 1.0);
-  float torque   = PIN(torque);
-  float vel      = PIN(vel);
-  float slip_n   = PIN(slip_n);
-  float torque_n = MAX(PIN(torque_n), 0.001);
+  float vel_n    = PIN(vel_n) * poles;
+  float t_n      = MAX(PIN(torque_n), 0.001);
+  float freq_n   = MAX(PIN(freq_n), 1.0);
+  float slip_n   = freq_n * 2.0 * M_PI - vel_n;
   float cur_n    = PIN(cur_n);
+  float u_n      = PIN(u_n);
+  float u_boost  = PIN(u_boost);
+  float t_boost  = PIN(t_boost);
 
-  float id   = 0.0;
-  float iq   = 0.0;
-  float freq = vel * poles / 2.0 / M_PI;
-  float slip = 0.0;
+  float torque   = PIN(torque);
+  float vel      = 0.0;
+  if(PIN(sensorless) > 0.0){
+    vel = PIN(vel_e);
+  }
+  else{
+    vel = PIN(vel_m) * poles;
+  }
+
+  float d_cmd    = 0.0;
+  float q_cmd    = 0.0;
+  float slip     = 0.0;
+  float cmd_mode = 0;
+
+  float id_n = cur_n / sqrtf(2.0);
+
+  PIN(scale) += (PIN(duty_setpoint) - PIN(duty)) * PIN(ki) * period;
+  PIN(scale) = CLAMP(PIN(scale), 0.01, 1);
 
   switch((int)PIN(mode)) {
-    case 0:                       // slip control
-      id   = cur_n / sqrtf(2.0);  // constant flux
-      iq   = cur_n / sqrtf(2.0) / torque_n * torque;
-      slip = slip_n / torque_n * torque;
+    case 0: // slip control
+      cmd_mode = 1.0; // cur cmd
+      // d_cmd = MIN(id_n, id_n * freq_n * 2.0 * M_PI * v_boost / vel); // constant flux
+      d_cmd = id_n * PIN(scale);
+      q_cmd = id_n / t_n * torque / PIN(scale);
+      slip = slip_n * q_cmd / d_cmd;
+
+      // id = id_n
+      // slip = slip_n * iq / id
+      // torque = 3/2 * p * K * iq * id
+
+      // id = id_n * scale
+      // // iq = toruqe / t_n * id_n * id_n / id
+      // iq = toruqe / t_n * id_n / scale
+      // slip = slip_n * iq / id
+
+      // torque = 3/2 * p * K * iq * id
+      // torque = t_n / id_n / id_n * id * iq
+      // iq = toruqe / t_n * id_n * id_n / id
       break;
 
-    case 1:  // mtpa
-      id   = 0.0;
-      iq   = cur_n / torque_n * torque;
+    case 1: // mtpa
+      cmd_mode = 1.0; // cur cmd
+      d_cmd = 0.0;
+      q_cmd = cur_n / t_n * torque;
       slip = slip_n * SIGN(torque);  // constant slip
       break;
 
+    case 2: // u/f slip
+      cmd_mode = 0; // volt cmd
+      slip = slip_n / t_n * torque;
+      d_cmd = MAX(u_n / freq_n * ABS(vel / 2.0 / M_PI), u_boost);
+      q_cmd = 0.0;
+      break;
+
     default:
-      id   = 0;
-      iq   = 0;
+      cmd_mode = 1.0; // cur cmd
+      d_cmd = 0;
+      q_cmd = 0;
       slip = 0.0;
   }
 
-  freq += slip;
+  float t_min = 0;
+  float t_max = 0;
 
-  PIN(id)   = id;
-  PIN(iq)   = iq;
-  PIN(freq) = freq;
+  if(PIN(vel_m) > 0.0){
+    t_max = t_n * t_boost * PIN(scale);
+    t_min = -t_max;
+  }
+  else{
+    t_min = -t_n * t_boost * PIN(scale);
+    t_max = -t_min;
+  }
+
+  slip = LIMIT(slip, slip_n * PIN(s_boost));
+
+  if(PIN(sensorless) > 0.0){
+    vel -= slip;
+    PIN(vel_m) = vel * poles;
+  }
+  else{
+    vel += slip;
+    PIN(vel_e) = vel;
+  }
+
+  PIN(t_min) = t_min;
+  PIN(t_max) = t_max;
+
+  PIN(cmd_mode) = cmd_mode;
+  PIN(d_cmd)   = d_cmd;
+  PIN(q_cmd)   = q_cmd;
+
+  PIN(slip_n) = slip_n;
   PIN(slip) = slip;
-  PIN(pos)  = mod(PIN(pos) + freq * period * 2.0 * M_PI);
+  PIN(pos)  = mod(PIN(pos) + vel * period);
 }
 
 hal_comp_t acim_ttc_comp_struct = {
@@ -74,7 +174,7 @@ hal_comp_t acim_ttc_comp_struct = {
     .nrt       = 0,
     .rt        = rt_func,
     .frt       = 0,
-    .nrt_init  = 0,
+    .nrt_init  = nrt_init,
     .rt_start  = 0,
     .frt_start = 0,
     .rt_stop   = 0,

shared/comps/and.c

@@ -9,7 +9,7 @@ HAL_PIN(in3);
 
 HAL_PIN(out);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct and_pin_ctx_t *pins = (struct and_pin_ctx_t *)pin_ptr;
 
   PIN(out) = (PIN(in0) > 0.0) & (PIN(in1) > 0.0) & (PIN(in2) > 0.0) & (PIN(in3) > 0.0);

shared/comps/auto_ac.c

@@ -9,7 +9,7 @@ HAL_PIN(lpf);
 HAL_PIN(th);
 HAL_PIN(offset);
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct auto_ac_ctx_t * ctx = (struct auto_ac_ctx_t *)ctx_ptr;
   struct auto_ac_pin_ctx_t *pins = (struct auto_ac_pin_ctx_t *)pin_ptr;
   PIN(lpf) = 1.0;
@@ -18,7 +18,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
 }
 
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct auto_ac_pin_ctx_t *pins = (struct auto_ac_pin_ctx_t *)pin_ptr;
 
   if(ABS(PIN(in)) <= PIN(th)){

shared/comps/avg.c

@@ -9,7 +9,7 @@ HAL_PIN(lpf);
 HAL_PIN(mult);
 HAL_PIN(offset);
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct avg_ctx_t * ctx = (struct avg_ctx_t *)ctx_ptr;
   struct avg_pin_ctx_t *pins = (struct avg_pin_ctx_t *)pin_ptr;
   PIN(lpf) = 100;
@@ -18,7 +18,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
 }
 
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct avg_pin_ctx_t *pins = (struct avg_pin_ctx_t *)pin_ptr;
   PIN(out) = (PIN(in) * PIN(mult) + PIN(offset)) * LP_HZ(PIN(lpf)) + (1.0 - LP_HZ(PIN(lpf))) * PIN(out);
 }

shared/comps/curpid.c

@@ -30,8 +30,7 @@ HAL_PIN(max_cur);
 HAL_PIN(pwm_volt);
 
 // d, q resistance and inductance
-HAL_PIN(rd);
-HAL_PIN(rq);
+HAL_PIN(r);
 HAL_PIN(ld);
 HAL_PIN(lq);
 
@@ -41,7 +40,11 @@ HAL_PIN(psi);
 HAL_PIN(ff);  // r feed forward
 HAL_PIN(kp);
 HAL_PIN(ki);
+HAL_PIN(ksp); // predictor
 HAL_PIN(kind);  // bemf feed forward
+HAL_PIN(kci);
+
+HAL_PIN(scale);
 
 HAL_PIN(vel);  // velocity input
 
@@ -54,34 +57,35 @@ struct curpid_ctx_t {
   float iq_error_sum;
 };
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct curpid_ctx_t * ctx = (struct curpid_ctx_t *)ctx_ptr;
   struct curpid_pin_ctx_t *pins = (struct curpid_pin_ctx_t *)pin_ptr;
 
-  PIN(rd)  = 0.5;
-  PIN(rq)  = 0.5;
+  PIN(r)  = 0.5;
   PIN(ld)  = 0.01;
   PIN(lq)  = 0.01;
   PIN(psi) = 0.05;
   PIN(kp)  = 0.1;
   PIN(ki)  = 0.005;
+  PIN(kci)  = 500.0;
+  PIN(ksp)  = 1.0;
+  PIN(scale) = 1.0;
 }
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct curpid_ctx_t *ctx      = (struct curpid_ctx_t *)ctx_ptr;
   struct curpid_pin_ctx_t *pins = (struct curpid_pin_ctx_t *)pin_ptr;
 
-  float rd = MAX(PIN(rd), 0.1);
-  float rq = MAX(PIN(rq), 0.1);
+  float r = MAX(PIN(r), 0.1);
   float ld = MAX(PIN(ld), 0.001);
   float lq = MAX(PIN(lq), 0.001);
 
   float ff   = PIN(ff);
   float kind = PIN(kind);
   float kpd  = ld * PIN(kp) / period / 2.0;
-  float kid  = rd * PIN(ki) / ld;
+  float kid  = r * PIN(ki) / ld;
   float kpq  = lq * PIN(kp) / period / 2.0;
-  float kiq  = rq * PIN(ki) / lq;
+  float kiq  = r * PIN(ki) / lq;
 
   float max_cur = MAX(PIN(max_cur), 0.01);
   float idc     = PIN(id_cmd);
@@ -95,26 +99,24 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
   // float ac_current = id * id + iq * iq;  //sqrtf(id * id + iq * iq);
   // PIN(ac_current)  = ac_current;
 
-  float k;
   float abscur;
-  
+  float absvolt;
+
   if(PIN(cmd_mode) == VOLT_MODE) {
+    absvolt = idc * idc + iqc * iqc; // clamp cmd
+    PIN(scale) *= sqrtf(CLAMP(max_volt * max_volt / MAX(absvolt, max_volt * 0.1), 0.0, 1.0));
+
     abscur = id * id + iq * iq; // clamp over fb
-    if(abscur > max_cur * max_cur) {
-      idc = 0;
-      iqc = 0;
-    }
+    PIN(scale) += (max_cur * max_cur - abscur) * PIN(kci) * period;
   }
   else{
     abscur = idc * idc + iqc * iqc; // clamp cmd
-    if(abscur > max_cur * max_cur) {
-      k = max_cur * max_cur / abscur;
-      idc *= k;
-      iqc *= k;
-    }
+    PIN(scale) = sqrtf(max_cur * max_cur / MAX(abscur, max_cur * 0.1));
   }
-
-
+  PIN(scale) = CLAMP(PIN(scale), 0.0, 1.0);
+  
+  idc *= PIN(scale);
+  iqc *= PIN(scale);
 
   float vel   = PIN(vel);
   float psi_d = ld * id + PIN(psi);
@@ -122,15 +124,19 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
   float indd  = vel * psi_q;
   float indq  = vel * psi_d;
 
+  // predictor to cancel pwm delay
+  id += (PIN(ud) - r * id + indd) / ld * period * PIN(ksp);
+  iq += (PIN(uq) - r * iq - indq) / lq * period * PIN(ksp);
+
   float id_error = idc - id;
   float iq_error = iqc - iq;
 
-  float ud = LIMIT(ff * rd * idc - kind * indd + kpd * id_error, max_volt);
-  float uq = LIMIT(ff * rq * iqc + kind * indq + kpq * iq_error, max_volt);
+  float ud = LIMIT(ff * r * idc - kind * indd + kpd * id_error, max_volt);
+  float uq = LIMIT(ff * r * iqc + kind * indq + kpq * iq_error, max_volt);
 
   if(kpd * kid > 0.0 && kpq * kiq > 0.0) {
-    ctx->id_error_sum = LIMIT(ctx->id_error_sum + kpd * kid * id_error, max_volt - ud);
-    ctx->iq_error_sum = LIMIT(ctx->iq_error_sum + kpq * kiq * iq_error, max_volt - uq);
+    ctx->id_error_sum = LIMIT(ctx->id_error_sum + kpd * kid * id_error * period, max_volt - ud);
+    ctx->iq_error_sum = LIMIT(ctx->iq_error_sum + kpq * kiq * iq_error * period, max_volt - uq);
   } else {
     ctx->id_error_sum = 0.0;
     ctx->iq_error_sum = 0.0;
@@ -140,8 +146,8 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
   uq += ctx->iq_error_sum;
 
   if(PIN(cmd_mode) == VOLT_MODE) {
-    ud                = LIMIT(idc, max_volt);
-    uq                = LIMIT(iqc, max_volt);
+    ud                = idc;
+    uq                = iqc;
     ctx->id_error_sum = 0.0;
     ctx->iq_error_sum = 0.0;
     id_error          = 0.0;

shared/comps/dc.c

@@ -13,7 +13,7 @@ HAL_PIN(u);
 HAL_PIN(v);
 HAL_PIN(w);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct hv_ctx_t * ctx = (struct hv_ctx_t *)ctx_ptr;
   struct dc_pin_ctx_t *pins = (struct dc_pin_ctx_t *)pin_ptr;
 

shared/comps/dc_limits.c

@@ -36,7 +36,7 @@ HAL_PIN(abs_max_vel);
 HAL_PIN(iq);
 HAL_PIN(vel);
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct dc_limits_ctx_t * ctx = (struct dc_limits_ctx_t *)ctx_ptr;
   struct dc_limits_pin_ctx_t *pins = (struct dc_limits_pin_ctx_t *)pin_ptr;
 
@@ -46,7 +46,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
   PIN(lq)  = 0.001;
 }
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct dc_limits_ctx_t * ctx = (struct dc_limits_ctx_t *)ctx_ptr;
   struct dc_limits_pin_ctx_t *pins = (struct dc_limits_pin_ctx_t *)pin_ptr;
 

shared/comps/dc_ttc.c

@@ -15,7 +15,7 @@ HAL_PIN(torque);
 // cur cmd out
 HAL_PIN(cur);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct dc_ttc_ctx_t * ctx = (struct dc_ttc_ctx_t *)ctx_ptr;
   struct dc_ttc_pin_ctx_t *pins = (struct dc_ttc_pin_ctx_t *)pin_ptr;
 

shared/comps/debounce.c

@@ -11,7 +11,7 @@ HAL_PIN(out);
 HAL_PIN(debounce_time);
 HAL_PIN(timer);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   //struct debounce_ctx_t *ctx      = (struct debounce_ctx_t *)ctx_ptr;
   struct debounce_pin_ctx_t *pins = (struct debounce_pin_ctx_t *)pin_ptr;
 

shared/comps/dq.c

@@ -27,7 +27,7 @@ HAL_PIN(y);
 HAL_PIN(d);
 HAL_PIN(q);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct dq_ctx_t * ctx = (struct dq_ctx_t *)ctx_ptr;
   struct dq_pin_ctx_t *pins = (struct dq_pin_ctx_t *)pin_ptr;
 

shared/comps/en.c

@@ -0,0 +1,65 @@
+#include "commands.h"
+#include "hal.h"
+#include "math.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(en);
+
+HAL_PIN(en_in);
+HAL_PIN(en_out0);
+HAL_PIN(en_out1);
+HAL_PIN(fault);
+
+HAL_PIN(time);
+HAL_PIN(timer);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr){
+  struct en_pin_ctx_t *pins = (struct en_pin_ctx_t *)pin_ptr;
+  PIN(time) = 5;
+}
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  //struct en_ctx_t *ctx      = (struct en_ctx_t *)ctx_ptr;
+  struct en_pin_ctx_t *pins = (struct en_pin_ctx_t *)pin_ptr;
+
+  if(PIN(en_in) <= 0.0){
+    PIN(timer) = 0.0;
+  }
+  else{
+    PIN(timer) += period;
+  }
+
+  if(PIN(fault) > 0.0){
+    PIN(timer) = 0.0;
+  }
+
+  if(PIN(timer) > PIN(time) / 2.0){
+    PIN(en_out0) = 1.0;
+  }
+  else{
+    PIN(en_out0) = 0.0;
+  }
+
+  if(PIN(timer) > PIN(time)){
+    PIN(en_out1) = 1.0;
+    PIN(timer) = PIN(time);
+  }
+  else{
+    PIN(en_out1) = 0.0;
+  }
+}
+
+hal_comp_t en_comp_struct = {
+    .name      = "en",
+    .nrt       = 0,
+    .rt        = rt_func,
+    .frt       = 0,
+    .nrt_init  = nrt_init,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = 0,
+    .pin_count = sizeof(struct en_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};

shared/comps/fanuc.c

@@ -15,7 +15,7 @@ HAL_PIN(C8);
 //rotor position output
 HAL_PIN(pos);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct fanuc_ctx_t * ctx = (struct fanuc_ctx_t *)ctx_ptr;
   struct fanuc_pin_ctx_t *pins = (struct fanuc_pin_ctx_t *)pin_ptr;
 

shared/comps/fault.c

@@ -95,7 +95,7 @@ struct fault_ctx_t {
   float mot_temp_error;
 };
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct fault_ctx_t *ctx      = (struct fault_ctx_t *)ctx_ptr;
   struct fault_pin_ctx_t *pins = (struct fault_pin_ctx_t *)pin_ptr;
 
@@ -113,7 +113,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
   PIN(fan_mot_temp)  = 60.0;
 }
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct fault_ctx_t *ctx      = (struct fault_ctx_t *)ctx_ptr;
   struct fault_pin_ctx_t *pins = (struct fault_pin_ctx_t *)pin_ptr;
 
@@ -278,7 +278,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
 }
 
 
-static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_func(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct fault_ctx_t *ctx      = (struct fault_ctx_t *)ctx_ptr;
   struct fault_pin_ctx_t *pins = (struct fault_pin_ctx_t *)pin_ptr;
 

shared/comps/fb_switch.c

@@ -65,7 +65,7 @@ struct fb_switch_ctx_t {
   int32_t phase_state;
 };
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct fb_switch_ctx_t *ctx      = (struct fb_switch_ctx_t *)ctx_ptr;
   struct fb_switch_pin_ctx_t *pins = (struct fb_switch_pin_ctx_t *)pin_ptr;
 
@@ -78,7 +78,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
   PIN(offset_first_enable) = 1.0;
 }
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct fb_switch_ctx_t *ctx      = (struct fb_switch_ctx_t *)ctx_ptr;
   struct fb_switch_pin_ctx_t *pins = (struct fb_switch_pin_ctx_t *)pin_ptr;
 

shared/comps/field.c

@@ -0,0 +1,152 @@
+/*
+* This file is part of the stmbl project.
+*
+* Copyright (C) 2013-2016 Rene Hopf <renehopf@mac.com>
+* Copyright (C) 2013-2016 Nico Stute <crinq@crinq.de>
+*
+* This program is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You shou l have received a copy of the GNU General Public License
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "commands.h"
+#include "hal.h"
+#include "math.h"
+#include "defines.h"
+#include "angle.h"
+
+
+HAL_COMP(field);
+
+//in
+HAL_PIN(vele_n);
+HAL_PIN(velm_n);
+HAL_PIN(i_n);
+HAL_PIN(t_n);
+HAL_PIN(vel_boost);
+HAL_PIN(t_boost);
+
+HAL_PIN(vel);
+
+HAL_PIN(vel_mode);
+HAL_PIN(in_mode);
+
+// inout
+HAL_PIN(t);
+HAL_PIN(iq);
+
+//out
+HAL_PIN(id);
+
+HAL_PIN(iq_max);
+HAL_PIN(iq_min);
+
+HAL_PIN(t_max);
+HAL_PIN(t_min);
+
+HAL_PIN(slip);
+HAL_PIN(velm);
+HAL_PIN(vele);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  // struct field_ctx_t *ctx      = (struct field_ctx_t *)ctx_ptr;
+  struct field_pin_ctx_t *pins = (struct field_pin_ctx_t *)pin_ptr;
+  PIN(vele_n) = 60.0 * 2.0 * M_PI / 2.0;
+  PIN(velm_n) = 1745.0 / 60.0 * 2.0 * M_PI;
+  PIN(i_n) = 15.0;
+  PIN(t_n) = 23.0;
+  PIN(vel_boost) = 1.0;
+  PIN(t_boost) = 1.5;
+}
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  // struct field_ctx_t *ctx      = (struct field_ctx_t *)ctx_ptr;
+  struct field_pin_ctx_t *pins = (struct field_pin_ctx_t *)pin_ptr;
+
+  float vel = MAX(ABS(PIN(vele)), 0.01);
+  float i_n = PIN(i_n);
+  float t_n = MAX(PIN(t_n), 0.01);
+  float vele_n = PIN(vele_n);
+  float velm_n = PIN(velm_n);
+  float t_boost = PIN(t_boost);
+  float vel_boost = PIN(vel_boost);
+  float slip_n = vele_n - velm_n;
+  float t = 0;
+  float iq = 0;
+  float slip = 0;
+  
+
+  // float p_max = vel_n * t_n * vel_boost;
+  // float t_max = MIN(t_n * t_boost, p_max / vel);
+
+  float id_n = i_n / sqrtf(2.0);
+  float id = MIN(id_n, id_n * vele_n * vel_boost / vel);
+
+  float iq_max = 0;
+  float iq_min = 0;
+  
+  if(PIN(vel) > 0.0){
+    iq_max = MIN(id_n * t_boost, id_n * vele_n * vel_boost / vel);
+    iq_min = -id_n * t_boost;
+  }
+  else{
+    iq_max = id_n * t_boost;
+    iq_min = -MIN(id_n * t_boost, id_n * vele_n * vel_boost / vel);
+  }
+
+  float t_max = iq_max * id / id_n / id_n * t_n;
+  float t_min = iq_min * id / id_n / id_n * t_n;
+  
+  if(PIN(in_mode) > 0){ // ipnut = torque
+    t = PIN(t);
+    t = CLAMP(t, t_min, t_max);
+    iq = t / t_n / id * id_n * id_n;
+    PIN(iq) = iq;
+  }
+  else{ // input = iq
+    iq = PIN(iq);
+    iq = CLAMP(iq, iq_min, iq_max);
+    t = iq * id / id_n / id_n * t_n;
+    PIN(t) = t;
+  }
+  slip = slip_n / t_n * t;
+
+  if(PIN(vel_mode) > 0){ // input = velm
+    PIN(vele) = PIN(vel) + slip;
+    PIN(velm) = PIN(vel);
+  }
+  else{ // input = vele
+    PIN(vele) = PIN(vel);
+    PIN(velm) = PIN(vel) - slip;
+  }
+
+  PIN(slip) = slip;
+  PIN(id) = id;
+  PIN(iq_max) = iq_max;
+  PIN(iq_min) = iq_min;
+  PIN(t_max) = t_max;
+  PIN(t_min) = t_min;  
+}
+
+hal_comp_t field_comp_struct = {
+    .name      = "field",
+    .nrt       = 0,
+    .rt        = rt_func,
+    .frt       = 0,
+    .nrt_init  = nrt_init,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = 0,
+    .pin_count = sizeof(struct field_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};

shared/comps/fmove.c

@@ -0,0 +1,152 @@
+#include "commands.h"
+#include "hal.h"
+#include "math.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(fmove);
+
+HAL_PIN(en);
+
+HAL_PIN(gravity);
+HAL_PIN(real_mass);
+HAL_PIN(virtual_mass);
+HAL_PIN(damping);
+HAL_PIN(friction);
+
+HAL_PIN(min_pos);
+HAL_PIN(max_pos);
+HAL_PIN(max_vel);
+HAL_PIN(max_acc);
+HAL_PIN(max_usr_vel);
+HAL_PIN(max_usr_acc);
+HAL_PIN(scale);
+
+HAL_PIN(force_in);
+HAL_PIN(force);
+HAL_PIN(force_offset);
+HAL_PIN(force_offset_lpf);
+HAL_PIN(pos);
+HAL_PIN(mpos);
+HAL_PIN(vel);
+HAL_PIN(vel_old);
+HAL_PIN(acc);
+HAL_PIN(target);
+
+HAL_PIN(force_time);
+HAL_PIN(force_timer);
+HAL_PIN(force_th);
+
+HAL_PIN(print_freq);
+HAL_PIN(print_timer);
+
+HAL_PIN(mode);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  struct fmove_pin_ctx_t *pins = (struct fmove_pin_ctx_t *)pin_ptr;
+  PIN(force_time) = 5.0;
+  PIN(max_acc) = 1;
+  PIN(max_vel) = 0.1;
+  PIN(max_usr_acc) = 2;
+  PIN(max_usr_vel) = 0.1;
+  PIN(min_pos) = 0.0;
+  PIN(max_pos) = 0.5;
+  PIN(real_mass) = 0.0;
+  PIN(virtual_mass) = 1.0;
+  PIN(damping) = 1.0;
+  PIN(friction) = 0.01;
+  PIN(scale) = 1000.0 / 25.0 * 2.0 * M_PI * 2.0;
+  PIN(force_th) = 0.1;
+  PIN(gravity) = 0.0;
+  PIN(force_offset_lpf) = 0.001;
+}
+
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  struct fmove_pin_ctx_t *pins = (struct fmove_pin_ctx_t *)pin_ptr;
+
+  PIN(vel_old) = PIN(vel);
+
+  PIN(force) = PIN(force_in) - PIN(gravity) * PIN(real_mass) - PIN(acc) * PIN(real_mass);
+  PIN(force) -= PIN(force_offset);
+  PIN(force_offset) += PIN(force) * PIN(force_offset_lpf) * period;
+  PIN(force) -= SIGN(PIN(vel)) * PIN(friction) + PIN(vel) * PIN(damping);
+
+  PIN(force_timer) += period;
+  if(ABS(PIN(force)) > PIN(force_th)){
+    PIN(force_timer) = 0.0;
+    PIN(mode) = 1;
+  }
+  else if(PIN(mode) > 0 && ABS(PIN(vel)) > PIN(max_usr_vel) * 0.05){
+    PIN(force_timer) = 0.0;
+  }
+  else if(PIN(force_timer) > PIN(force_time)){
+    PIN(force_timer) = PIN(force_time);
+    PIN(mode) = 0;
+  }
+
+  PIN(target) = CLAMP(PIN(target), PIN(min_pos), PIN(max_pos));
+
+  switch((int) PIN(mode)){
+    case 0: // auto move
+      PIN(vel) = SIGN(PIN(target) - PIN(pos)) * sqrtf(ABS(PIN(target) - PIN(pos)) * 2.0 * PIN(max_acc));
+      PIN(vel) = CLAMP(PIN(vel), PIN(vel_old) - PIN(max_acc) * period, PIN(vel_old) + PIN(max_acc) * period);
+      PIN(vel) = LIMIT(PIN(vel), PIN(max_vel));
+    break;
+    case 1: // force move
+      PIN(acc) = PIN(force) / PIN(virtual_mass);
+      PIN(acc) = LIMIT(PIN(acc), PIN(max_usr_acc));
+      PIN(vel) += PIN(acc) * period;
+      PIN(vel) = CLAMP(PIN(vel), PIN(vel_old) - PIN(max_usr_acc) * period, PIN(vel_old) + PIN(max_usr_acc) * period);
+      PIN(vel) = LIMIT(PIN(vel), PIN(max_usr_vel));
+    break;
+  }
+
+  PIN(vel) = CLAMP(PIN(vel), -sqrtf(ABS(PIN(pos) - PIN(min_pos)) * 2.0 * MAX(PIN(max_acc), PIN(max_usr_acc))), sqrtf(ABS(PIN(pos) - PIN(max_pos)) * 2.0 * MAX(PIN(max_acc), PIN(max_usr_acc))));
+  PIN(acc) = (PIN(vel) - PIN(vel_old)) / period;
+
+  PIN(pos) += PIN(vel) * period;
+  PIN(pos) = CLAMP(PIN(pos), PIN(min_pos), PIN(max_pos));
+  PIN(mpos) = mod(PIN(pos) * PIN(scale));
+  PIN(print_timer) += period;
+
+  if(PIN(en) <= 0.0){
+    PIN(pos) = 0.0;
+    PIN(mpos) = 0.0;
+    PIN(vel) = 0.0;
+  }
+}
+
+static void nrt_func(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  struct fmove_pin_ctx_t *pins = (struct fmove_pin_ctx_t *)pin_ptr;
+  if(PIN(print_freq) > 0.0){
+    if(PIN(print_timer) > 1.0 / PIN(print_freq)){
+      PIN(print_timer) = 0.0;
+      if(PIN(mode) > 0.0){
+        printf("force_move %f\n", PIN(pos));
+      }
+      else if(ABS(PIN(target) - PIN(pos)) < 0.01 & ABS(PIN(vel)) < PIN(max_vel) * 0.1){
+        printf("on_target %f\n", PIN(pos));
+      }
+      else{
+        printf("moving %f\n", PIN(pos));
+      }
+    }
+  }
+  else{
+    PIN(print_timer) = 0.0;
+  }
+}
+
+hal_comp_t fmove_comp_struct = {
+    .name      = "fmove",
+    .nrt       = nrt_func,
+    .rt        = rt_func,
+    .frt       = 0,
+    .nrt_init  = nrt_init,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = 0,
+    .pin_count = sizeof(struct fmove_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};

shared/comps/gain.c

@@ -0,0 +1,35 @@
+#include "commands.h"
+#include "hal.h"
+#include "math.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(gain);
+
+HAL_PIN(vel);
+HAL_PIN(max_vel);
+
+HAL_PIN(scale);
+
+
+
+static void rt_func(float  gainriod, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  // struct gain_ctx_t * ctx = (struct gain_ctx_t *)ctx_ptr;
+  struct gain_pin_ctx_t *pins = (struct gain_pin_ctx_t *)pin_ptr;
+
+  PIN(scale) = CLAMP(ABS(PIN(vel) / MAX(PIN(max_vel), 0.1)), 0.0, 1.0);
+}
+
+hal_comp_t gain_comp_struct = {
+    .name      = "gain",
+    .nrt       = 0,
+    .rt        = rt_func,
+    .frt       = 0,
+    .nrt_init  = 0,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = 0,
+    .pin_count = sizeof(struct gain_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};

shared/comps/hal_test.c

@@ -5,7 +5,7 @@ HAL_COMP(hal_test);
 HAL_PIN(rt_wait);
 HAL_PIN(frt_wait);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct rev_ctx_t * ctx = (struct rev_ctx_t *)ctx_ptr;
   struct hal_test_pin_ctx_t *pins = (struct hal_test_pin_ctx_t *)pin_ptr;
 
@@ -16,7 +16,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
   }
 }
 
-static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct rev_ctx_t * ctx = (struct rev_ctx_t *)ctx_ptr;
   struct hal_test_pin_ctx_t *pins = (struct hal_test_pin_ctx_t *)pin_ptr;
 

shared/comps/home.c

@@ -6,57 +6,56 @@
 
 HAL_COMP(home);
 
-HAL_PIN(en);
-
 HAL_PIN(home_vel);
 HAL_PIN(home_acc);
-HAL_PIN(home_pos);
 
 HAL_PIN(pos_in);
 HAL_PIN(pos_out);
-HAL_PIN(vel_cmd);
+HAL_PIN(vel);
 
 HAL_PIN(home_in);
 HAL_PIN(home_polarity);
 
-HAL_PIN(limit_in0);
-HAL_PIN(limit_in1);
-HAL_PIN(limit_polarity);
-
-HAL_PIN(state);
-HAL_PIN(en_out);
-HAL_PIN(target);
+HAL_PIN(offset);
 HAL_PIN(home_offset);
 
-HAL_PIN(re_home);
-HAL_PIN(limit_fault);
+HAL_PIN(state);
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr){
+HAL_PIN(en_in);
+HAL_PIN(en_out);
+
+HAL_PIN(re_home);
+
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr){
   struct home_pin_ctx_t *pins = (struct home_pin_ctx_t *)pin_ptr;
   PIN(state) = 0;
   PIN(re_home) = 1;
   PIN(home_polarity) = 1.0;
+  PIN(home_vel) = 2.0 * M_PI;
+  PIN(home_acc) = 2.0 * M_PI / 0.1;
 }
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   //struct home_ctx_t *ctx      = (struct home_ctx_t *)ctx_ptr;
   struct home_pin_ctx_t *pins = (struct home_pin_ctx_t *)pin_ptr;
 
   float vel = 0.0;
 
-  if(PIN(en) <= 0.0){
+  if(PIN(en_in) <= 0.0){
     if(PIN(re_home) > 0.0){
       PIN(state) = 0;
     }
     PIN(en_out) = 0;
-    PIN(limit_fault) = 0;
   }
 
   switch((int)PIN(state)){
     case 0: // not homed
-      if(PIN(en) > 0.0){
+      if(PIN(en_in) > 0.0){
         PIN(state) = 1;
         PIN(en_out) = 0;
+        PIN(offset) = 0.0;
+        PIN(home_offset) = 0.0;
+        PIN(vel) = 0.0;
       }
     break;
 
@@ -75,9 +74,11 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
     case 2: // search home falling
       if(PIN(home_polarity) <= 0.0 && PIN(home_in) > 0.0){
         PIN(state) = 3;
+        PIN(home_offset) = PIN(offset);
       }
       else if(PIN(home_polarity) > 0.0 && PIN(home_in) <= 0.0){
         PIN(state) = 3;
+        PIN(home_offset) = PIN(offset);
       }
       else{
         vel = -PIN(home_vel);
@@ -85,47 +86,27 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
     break;
 
     case 3: // set home pos
-      PIN(target) = PIN(home_pos);
-      PIN(state) = 4;
-    break;
+      vel = SIGN(PIN(home_offset) - PIN(offset)) * sqrtf(ABS(PIN(home_offset) - PIN(offset)) * 2.0 * PIN(home_acc));
 
-
-    case 4: // go to home pos
-      vel = PIN(target) * 10;
-      if(ABS(PIN(target)) < 0.0001){
-        PIN(state) = 5;
+      if(ABS(PIN(offset) - PIN(home_offset)) < 0.0001 && PIN(vel) < PIN(home_vel) * 0.001){
+        PIN(state) = 4;
       }
     break;
 
-    case 5: // homed
+    case 4: // homed
       PIN(en_out) = 1;
-
-      if(PIN(limit_polarity) > 0.0 && (PIN(limit_in0) > 0.0 || PIN(limit_in1) > 0.0)){
-        PIN(state) = 6;
-        PIN(en_out) = 0;
-        PIN(limit_fault) = 1;
-      }
-      else if(PIN(limit_polarity) <= 0.0 && (PIN(limit_in0) <= 0.0 || PIN(limit_in1) <= 0.0)){
-        PIN(state) = 6;
-        PIN(en_out) = 0;
-        PIN(limit_fault) = 1;
-      }
-    break;
-
-    case 6: // fault
-      PIN(en_out) = 0;
-      PIN(limit_fault) = 1;
+      PIN(offset) = PIN(home_offset);
+      PIN(vel) = 0.0;
     break;
   }
 
   vel = LIMIT(vel, ABS(PIN(home_vel)));
-  PIN(vel_cmd) = CLAMP(vel, PIN(vel_cmd) - PIN(home_acc) * period, PIN(vel_cmd) + PIN(home_acc) * period);
+  PIN(vel) = CLAMP(vel, PIN(vel) - PIN(home_acc) * period, PIN(vel) + PIN(home_acc) * period);
 
-  PIN(target) -= PIN(vel_cmd) * period;
-  PIN(home_offset) += PIN(vel_cmd) * period;
-  PIN(pos_out) = PIN(pos_in) + PIN(home_offset);
-  PIN(pos_out) = mod(PIN(pos_out));
-  
+  PIN(offset) += PIN(vel) * period;
+  PIN(offset) = mod(PIN(offset));
+
+  PIN(pos_out) = mod(PIN(pos_in) + PIN(offset));
 }
 
 hal_comp_t home_comp_struct = {

shared/comps/idq.c

@@ -26,7 +26,7 @@ HAL_PIN(u);
 HAL_PIN(v);
 HAL_PIN(w);
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct idq_ctx_t * ctx = (struct idq_ctx_t *)ctx_ptr;
   struct idq_pin_ctx_t *pins = (struct idq_pin_ctx_t *)pin_ptr;
 

shared/comps/idx_home.c

@@ -19,7 +19,7 @@ struct idx_home_ctx_t {
   int waitabs;
 };
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct idx_home_ctx_t *ctx      = (struct idx_home_ctx_t *)ctx_ptr;
   struct idx_home_pin_ctx_t *pins = (struct idx_home_pin_ctx_t *)pin_ptr;
 

shared/comps/iit.c

@@ -22,7 +22,7 @@ struct iit_ctx_t {
   float e;
 };
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct iit_ctx_t *ctx      = (struct iit_ctx_t *)ctx_ptr;
   struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;
   PIN(amb_temp) = 30.0;
@@ -33,7 +33,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
   ctx->e = 0.0;
 }
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct iit_ctx_t *ctx      = (struct iit_ctx_t *)ctx_ptr;
   struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;
 

shared/comps/iit_old.c

@@ -27,7 +27,7 @@ struct iit_ctx_t {
   float mot_temp;
 };
 
-static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct iit_ctx_t *ctx      = (struct iit_ctx_t *)ctx_ptr;
   struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;
   ctx->mot_temp              = 25.0;
@@ -49,7 +49,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
   PIN(flow)                  = 0.0;
 }
 
-static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct iit_ctx_t *ctx      = (struct iit_ctx_t *)ctx_ptr;
   struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;