stmbl

HAL_COMP(vel);

HAL_PIN(pos_in) = 0.0;
HAL_PIN(pos_out) = 0.0;
HAL_PIN(vel) = 0.0;
HAL_PIN(acc) = 0.0;
HAL_PIN(w) = 1000.0;
HAL_PIN(d) = 0.9;
HAL_PIN(g) = 1.0;
HAL_PIN(h) = 1.0;
HAL_PIN(j) = 0.00001;
HAL_PIN(lp) = 100.0;
HAL_PIN(torque) = 0.0;
HAL_PIN(vel_ff) = 0.0;
HAL_PIN(en) = 1.0;
HAL_PIN(pos_error) = 0.0;

MEM(float last_acc) = 0.0;
MEM(float acc_sum) = 0.0;
MEM(float vel_sum) = 0.0;

RT(
   if(PIN(en) == 0.0){
      vel_sum = PIN(pos_in);
      acc_sum = 0.0;
   }
   float vel_ff = PIN(vel_ff) * PIN(h);
   vel_sum += (acc_sum + vel_ff) * period; // ff
   
   float pos_error = minus(PIN(pos_in), vel_sum);
   float w = PIN(w);
   float d = PIN(d);
   float g = PIN(g);
   float lp = LP_HZ(PIN(lp));
   float j = MAX(PIN(j), 0.0000001);
   float acc = g * PIN(torque) / j;

   last_acc = acc * lp + (1.0 - lp) * last_acc;

   float acc_ff = acc - last_acc;

   acc_ff += pos_error * w * w;

   acc_sum += acc_ff * period;

   PIN(vel) = acc_sum + vel_ff;
   PIN(acc) = acc_ff;

   vel_ff = 2.0 * d * w * pos_error;
   
   vel_sum += vel_ff * period;
   vel_sum = mod(vel_sum);

   PIN(pos_out) = vel_sum;
   PIN(pos_error) = minus(PIN(pos_in), vel_sum);
);

ENDCOMP;