mirror of https://github.com/rene-dev/stmbl.git
98 lines
2.3 KiB
C
98 lines
2.3 KiB
C
#include "commands.h"
|
|
#include "hal.h"
|
|
#include "math.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
HAL_COMP(sim);
|
|
|
|
HAL_PIN(amp);
|
|
HAL_PIN(freq);
|
|
HAL_PIN(sin);
|
|
HAL_PIN(msin);
|
|
HAL_PIN(sin2);
|
|
HAL_PIN(msin2); //const vel, const max vel = amp
|
|
HAL_PIN(sin3);
|
|
HAL_PIN(msin3); //const max acc = amp
|
|
HAL_PIN(square);
|
|
HAL_PIN(vel);
|
|
HAL_PIN(res);
|
|
HAL_PIN(offset);
|
|
|
|
struct sim_ctx_t {
|
|
float time;
|
|
float amp;
|
|
float freq;
|
|
float vel;
|
|
};
|
|
|
|
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
|
// struct sim_ctx_t * ctx = (struct sim_ctx_t *)ctx_ptr;
|
|
struct sim_pin_ctx_t *pins = (struct sim_pin_ctx_t *)pin_ptr;
|
|
|
|
PIN(amp) = 3.1;
|
|
PIN(freq) = 1.0;
|
|
PIN(res) = 100000.0;
|
|
}
|
|
|
|
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
|
struct sim_ctx_t *ctx = (struct sim_ctx_t *)ctx_ptr;
|
|
struct sim_pin_ctx_t *pins = (struct sim_pin_ctx_t *)pin_ptr;
|
|
|
|
ctx->amp = PIN(amp) * 0.001 + ctx->amp * 0.999;
|
|
ctx->freq = PIN(freq) * 0.001 + ctx->freq * 0.999;
|
|
float sin;
|
|
float sin2;
|
|
float sin3;
|
|
float amp2;
|
|
float r = MAX(PIN(res), 1);
|
|
|
|
if(ABS(ctx->freq) > 0.01) {
|
|
amp2 = 1 / (ctx->freq * 2.0 * M_PI);
|
|
} else {
|
|
amp2 = 0;
|
|
}
|
|
|
|
ctx->time += period;
|
|
|
|
if(ABS(ctx->freq * ctx->time) >= 1.0 && ABS(ctx->freq) > 0.0) { // TODO fix
|
|
ctx->time -= 1.0 / ABS(ctx->freq);
|
|
}
|
|
|
|
float co = 0.0;
|
|
float si = 0.0;
|
|
sincos_fast(ctx->freq * ctx->time * 2.0 * M_PI, &si, &co);
|
|
|
|
sin = ctx->amp * si;
|
|
sin2 = sin * amp2;
|
|
sin3 = sin2 * amp2;
|
|
|
|
float s = sin;
|
|
float o = PIN(offset);
|
|
ctx->vel += ctx->freq * 2.0 * M_PI * period;
|
|
ctx->vel = mod(ctx->vel);
|
|
|
|
PIN(sin) = s + o;
|
|
PIN(sin2) = sin2 + o;
|
|
PIN(sin3) = sin3 + o;
|
|
PIN(msin) = ((int)(mod(s + o) * r)) / r;
|
|
PIN(msin2) = ((int)(mod(sin2 + o) * r)) / r;
|
|
PIN(msin3) = ((int)(mod(sin3 + o) * r)) / r;
|
|
PIN(square) = (sin > 0.0) ? (ctx->amp + o) : (-ctx->amp + o);
|
|
PIN(vel) = ctx->vel; //mod(((int)(vel * r)) / r + o);
|
|
}
|
|
|
|
hal_comp_t sim_comp_struct = {
|
|
.name = "sim",
|
|
.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 sim_ctx_t),
|
|
.pin_count = sizeof(struct sim_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
};
|