Diff: PID.cpp

Revision:

2:c8ab3e8d4c51

Parent:

1:4705d8930670

Child:

3:c690a8974246

--- a/PID.cpp	Sun Apr 07 13:52:44 2019 +0000
+++ b/PID.cpp	Sun Apr 14 14:03:46 2019 +0000
@@ -1,78 +1,112 @@
 #include "PID.h"
 #include "mbed.h"
 
-PID::PID(Timer *_timer){
-  integral = 0;
-  prev_hensa = 0;
-  nowtime = 0;
-  prev_time = 0;
-  lateD = 0;
-  timer = _timer;
-}
+PID::PID(double* _Input, double* _Output, double* _Setpoint, double _Kp, double _Ki, double _Kd, PID_Mode _mode, Timer *_timer)
+{
+
+    Input = _Input;
+    Output = _Output;
+    Setpoint = _Setpoint;
 
-float PID::control(float target, float nowrpm){
-  //timer->start();
-  nowtime =timer->read();
-  float hensa = target - nowrpm;
-  float dt = 1000000 / (nowtime - prev_time);
-  integral += (hensa + prev_hensa) / 2 * dt;
-  float differential = (hensa - prev_hensa) / dt;
-  float sousaryou = Kp*hensa + Ki*integral + Kd*differential;
-  prev_hensa = hensa;
-  prev_time =timer->read();
-  return sousaryou;
-}
-
-float PID::PI_lateD(float target, float nowrpm){
-  nowtime = (*timer).read();
-  float hensa = target - nowrpm;
-  float dt = nowtime - prev_time;
-  integral += (hensa + prev_hensa) / 2 *dt;
-  float sousaryou = Kp*hensa + Ki*integral + Kd*lateD;
-  prev_hensa = hensa;
-  prev_time = timer->read();
-  lateD = (hensa - prev_hensa) / dt;
-  return sousaryou;
+    timer = _timer;
+    
+    SampleTime = 100;
+    lastTime = timer->read_ms();
+    setOutputLimits(-1,1);
+    setParameter_pid(_Kp,_Ki,_Kd);
+    setPIDMode(_mode);
+    Initialize();
 }
 
-float PID::control_P(float target, float nowrpm, float new_Kp){
-  float hensa = target - nowrpm;
-  float sousaryou = new_Kp*hensa;
-  return sousaryou;
-}
-
-float PID::control_PI(float target, float nowrpm){
-  Kp = 0.45 * Ku;
-  Ti = 0.83 * Pu;
-  Ki = (1 / Ti) * Kp;
-  nowtime = timer->read();
-  float hensa = target - nowrpm;
-  float dt = nowtime -prev_time;
-  integral += (hensa + prev_hensa) / 2 * dt;
-  float sousaryou = Kp*hensa + Ki*integral;
-  prev_hensa = hensa;
-  prev_time = timer->read();
-  return sousaryou;
+void PID::control()
+{
+    //timer->start();
+    double nowtime =timer->read_ms();
+    double input = *Input;
+    
+    if((nowtime - lastTime) > SampleTime)
+    {    
+        double error = *Setpoint - input;
+        double dInput = input - lastInput;
+        double derror = error - lastError;
+        outputSum += (Ki*error);
+        double output = 0;
+        
+        if(outputSum > outMax) outputSum= outMax;
+        else if(outputSum < outMin) outputSum= outMin;
+        
+        switch(mode)
+        {
+            case pos_PID:
+                output = outputSum + Kp*error + Kd*derror;
+                break; 
+                
+            case P_D:
+                output = Kp*error - Kd*dInput;
+                break;
+            
+            case PI_D:
+                output = outputSum + Kp*error - Kd*dInput;
+                break;
+            
+            case I_PD:
+                output = outputSum - Kp*input - Kd*dInput;
+                break;
+                
+        }
+        
+        *Output = output;
+        lastTime = nowtime;
+        lastInput = input;
+    }
 }
 
-void PID::setParameter_pid(float new_Kp, float new_Ki, float new_Kd){
-  Kp = new_Kp;
-  Ki = new_Ki;
-  Kd = new_Kd;
+void PID::setParameter_pid(double new_Kp, double new_Ki, double new_Kd)
+{
+    Kp = new_Kp;
+    Ki = new_Ki;
+    Kd = new_Kd;
+}
+
+void PID::setParameter_KuPu(double new_Ku, double new_Pu)
+{
+    Ku = new_Ku;
+    Pu = new_Pu;
+    Kp = 0.60 * Ku;
+    Ti = 0.50 * Pu;
+    Td = 0.125 * Pu;
+    Ki = (1 / Ti) * Kp;
+    Kd = Td * Kp;
+}
+
+void PID::setPIDMode(PID_Mode _mode)
+{
+    mode = _mode;
+    Initialize();
 }
 
-void PID::setParameter_KuPu(float new_Ku, float new_Pu){
-  Ku = new_Ku;
-  Pu = new_Pu;
-  Kp = 0.60 * Ku;
-  Ti = 0.50 * Pu;
-  Td = 0.125 * Pu;
-  Ki = (1 / Ti) * Kp;
-  Kd = Td * Kp;
+void PID::setOutputLimits(double Min, double Max)
+{
+    if(Min >= Max) return;
+    outMin = Min;
+    outMax = Max;
+
+    if(*Output > outMax) *Output = outMax;
+    else if(*Output < outMin) *Output = outMin;
+    if(outputSum > outMax) outputSum = outMax;
+    else if(outputSum < outMin) outputSum = outMin;
 }
 
-void PID::reset(float target){
-  integral = 0;
-  prev_hensa = target;
-  prev_time = timer->read();
+void PID::reset(double target)
+{
+    outputSum = 0;
 }
+
+void PID::Initialize()
+{
+    outputSum = *Output;
+    lastInput = *Input;
+    lastError = *Setpoint - lastInput;
+    if(outputSum > outMax) outputSum = outMax;
+    else if(outputSum < outMin) outputSum = outMin;
+}
\ No newline at end of file