From d39df2d19914e75d7fdbd032af4ec09dd0cf0152 Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Sat, 31 Oct 2020 14:58:34 -0700 Subject: [PATCH 1/2] 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 From 3fdf60b9569da12328c029c79383f28c439b43c8 Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Sat, 31 Oct 2020 16:59:35 -0700 Subject: [PATCH 2/2] pump controller --- firmware/Makefile | 1 + firmware/main.cpp | 10 ++++++---- firmware/pump_control.cpp | 36 ++++++++++++++++++++++++++++++++++++ firmware/pump_control.h | 3 +++ firmware/wideband_config.h | 6 ++---- 5 files changed, 48 insertions(+), 8 deletions(-) create mode 100644 firmware/pump_control.cpp create mode 100644 firmware/pump_control.h diff --git a/firmware/Makefile b/firmware/Makefile index 500a153..9a34b65 100644 --- a/firmware/Makefile +++ b/firmware/Makefile @@ -129,6 +129,7 @@ CPPSRC = $(ALLCPPSRC) \ sampling.cpp \ heater_control.cpp \ pid.cpp \ + pump_control.cpp \ main.cpp # List ASM source files here. diff --git a/firmware/main.cpp b/firmware/main.cpp index 866bff1..65597de 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/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 434b673..1750602 100644 --- a/firmware/wideband_config.h +++ b/firmware/wideband_config.h @@ -24,11 +24,9 @@ #define LSU_SENSE_R (61.9f) // ******************************* -// Pump driver +// Pump controller // ******************************* - -// todo - +#define NERNST_TARGET (0.45f) // ******************************* // Heater controller config