mirror of https://github.com/rene-dev/stmbl.git
61 lines
1.3 KiB
C
61 lines
1.3 KiB
C
#include "comps/wobl_comp.h"
|
|
#include "commands.h"
|
|
#include "hal.h"
|
|
#include "math.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
HAL_COMP(wobl);
|
|
|
|
HAL_PIN(home_pos);
|
|
HAL_PIN(amp);
|
|
HAL_PIN(freq);
|
|
HAL_PIN(freq_diff);
|
|
HAL_PIN(duty);
|
|
HAL_PIN(en);
|
|
|
|
HAL_PIN(pos);
|
|
HAL_PIN(out);
|
|
HAL_PIN(out_freq);
|
|
|
|
struct wobl_ctx_t {
|
|
float ang;
|
|
float ang2;
|
|
};
|
|
|
|
static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
struct wobl_ctx_t *ctx = (struct wobl_ctx_t *)ctx_ptr;
|
|
struct wobl_pin_ctx_t *pins = (struct wobl_pin_ctx_t *)pin_ptr;
|
|
|
|
|
|
if(PIN(en) <= 0.0){
|
|
ctx->ang = 0.0;
|
|
ctx->ang2 = 0.0;
|
|
}
|
|
else{
|
|
ctx->ang += period * PIN(freq) * 2.0 * M_PI;
|
|
ctx->ang = mod(ctx->ang);
|
|
ctx->ang2 += period * (PIN(freq) + PIN(freq_diff)) * 2.0 * M_PI;
|
|
ctx->ang2 = mod(ctx->ang2);
|
|
}
|
|
|
|
PIN(pos) = PIN(home_pos) + sinf(ctx->ang) * PIN(amp);
|
|
PIN(out_freq) = PIN(freq) + PIN(freq_diff);
|
|
PIN(out) = ((sinf(ctx->ang2) / 2 - 0.5 + PIN(duty)) > 0.0) ? 1.0 : 0.0;
|
|
|
|
}
|
|
|
|
hal_comp_t wobl_comp_struct = {
|
|
.name = "wobl",
|
|
.nrt = 0,
|
|
.rt = 0,
|
|
.frt = frt_func,
|
|
.nrt_init = 0,
|
|
.rt_start = 0,
|
|
.frt_start = 0,
|
|
.rt_stop = 0,
|
|
.frt_stop = 0,
|
|
.ctx_size = sizeof(struct wobl_ctx_t),
|
|
.pin_count = sizeof(struct wobl_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
};
|