stmbl

COMP(pmsm_limits);

// motor values
HAL_PIN(psi) = 0.01;
HAL_PIN(r) = 1.0;
HAL_PIN(ld) = 0.001;
HAL_PIN(lq) = 0.001;
HAL_PIN(polecount) = 1.0;

// sys limit
HAL_PIN(ac_volt) = 0.0;

// next min max out -> pid, curpid
HAL_PIN(next_max_cur) = 0.0;
HAL_PIN(next_max_torque) = 0.0;
HAL_PIN(next_min_cur) = 0.0;
HAL_PIN(next_min_torque) = 0.0;

// min max out @ current vel
HAL_PIN(max_cur) = 0.0;
HAL_PIN(max_torque) = 0.0;
HAL_PIN(min_cur) = 0.0;
HAL_PIN(min_torque) = 0.0;

// abs max out
HAL_PIN(abs_max_cur) = 0.0;
HAL_PIN(abs_max_torque) = 0.0;
HAL_PIN(abs_max_vel) = 0.0;

// pmsm feedback
HAL_PIN(iq) = 0.0;
HAL_PIN(indq) = 0.0;

RT(
   float p = (int)MAX(PIN(polecount), 1.0);
   //float ld = MAX(PIN(ld), 0.0001);
   float lq = MAX(PIN(lq), 0.0001);
   float psi_m = MAX(PIN(psi), 0.01);
   float r = MAX(PIN(r), 0.001);
   float iq = PIN(iq);

   float indq = PIN(indq);
   float psid = PIN(psi);

   float volt = PIN(ac_volt);
   float abs_max_cur = volt / r;
   float abs_max_vel = volt / psid / p;
   float abs_max_torque = 3.0 / 2.0 * p * psi_m * abs_max_cur;

   float next_max_cur = iq + (volt - r * iq - indq) / lq * period;
   float next_min_cur = iq + (-volt - r * iq - indq) / lq * period;
   float next_max_torque = 3.0 / 2.0 * p * (psi_m * next_max_cur);
   float next_min_torque = 3.0 / 2.0 * p * (psi_m * next_min_cur);

   float max_cur = (volt - indq) / r;
   float min_cur = (-volt - indq) / r;
   float max_torque = 3.0 / 2.0 * p * (psi_m * max_cur);
   float min_torque = 3.0 / 2.0 * p * (psi_m * min_cur);


   PIN(abs_max_cur) = abs_max_cur;
   PIN(abs_max_vel) = abs_max_vel;
   PIN(abs_max_torque) = abs_max_torque;
   PIN(next_max_cur) = next_max_cur;
   PIN(next_min_cur) = next_min_cur;
   PIN(next_max_torque) = next_max_torque;
   PIN(next_min_torque) = next_min_torque;
   PIN(max_cur) = max_cur;
   PIN(min_cur) = min_cur;
   PIN(max_torque) = max_torque;
   PIN(min_torque) = min_torque;
);

ENDCOMP;