uf

2026-09-28 15:49:03 +08:00 · 2018-10-04 21:13:01 +02:00
parent 5dd37a4bae
commit 591ab934a4
4 changed files with 164 additions and 1 deletions
@@ -81,7 +81,8 @@ COMPS += shared/comps/vel_int.c
 COMPS += shared/comps/linrev.c
 COMPS += shared/comps/psi.c
 COMPS += shared/comps/stp.c
-COMPS += shared/comps/uf.c
+#COMPS += shared/comps/uf.c
+COMPS += shared/comps/uf2.c
 COMPS += shared/comps/ramp.c
 COMPS += shared/comps/scale.c
 COMPS += shared/comps/idx_home.c
@@ -0,0 +1,36 @@
+link uf
+load avg
+avg0.rt_prio = 5
+conf0.r = 1.4
+conf0.l = 0.0013
+conf0.polecount = 1
+uf0.u_n = 155.0
+uf0.vel_n = 2513.0
+conf0.max_ac_cur = 20
+iit0.cur_boost = 1.5
+iit0.max_time = 60
+uf0.cur_n = 12
+
+avg0.mult = 105.0
+avg0.in = io0.in0
+uf0.vel_cmd = avg0.out
+
+fault0.fb_ready = 1
+fault0.cmd_ready = 1
+fault0.en = io0.ind1
+
+term0.wave0 = avg0.in
+term0.wave1 = avg0.out
+term0.wave2 = uf0.u_cmd
+term0.wave3 = hv0.abs_cur
+
+term0.gain0 = 1
+term0.gain1 = 0.05
+term0.gain2 = 0.5
+term0.gain3 = 10
+
+avg0.mult = 0.1047
+avg0.in = 0
+avg0.lpf = 0.1
+
+conf0.out_rev = 1
@@ -0,0 +1,61 @@
+load adc
+load fault
+load hv
+load sim
+load io
+load iit
+link conf
+load uf
+hv0.rt_prio = 0.9
+hv0.frt_prio = 1
+adc0.rt_prio = 1
+uf0.rt_prio = 6
+io0.rt_prio = 7
+fault0.rt_prio = 10
+iit0.rt_prio = 13
+sim0.rt_prio = 13
+term0.rt_prio = 16
+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
+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
+uf0.polecount = conf0.polecount
+uf0.scale = fault0.scale
+uf0.cur = hv0.abs_cur
+hv0.pos = uf0.com_pos
+hv0.d_cmd = uf0.u_cmd
+hv0.cmd_mode = 0
+hv0.phase_mode = 2
+fault0.dc_volt = hv0.dc_volt
+fault0.hv_temp = hv0.hv_temp
+fault0.dc_cur = hv0.dc_cur
+fault0.hv_error = hv0.fault
+hv0.en = fault0.en_out
+hv0.scale = fault0.scale
+io0.fan = fault0.hv_fan
+io0.fault = fault0.fault
+io0.state = fault0.state
+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
@@ -0,0 +1,65 @@
+#include "hal.h"
+#include "defines.h"
+#include "angle.h"
+
+HAL_COMP(uf);
+
+HAL_PIN(u_n);
+HAL_PIN(vel_n);
+HAL_PIN(u_min);
+HAL_PIN(polecount);
+HAL_PIN(cur_n);
+HAL_PIN(slip_n);
+
+
+HAL_PIN(scale);
+
+HAL_PIN(vel_cmd);
+HAL_PIN(cur);
+
+HAL_PIN(u_cmd);
+HAL_PIN(vel_cmd_out);
+HAL_PIN(com_pos);
+
+struct uf_ctx_t {
+  float cur;
+};
+
+static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  struct uf_ctx_t *ctx      = (struct uf_ctx_t *)ctx_ptr;
+  struct uf_pin_ctx_t *pins = (struct uf_pin_ctx_t *)pin_ptr;
+
+  PIN(u_n) = 220.0 * sqrtf(2.0) / 2.0;
+  PIN(vel_n) = 3000.0 / 60.0 * 2.0 * M_PI;
+  PIN(u_min) = PIN(u_n) * 0.01;
+  PIN(com_pos) = 0.0;
+  PIN(slip_n) = 0.01;
+  ctx->cur = 0.0;
+}
+
+static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  struct uf_ctx_t *ctx      = (struct uf_ctx_t *)ctx_ptr;
+  struct uf_pin_ctx_t *pins = (struct uf_pin_ctx_t *)pin_ptr;
+
+  ctx->cur = PIN(cur) * 0.01 + ctx->cur * 0.99;
+
+  float vel_cmd = PIN(vel_cmd) + ctx->cur / MAX(PIN(cur_n), 0.1) * PIN(slip_n) * PIN(vel_n);
+  PIN(vel_cmd_out) = vel_cmd;
+  PIN(u_cmd) = MAX(PIN(u_min), vel_cmd / MAX(PIN(vel_n), 0.1) * PIN(u_n)) * PIN(scale);
+  PIN(com_pos) = mod(PIN(com_pos) + vel_cmd * PIN(polecount) * period);
+}
+
+const hal_comp_t uf_comp_struct = {
+    .name      = "uf",
+    .nrt       = 0,
+    .rt        = rt_func,
+    .frt       = 0,
+    .nrt_init  = nrt_init,
+    .hw_init   = 0,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = sizeof(struct uf_ctx_t),
+    .pin_count = sizeof(struct uf_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};