stmbl

HAL_COMP(vel_int);

HAL_PIN(pos_in) = 0.0;
HAL_PIN(pos_out) = 0.0;

HAL_PIN(vel_in) = 0.0;
HAL_PIN(vel_out) = 0.0;

HAL_PIN(wd) = 0.002;
HAL_PIN(error) = 0.0;

MEM(float pos) = 0.0;
MEM(float counter) = 0.0;

RT(
   float p = PIN(pos_in);
   float v = PIN(vel_in);

   if(counter > PIN(wd) && v != 0.0){
      v = 0;
      PIN(error) = 1.0;
   }
   else{
      PIN(error) = 0.0;
      counter += period;
   }
   
   if(EDGE(p)){
      counter = 0.0;
      pos = p;
   }
   else{
      pos += v * period;
   }
   
   PIN(pos_out) = pos;
   PIN(vel_out) = v;
);

ENDCOMP;