File content as of revision 1:fa0730bf53ef:

#include "PID.h"
 
extern Serial bt;
 
float PID::getP(){
    return Kp_;
}
void PID::setP(float Kp){
    Kp_ = Kp;
}
float PID::getI(){
    return Ki_;
}
void PID::setI(float Ki){
    Ki_ = Ki;
}
float PID::getD(){
    return Kd_;
}
void PID::setD(float Kd){
    Kd_ = Kd;
}
 
PID::PID(float Kp, float Ki, float Kd, float interval)
{
    Kp_=Kp;
    Ki_=Ki;
    Kd_=Kd;
    interval_=interval;
    
    err_ = 0;
    accErr_ = 0;
    prevCV_ = 0;
    
    prevVelocity_=0; // speed control
 
    TV_ = 0;
    
    Scale_ = PID_SCALE;           // pulses / rotation
}
 
void PID::ReadCurrentValue(int CurrentValue) // current Encoder Value
{
    CV_ = CurrentValue;
}
 
void PID::SetTargetValue(int targetValue)
{
    TV_ = targetValue;
}

float PID::ReadCurrentVelocity(void)
{
    float CurrentVelocity_ = (CV_ - prevCV_) / interval_ / PID_SCALE;
    
    return CurrentVelocity_;
}
 
float PID::Performance_Speed(void)
{
    float CurrentVelocity_ = (CV_ - prevCV_) / interval_ / PID_SCALE;
 
    err_= TV_ - CurrentVelocity_;
    accErr_ += err_;

/*  // wrong code    
    float slope = (CurrentVelocity_ - prevVelocity_) / interval_;
    controllerOutput_ = (Kp_ * err_) + (Ki_ * accErr_) + (Kd_ * slope);
 */
    
    controllerOutput_ = (Kp_ * err_) + (Ki_ * interval_ * accErr_) + (Kd_ * (ScaledCV_ - ScaledPrevCV_) / interval_);    
    
    
    prevCV_ = CV_;
    
    prevVelocity_ = CurrentVelocity_;
    
    return controllerOutput_;
}
 
float PID::Performance_Location(void)
{
    ScaledCV_ = CV_ / Scale_;
    ScaledTV_ = TV_ / Scale_;
    ScaledPrevCV_ = prevCV_ / Scale_;


    err_ = ScaledTV_ - ScaledCV_;
    accErr_ += err_;
    
/* // wrong code 
    float slope = (ScaledCV_ - ScaledPrevCV_) / interval_;
    controllerOutput_ = (Kp_ * err_) + (Ki_ * accErr_) + (Kd_ * slope);     // missed the sampling time in Ki terms
*/
        
    controllerOutput_ = (Kp_ * err_) + (Ki_ * interval_ * accErr_) + (Kd_ * (ScaledCV_ - ScaledPrevCV_) / interval_);
    
    prevCV_ = CV_;
    
    return controllerOutput_;
}
 
void PID::Reset(void)
{
    err_ = 0;
    accErr_ = 0;
    prevCV_ = 0;
    
    prevVelocity_=0; // speed control
 
    TV_ = 0;
}

void PID::ResetError(void)
{
    accErr_ = 0;
}