2020-09-23 18:59:59 +00:00
|
|
|
#include "veltime_comp.h"
|
2019-12-11 09:22:44 +00:00
|
|
|
#include "commands.h"
|
|
|
|
|
#include "hal.h"
|
|
|
|
|
#include "math.h"
|
|
|
|
|
#include "defines.h"
|
|
|
|
|
#include "angle.h"
|
|
|
|
|
|
|
|
|
|
HAL_COMP(veltime);
|
|
|
|
|
|
|
|
|
|
HAL_PIN(pos);
|
|
|
|
|
HAL_PIN(old_pos);
|
|
|
|
|
HAL_PIN(timer);
|
|
|
|
|
HAL_PIN(vel);
|
|
|
|
|
HAL_PIN(vel_lp);
|
|
|
|
|
|
|
|
|
|
HAL_PIN(max_time);
|
|
|
|
|
HAL_PIN(lpf)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
|
|
|
// struct veltime_ctx_t *ctx = (struct veltime_ctx_t *)ctx_ptr;
|
|
|
|
|
struct veltime_pin_ctx_t *pins = (struct veltime_pin_ctx_t *)pin_ptr;
|
|
|
|
|
|
|
|
|
|
PIN(max_time) = 0.1;
|
|
|
|
|
|
|
|
|
|
PIN(lpf) = 100.0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
|
|
|
// struct veltime_ctx_t *ctx = (struct veltime_ctx_t *)ctx_ptr;
|
|
|
|
|
struct veltime_pin_ctx_t *pins = (struct veltime_pin_ctx_t *)pin_ptr;
|
|
|
|
|
|
|
|
|
|
PIN(timer) += period;
|
|
|
|
|
|
|
|
|
|
if(PIN(pos) != PIN(old_pos)){
|
|
|
|
|
PIN(vel) = minus(PIN(pos), PIN(old_pos)) / PIN(timer);
|
|
|
|
|
PIN(old_pos) = PIN(pos);
|
|
|
|
|
PIN(timer) = 0.0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if(PIN(timer) > PIN(max_time)){
|
|
|
|
|
PIN(timer) = PIN(max_time);
|
|
|
|
|
PIN(vel) = 0.0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
PIN(vel_lp) = PIN(vel) * LP_HZ(PIN(lpf)) + PIN(vel_lp) * (1.0 - LP_HZ(PIN(lpf)));
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hal_comp_t veltime_comp_struct = {
|
|
|
|
|
.name = "veltime",
|
|
|
|
|
.nrt = 0,
|
|
|
|
|
.rt = 0,
|
|
|
|
|
.frt = frt_func,
|
|
|
|
|
.nrt_init = nrt_init,
|
|
|
|
|
.rt_start = 0,
|
|
|
|
|
.frt_start = 0,
|
|
|
|
|
.rt_stop = 0,
|
|
|
|
|
.frt_stop = 0,
|
|
|
|
|
.ctx_size = 0,
|
|
|
|
|
.pin_count = sizeof(struct veltime_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
|
|
|
};
|
