diff --git a/firmware/Makefile b/firmware/Makefile
index c3821f9..9a34b65 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -128,6 +128,8 @@ CPPSRC = $(ALLCPPSRC) \
           pump_dac.cpp \
           sampling.cpp \
           heater_control.cpp \
+          pid.cpp \
+          pump_control.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/main.cpp b/firmware/main.cpp
index 5770c50..17747a4 100644
--- a/firmware/main.cpp
+++ b/firmware/main.cpp
@@ -4,6 +4,7 @@
 
 #include "can.h"
 #include "heater_control.h"
+#include "pump_control.h"
 #include "pump_dac.h"
 #include "sampling.h"
 
@@ -32,15 +33,16 @@ int main() {
     halInit();
     chSysInit();
 
+    // Fire up all of our threads
     StartSampling();
-
     InitPumpDac();
-
-    InitCan();
+    StartHeaterControl();
+    StartPumpControl();
 
     uartStart(&UARTD1, &uartCfg);
 
-    StartHeaterControl();
+
+    InitCan();
 
     /*for (int i = 0; i < 500; i++) {
         SetPumpCurrentTarget(current);
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/pump_control.cpp b/firmware/pump_control.cpp
new file mode 100644
index 0000000..513dab8
--- /dev/null
+++ b/firmware/pump_control.cpp
@@ -0,0 +1,36 @@
+#include "pump_control.h"
+#include "wideband_config.h"
+#include "heater_control.h"
+#include "sampling.h"
+#include "pump_dac.h"
+#include "pid.h"
+
+#include "ch.h"
+
+static Pid pumpPid(0, 0.01f, 2);
+
+static THD_WORKING_AREA(waPumpThread, 256);
+static void PumpThread(void*)
+{
+    while(true)
+    {
+        // Only actuate pump when running closed loop!
+        if (IsRunningClosedLoop())
+        {
+            float nernstVoltage = GetNernstDc();
+
+            float result = pumpPid.GetOutput(NERNST_TARGET, nernstVoltage);
+
+            // result is in mA
+            SetPumpCurrentTarget(result / 1000.0f);
+
+            // Run at 500hz
+            chThdSleepMilliseconds(2);
+        }
+    }
+}
+
+void StartPumpControl()
+{
+    chThdCreateStatic(waPumpThread, sizeof(waPumpThread), NORMALPRIO + 4, PumpThread, nullptr);
+}
diff --git a/firmware/pump_control.h b/firmware/pump_control.h
new file mode 100644
index 0000000..91670fc
--- /dev/null
+++ b/firmware/pump_control.h
@@ -0,0 +1,3 @@
+#pragma once
+
+void StartPumpControl();
diff --git a/firmware/wideband_config.h b/firmware/wideband_config.h
index cd65672..1750602 100644
--- a/firmware/wideband_config.h
+++ b/firmware/wideband_config.h
@@ -24,7 +24,12 @@
 #define LSU_SENSE_R (61.9f)
 
 // *******************************
-//          Pump driver
+//        Pump controller
 // *******************************
+#define NERNST_TARGET (0.45f)
 
-// todo
+// *******************************
+//    Heater controller config
+// *******************************
+#define HEATER_CONTROL_PERIOD 50
+#define HEATER_TARGET_ESR 300