diff --git a/Makefile b/Makefile index bc165b2b..2c92e818 100644 --- a/Makefile +++ b/Makefile @@ -81,6 +81,7 @@ COMPS += shared/comps/uf.c COMPS += shared/comps/ramp.c COMPS += shared/comps/scale.c COMPS += shared/comps/idx_home.c +COMPS += shared/comps/move.c SOURCES += $(COMPS) diff --git a/conf/dual_dc_sserial.txt b/conf/dual_dc_sserial.txt new file mode 100644 index 00000000..3d75a509 --- /dev/null +++ b/conf/dual_dc_sserial.txt @@ -0,0 +1,17 @@ +link pid +link sserial +load move +load move +move0.rt_prio = 20 +move1.rt_prio = 20 +move0.scale = sserial0.pos_cmd_d +move1.scale = sserial0.pos_cmd_d +move0.fwd = sserial0.out0 +move0.rev = sserial0.out1 +move1.fwd = sserial0.out2 +move1.rev = sserial0.out3 +hv0.phase_mode = 0 +hv0.cmd_mode = 0 +hv0.d_cmd = move0.out +hv0.q_cmd = move1.out +hv0.en = 1 diff --git a/shared/comps/move.c b/shared/comps/move.c new file mode 100644 index 00000000..224d17d1 --- /dev/null +++ b/shared/comps/move.c @@ -0,0 +1,31 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(move); + +HAL_PIN(fwd); +HAL_PIN(rev); +HAL_PIN(scale); +HAL_PIN(out); + +static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { + struct move_pin_ctx_t *pins = (struct move_pin_ctx_t *)pin_ptr; + PIN(out) = PIN(scale) * PIN(fwd) + PIN(scale) * PIN(rev) * -1; +} + +hal_comp_t move_comp_struct = { + .name = "move", + .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 move_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +};