From d39df2d19914e75d7fdbd032af4ec09dd0cf0152 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Sat, 31 Oct 2020 14:58:34 -0700
Subject: [PATCH] heater pid

---
 firmware/Makefile           |  1 +
 firmware/heater_control.cpp | 13 ++++++-------
 firmware/pid.cpp            | 13 +++++++++++++
 firmware/pid.h              | 22 ++++++++++++++++++++++
 firmware/wideband_config.h  |  7 +++++++
 5 files changed, 49 insertions(+), 7 deletions(-)
 create mode 100644 firmware/pid.cpp
 create mode 100644 firmware/pid.h

diff --git a/firmware/Makefile b/firmware/Makefile
index c3821f9..500a153 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -128,6 +128,7 @@ CPPSRC = $(ALLCPPSRC) \
           pump_dac.cpp \
           sampling.cpp \
           heater_control.cpp \
+          pid.cpp \
           main.cpp
 
 # List ASM source files here.
diff --git a/firmware/heater_control.cpp b/firmware/heater_control.cpp
index 4e54aeb..85eabb6 100644
--- a/firmware/heater_control.cpp
+++ b/firmware/heater_control.cpp
@@ -1,9 +1,11 @@
 #include "heater_control.h"
+#include "wideband_config.h"
 
 #include "ch.h"
 #include "hal.h"
 #include "pwm.h"
 #include "sampling.h"
+#include "pid.h"
 
 // 400khz / 1024 = 390hz PWM
 Pwm heaterPwm(PWMD1, 0, 400'000, 1024);
@@ -48,6 +50,8 @@ static HeaterState GetNextState(HeaterState state, float sensorEsr)
     return state;
 }
 
+static Pid heaterPid(0.1f, 0, HEATER_CONTROL_PERIOD);
+
 static float GetDutyForState(HeaterState state, float heaterEsr)
 {
     switch (state)
@@ -62,12 +66,7 @@ static float GetDutyForState(HeaterState state, float heaterEsr)
 
             return rampDuty;
         case HeaterState::ClosedLoop:
-        {
-            // do something more intelligent here
-            float error = (heaterEsr - 250) / 100;
-
-            return error * 1.0f;
-        }
+            return heaterPid.GetOutput(HEATER_TARGET_ESR, heaterEsr);
     }
 }
 
@@ -90,7 +89,7 @@ static void HeaterThread(void*)
         heaterPwm.SetDuty(duty);
 
         // Loop at ~20hz
-        chThdSleepMilliseconds(50);
+        chThdSleepMilliseconds(HEATER_CONTROL_PERIOD);
     }
 }
 
diff --git a/firmware/pid.cpp b/firmware/pid.cpp
new file mode 100644
index 0000000..ee95810
--- /dev/null
+++ b/firmware/pid.cpp
@@ -0,0 +1,13 @@
+#include "pid.h"
+
+float Pid::GetOutput(float setpoint, float observation)
+{
+    // TODO: is this backwards?
+    float error = setpoint - observation;
+
+    // Integrate error
+    m_integrator += error * m_period;
+
+    // Multiply by gains and sum
+    return m_kp * error + m_ki * m_integrator;
+}
diff --git a/firmware/pid.h b/firmware/pid.h
new file mode 100644
index 0000000..0c4d37b
--- /dev/null
+++ b/firmware/pid.h
@@ -0,0 +1,22 @@
+#pragma once
+
+class Pid
+{
+public:
+    Pid(float kP, float kI, int periodMs) 
+        : m_period(periodMs / 1000.0f)
+        , m_kp(kP)
+        , m_ki(kI)
+    {
+    }
+
+    float GetOutput(float setpoint, float observation);
+
+private:
+    const float m_period;
+    const float m_kp;
+    const float m_ki;
+
+    float m_lastError;
+    float m_integrator;
+};
diff --git a/firmware/wideband_config.h b/firmware/wideband_config.h
index cd65672..434b673 100644
--- a/firmware/wideband_config.h
+++ b/firmware/wideband_config.h
@@ -28,3 +28,10 @@
 // *******************************
 
 // todo
+
+
+// *******************************
+//    Heater controller config
+// *******************************
+#define HEATER_CONTROL_PERIOD 50
+#define HEATER_TARGET_ESR 300