Diff: PID.cpp

Revision:

1:fa0730bf53ef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Tue Aug 23 08:55:01 2022 +0000
@@ -0,0 +1,118 @@
+#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;
+}
+ 
\ No newline at end of file