stmbl

HAL_COMP(curpid);

HAL_PIN(id_cmd) = 0.0;
HAL_PIN(iq_cmd) = 0.0;

HAL_PIN(id_fb) = 0.0;
HAL_PIN(iq_fb) = 0.0;

HAL_PIN(indd_fb) = 0.0;
HAL_PIN(indq_fb) = 0.0;

HAL_PIN(ud) = 0.0;
HAL_PIN(uq) = 0.0;

HAL_PIN(ac_volt) = 0.0;
HAL_PIN(dc_volt) = 0.0;
HAL_PIN(max_cur) = 0.0;

HAL_PIN(scale) = 0.0;

HAL_PIN(rd) = 0.0;
HAL_PIN(rq) = 0.0;
HAL_PIN(ld) = 0.0;
HAL_PIN(lq) = 0.0;

HAL_PIN(ff) = 0.0;
HAL_PIN(kp) = 0.0;
HAL_PIN(ki) = 0.0;
HAL_PIN(kind) = 0.0;

HAL_PIN(id_error) = 0.0;
HAL_PIN(iq_error) = 0.0;

HAL_PIN(res) = PWM_RES;
HAL_PIN(volt_res) = 0.0;

MEM(float id_error_sum) = 0.0;
MEM(float iq_error_sum) = 0.0;

RT(
   float rd = MAX(PIN(rd), 0.1);
   float rq = MAX(PIN(rq), 0.1);
   float ld = MAX(PIN(ld), 0.001);
   float lq = MAX(PIN(lq), 0.001);
   float volt = MAX(PIN(ac_volt) , 0.1);
   float volt_res = PIN(dc_volt) / MAX(PIN(res), 1.0);

   float ff = PIN(ff);
   float kpd = ld * PIN(kp) / period;
   float kpq = lq * PIN(kp) / period;
   float kid = rd * PIN(ki) / ld;
   float kiq = rq * PIN(ki) / lq;
   float kind = PIN(kind);

   //TODO curpid: sqrt(di^2+qi^2) auf max_ac_cur clampen
   float idc = LIMIT(PIN(id_cmd), PIN(max_cur) * PIN(scale));
   float iqc = LIMIT(PIN(iq_cmd), PIN(max_cur) * PIN(scale));

   float id = PIN(id_fb);
   float iq = PIN(iq_fb);

   float indd = PIN(indd_fb);
   float indq = PIN(indq_fb);

   float id_error = (idc - id);
   float iq_error = (iqc - iq);

   if(kpd * kid > 0.0 && kpq * kiq > 0.0 ){
      id_error_sum = LIMIT(id_error_sum + id_error, volt / kpd / kid);
      iq_error_sum = LIMIT(iq_error_sum + iq_error, volt / kpq / kiq);
   }
   else{
      id_error_sum = 0.0;
      iq_error_sum = 0.0;
   }

   float ud = CLAMP(ff * rd * idc + kpd * id_error + kpd * kid * id_error_sum + kind * indd, -volt, volt);
   float uq = CLAMP(ff * rq * iqc + kpq * iq_error + kpq * kiq * iq_error_sum + kind * indq, -volt, volt);

   // ud = ((int)(ud / volt_res)) * volt_res;
   // uq = ((int)(uq / volt_res)) * volt_res;

   PIN(ud) = ud;
   PIN(uq) = uq;
   PIN(volt_res) = volt_res;

   PIN(id_error) = id_error;
   PIN(iq_error) = iq_error;
);

ENDCOMP;