clamp pid

firmware/heater_control.cpp

@@ -73,7 +73,7 @@ static HeaterState GetNextState(HeaterState state, float sensorEsr)
     return state;
 }
 
-static Pid heaterPid(0.01f, 0.05f, HEATER_CONTROL_PERIOD);
+static Pid heaterPid(0.01f, 0.05f, 1, HEATER_CONTROL_PERIOD);
 
 static float GetDutyForState(HeaterState state, float heaterEsr)
 {

firmware/pid.cpp

@@ -5,8 +5,12 @@ float Pid::GetOutput(float setpoint, float observation)
     float error = setpoint - observation;
 
     // Integrate error
-    m_integrator += error * m_period;
+    m_integrator += error * m_period * m_ki;
+
+    // Clamp to +- 1
+    if (m_integrator > m_clamp) m_integrator = m_clamp;
+    if (m_integrator < -m_clamp) m_integrator = -m_clamp;
 
     // Multiply by gains and sum
-    return m_kp * error + m_ki * m_integrator;
+    return m_kp * error + m_integrator;
 }

firmware/pid.h

@@ -3,10 +3,11 @@
 class Pid
 {
 public:
-    Pid(float kP, float kI, int periodMs) 
+    Pid(float kP, float kI, float clamp, int periodMs) 
         : m_period(periodMs / 1000.0f)
         , m_kp(kP)
         , m_ki(kI)
+        , m_clamp(clamp)
     {
     }
 
@@ -16,6 +17,7 @@ private:
     const float m_period;
     const float m_kp;
     const float m_ki;
+    const float m_clamp;
 
     float m_lastError;
     float m_integrator;

firmware/pump_control.cpp

@@ -8,7 +8,7 @@
 #include "ch.h"
 
 // Bosch CJ125 is somewhere VERY ROUGHLY like 200-400A/(v*s) integrator gain
-static Pid pumpPid(50.0f, 5000.0f, 2);
+static Pid pumpPid(50.0f, 5000.0f, 10, 2);
 
 static THD_WORKING_AREA(waPumpThread, 256);
 static void PumpThread(void*)