stmbl

COMP(ap);

HAL_PIN(fb_in) = 0.0;
HAL_PIN(fb_out) = 0.0;
HAL_PIN(pwm_in) = 0.0;
HAL_PIN(pwm_out) = 0.0;
HAL_PIN(mag_pos_out) = 0.0;
HAL_PIN(fb_offset_in) = 0.0;
HAL_PIN(fb_offset_out) = 0.0;
HAL_PIN(start) = 0.0;
HAL_PIN(ready) = 0.0;
HAL_PIN(min_step) = 0.01;
HAL_PIN(max_error) = 0.02;
HAL_PIN(max_time) = 0.1;
HAL_PIN(pole_count) = 1.0;
HAL_PIN(err) = 0.0;



MEM(int state) = 0;
MEM(float fb_offset) = 0;
MEM(int count) = 0;
MEM(float time) = 0.0;
MEM(float min_s) = 0.0;
//MEM(float max_e) = 0.0;
MEM(float max_t) = 0.0;
MEM(float pos) = 0.0;


RT_CALC(
  float start_ = PIN(start);
  float ready_ = 0.0;
  float magp = 0.0;
  float pwm = 0.0;
  float fb = PIN(fb_in);
  float pc = PIN(pole_count);
  float e = 0.0;

  switch(state){
    case 0: // standby
      magp = mod((mod(fb * pc) + fb_offset) + DEG(90));
      pwm = PIN(pwm_in);
      if(start_ == 1.0){
        ready_ = 0.0;
        min_s = PIN(min_step);
        //max_e = PIN(max_error);
        max_t = PIN(max_time);
        fb_offset = PIN(fb_offset_in);
        pos = fb;
        state = 1;
        count = 0;
        time = 0.0;
      }
      break;
    case 1: // out const pwm vec
      pwm = 0.2;

      e = minus(fb, pos);
      time += period;

      if(time >= max_t){
        if(count == 0){
          fb_offset += DEG(90);
          time = 0.0;
        }
        else{
          state = 2;
          pwm = 0.0;
        }
      }

      if(ABS(e) >= min_s){
        count++;
        time = 0.0;
        if(e > 0.0){
          fb_offset -= DEG(1);// / count;
        }
        else{
          fb_offset += DEG(1);// / count;
        }
      }

      //if(DEG(90) / count <= max_e){
      if(count > 200){
        state = 2;
        pwm = 0.0;
      }

      magp = (mod(pos * pc) + fb_offset);
      break;
    case 2: // running
      ready_ = 1.0;
      magp = mod((mod(fb * pc) + fb_offset) + DEG(90));
      pwm = PIN(pwm_in);
      if(start_ == 0.0){
        state = 0;
      }
      break;
    default:
      ready_ = 1.0;
  }

  PIN(err) = e;
  PIN(pwm_out) = pwm;
  PIN(mag_pos_out) = magp;
  PIN(ready) = ready_;
  PIN(fb_offset_out) = fb_offset;
  PIN(fb_out) = mod(fb + fb_offset / pc);
);


ENDCOMP;