mirror of https://github.com/rene-dev/stmbl.git
68 lines
1.6 KiB
C
68 lines
1.6 KiB
C
#include "uf2_comp.h"
|
|
#include "hal.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
HAL_COMP(uf);
|
|
|
|
HAL_PIN(u_n);
|
|
HAL_PIN(vel_n);
|
|
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);
|
|
|
|
HAL_PIN(load);
|
|
|
|
struct uf_ctx_t {
|
|
float cur;
|
|
};
|
|
|
|
static void nrt_init(void *ctx_ptr, 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(com_pos) = 0.0;
|
|
PIN(slip_n) = 0.01;
|
|
ctx->cur = 0.0;
|
|
}
|
|
|
|
static void rt_func(float period, void *ctx_ptr, 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) = 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);
|
|
PIN(load) = ctx->cur / MAX(PIN(cur_n), 0.1);
|
|
}
|
|
|
|
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),
|
|
};
|