File content as of revision 1:ebb9cfc0cff5:

#include "PID_lib.h"
#include "mbed.h"

PID_lib::PID_lib(PinName direksi1, PinName direksi2, PinName pulseWidth, float rpm) : dir1(direksi1), dir2(direksi2), pwm(pulseWidth), rpm(rpm){
    rpm = rpm; 
    dir1 = 0;
    dir2 = 0;
    pwm = 0;
    
//    float kpA = 0.007770; 
//    float kiA = 0.003297; 
//    float kdA = 0.000015;
//
//    float kpB = 0.03134; 
//    float kiB = 0.003605; 
//    float kdB = 0.001002;
//
//    float kpC = 0.02040;  
//    float kiC = 0.003292; 
//    float kdC = 0.000015;
//
//    float kpD = 0.02031; 
//    float kiD = 0.002760; 
//    float kdD = 3.943e-04;
    
    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_pwm(float target, float kp_,float ki_,float kd_){
  tim = t;
  dt = tim - lastime;

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

  //error computing start          
  e = sp - 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(pwmLebih1);
  //motor end
  
  lastPid = pidPwm;
  pc.printf("%f\n", pwmLebih);
}