File content as of revision 2:77ef3d60d8d9:

#include "PID_lib.h"


PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth) : dir1(direksi1), dir2(direksi2), pwm(pulseWidth){
    rpm = rpm; 
    dir1 = 0;
    dir2 = 0;
    pwm = 0;
    
    float ppr = 540.0f;
    float phi = 3.14285714;
    
    t.start();
    t.reset();
}



// kp ki kd untuk sp 50 tuning manual lagi stelah tuning matlab



void PID_lib::pid_pwm(float target, float kp_,float ki_,float kd_, float rpm){
  tim = t;
  dt = tim - lastime;

  rpmFilt = 0.03046875*rpm + 0.03046875*rpmn1 + 0.93906251*rpmFiltn1;//10hz filter 

  //error computing start          
  e = target - rpmFilt;
  eI += e;
  eD = e - laste;
  //error computing end
  
  //storing error value start
  hP = e*kp_;
  hI = eI*ki_*dt;
  hD = eD*kd_/dt;
  //storing error value end
  
  //saturasu ki start
  if(hI < 0.5 && hI > 0){
    setI = hI;
  }else if(hI > 0.5){
    setI = 0.5;
  }
  else if(hI < 0 && hI > -0.5){
    setI = hI;
  }else if(hI < -0.5){
    setI = -0.5;
  }
  //saturasi ki end
  
  pidPwm = hP+setI+hD;//pwm pid
  lastime = tim;//update timer
  laste = e;//update error
  
  rpmFiltn1 = rpmFilt;rpmn1 = rpm;//update filter
  //motor direksi start
  if(pidPwm < 0){dir1 = 1; dir2 = 0;}
  else{dir1 = 0; dir2 = 1;}
  //motor direksi end
  
  //pwm saturasi start
  if(pidPwm > 1){
     pwmLebih = 1;
  }else if(pidPwm < 1 && pidPwm > 0){
     pwmLebih = fabs(pidPwm);
  }else if(pidPwm < -1){
     pwmLebih = -1;
  }else if(pidPwm > -1 && pidPwm < 0){
     pwmLebih = fabs(pidPwm);
  }
  //pwm saturasi end
  
  //motor start
  pwm = fabs(pwmLebih);
  //motor end
  
  lastPid = pidPwm;
  printf("%f\n", pwmLebih);
}