stmbl

#include "../hal.h"
COMP(auto);

HAL_PIN(pwm_in) = 0.0;
HAL_PIN(pwm_out) = 0.0;
HAL_PIN(mag_pos_out) = 0.0;

HAL_PIN(start) = 0.0;
HAL_PIN(ready) = 0.0;

HAL_PIN(offset) = 0.0;

HAL_PIN(fb_in) = 0.0;
HAL_PIN(fb_d_in) = 0.0;

HAL_PIN(pole_count) = 0.0;

HAL_PIN(freq) = 200.0;
HAL_PIN(time) = 0.1;
HAL_PIN(scale) = 0.9;
HAL_PIN(real) = 0.0;
HAL_PIN(amp) = 0.9;
HAL_PIN(phase) = 0.0;
HAL_PIN(imag) = 0.0;
HAL_PIN(state) = 0.0;

MEM(float mpos) = 0.0;
MEM(float q0) = 0.0;
MEM(float q1) = 0.0;
MEM(float q2) = 0.0;
MEM(float w) = 0.0;
MEM(float sine) = 0.0;
MEM(float cosine) = 0.0;
MEM(float coeff) = 0.0;
MEM(int N) = 100;
MEM(int k) = 0;
MEM(int i) = 0;
MEM(float f) = 0.0;



MEM(float startpos) = 0.0;
MEM(float cov) = 0.0;
MEM(float covs) = 0.0;
MEM(float covc) = 0.0;
MEM(float pwm) = 0.0;
MEM(float offset_) = 0.0;
MEM(float offset1_) = DEG(90);


RT_CALC(
  float fb = PIN(fb_in);
  float fb_d = PIN(fb_d_in);
  float re;
  float im;
  //offset_ = 0.0;//PIN(offset);

  HT(
    GOTO(0);
    STATE(0){
      offset1_ = mod(DEG(180.0) + offset1_);

      SLEEP((0.5 / PIN(freq)));
    }
  );

  HT(
    GOTO(0);
    STATE(0){
      offset_ = mod(offset_ += DEG(10));

      f = PIN(freq);
      k = (int)(0.5 + N * f * period);
      w = 2.0 * M_PI * k / N;
      sine = sinf(w);
      cosine = cosf(w);
      coeff = 2.0 * cosine;

      q0 = 0.0;
      q1 = 0.0;
      q2 = 0.0;
      i = 0;

      SLEEP(0.025);
    }
  );

  HT(
    GOTO(0);
    STATE(0){
      i++;
      q0 = coeff * q1 - q2 + fb_d;
      q2 = q1;
      q1 = q0;
      if(i == N){
        re = (q1 - q2 * cosine) * 2.0;
        im = (q2 * sine) * 2.0;
        PIN(real) = re;
        PIN(imag) = im;

        PIN(amp) = re * re + im * im;
        PIN(phase) = atan2f(im, re);
      }
    }
  );


  mpos = mod(fb * PIN(pole_count) + offset_ + offset1_);
  pwm = PIN(scale);



  PIN(offset) = offset_;
  PIN(pwm_out) = pwm;
  PIN(mag_pos_out) = mpos;
);


ENDCOMP;