mirror of
https://github.com/rene-dev/stmbl.git
synced 2024-12-29 20:12:10 +00:00
95 lines
2.1 KiB
C
95 lines
2.1 KiB
C
#include "commands.h"
|
|
#include "hal.h"
|
|
#include "math.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
HAL_COMP(vel);
|
|
|
|
HAL_PIN(pos_in);
|
|
HAL_PIN(pos_out);
|
|
HAL_PIN(vel);
|
|
HAL_PIN(acc);
|
|
HAL_PIN(w);
|
|
HAL_PIN(d);
|
|
HAL_PIN(g);
|
|
HAL_PIN(h);
|
|
HAL_PIN(j);
|
|
HAL_PIN(lp);
|
|
HAL_PIN(torque);
|
|
HAL_PIN(vel_ff);
|
|
HAL_PIN(en);
|
|
HAL_PIN(pos_error);
|
|
|
|
struct vel_ctx_t {
|
|
float last_acc;
|
|
float acc_sum;
|
|
float vel_sum;
|
|
};
|
|
|
|
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
|
// struct vel_ctx_t * ctx = (struct vel_ctx_t *)ctx_ptr;
|
|
struct vel_pin_ctx_t *pins = (struct vel_pin_ctx_t *)pin_ptr;
|
|
|
|
PIN(w) = 1000.0;
|
|
PIN(d) = 0.9;
|
|
PIN(g) = 1.0;
|
|
PIN(h) = 1.0;
|
|
PIN(j) = 0.00001;
|
|
PIN(lp) = 50.0;
|
|
PIN(en) = 1.0;
|
|
}
|
|
|
|
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
|
struct vel_ctx_t *ctx = (struct vel_ctx_t *)ctx_ptr;
|
|
struct vel_pin_ctx_t *pins = (struct vel_pin_ctx_t *)pin_ptr;
|
|
|
|
if(PIN(en) == 0.0) {
|
|
ctx->vel_sum = PIN(pos_in);
|
|
ctx->acc_sum = 0.0;
|
|
}
|
|
float vel_ff = PIN(vel_ff) * PIN(h);
|
|
ctx->vel_sum += (ctx->acc_sum + vel_ff) * period; // ff
|
|
|
|
float pos_error = minus(PIN(pos_in), ctx->vel_sum);
|
|
float w = PIN(w);
|
|
float d = PIN(d);
|
|
float g = PIN(g);
|
|
float lp = LP_HZ(PIN(lp));
|
|
float j = MAX(PIN(j), 0.0000001);
|
|
float acc = g * PIN(torque) / j;
|
|
|
|
ctx->last_acc = acc * lp + (1.0 - lp) * ctx->last_acc;
|
|
|
|
float acc_ff = acc - ctx->last_acc;
|
|
|
|
acc_ff += pos_error * w * w;
|
|
|
|
ctx->acc_sum += acc_ff * period;
|
|
|
|
PIN(vel) = ctx->acc_sum + vel_ff;
|
|
PIN(acc) = acc_ff;
|
|
|
|
vel_ff = 2.0 * d * w * pos_error;
|
|
|
|
ctx->vel_sum += vel_ff * period;
|
|
ctx->vel_sum = mod(ctx->vel_sum);
|
|
|
|
PIN(pos_out) = ctx->vel_sum;
|
|
PIN(pos_error) = minus(PIN(pos_in), ctx->vel_sum);
|
|
}
|
|
|
|
hal_comp_t vel_comp_struct = {
|
|
.name = "vel",
|
|
.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 = sizeof(struct vel_ctx_t),
|
|
.pin_count = sizeof(struct vel_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
};
|