use miniconf::Tree; use serde::{Deserialize, Serialize}; use uom::si::{ electric_potential::volt, electrical_resistance::ohm, f32::{ ElectricPotential, ElectricalResistance, ThermodynamicTemperature }, thermodynamic_temperature::degree_celsius }; use crate::thermostat::{ ad7172, steinhart_hart as sh, }; const R_INNER: f32 = 2.0 * 5100.0; const VREF_SENS: f32 = 3.3 / 2.0; #[derive(Deserialize, Serialize, Clone, Copy, Debug, PartialEq, Tree)] pub struct Parameters { /// Gain coefficient for proportional term pub kp: f32, /// Gain coefficient for integral term pub ki: f32, /// Gain coefficient for derivative term pub kd: f32, /// Output limit minimum pub output_min: f32, /// Output limit maximum pub output_max: f32, } impl Default for Parameters { fn default() -> Self { Parameters { kp: 0.0, ki: 0.0, kd: 0.0, output_min: -1.0, output_max: 1.0, } } } #[derive(Clone)] pub struct Controller { pub parameters: Parameters, u1 : f64, x1 : f64, x2 : f64, pub y1 : f64, } pub struct PidState { adc_data: Option, adc_calibration: ad7172::ChannelCalibration, pid_engaged: bool, set_point: ThermodynamicTemperature, sh: sh::Parameters, controller: Controller, } impl Default for PidState { fn default() -> Self { PidState { adc_data: None, adc_calibration: ad7172::ChannelCalibration::default(), pid_engaged: false, set_point: ThermodynamicTemperature::new::(0.0), sh: sh::Parameters::default(), controller: Controller { parameters: Parameters::default(), u1 : 0.0, x1 : 0.0, x2 : 0.0, y1 : 0.0, }, } } } pub enum PidSettings { Kp, Ki, Kd, Min, Max, } impl PidState { pub fn update(&mut self, adc_data: u32) { self.adc_data = if adc_data == ad7172::MAX_VALUE { // this means there is no thermistor plugged into the ADC. None } else { Some(adc_data) }; } // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation // Input x(t), target u(t), output y(t) // y0' = y1 - ki * u0 // + x0 * (kp + ki + kd) // - x1 * (kp + 2kd) // + x2 * kd // + kp * (u0 - u1) // y0 = clip(y0', ymin, ymax) pub fn update_pid(&mut self) -> Option { let input = self.get_temperature()?.get::(); let setpoint = self.set_point.get::(); let mut output: f64 = self.controller.y1 - setpoint as f64 * f64::from(self.controller.parameters.ki) + input as f64 * f64::from(self.controller.parameters.kp + self.controller.parameters.ki + self.controller.parameters.kd) - self.controller.x1 * f64::from(self.controller.parameters.kp + 2.0 * self.controller.parameters.kd) + self.controller.x2 * f64::from(self.controller.parameters.kd) + f64::from(self.controller.parameters.kp) * (setpoint as f64 - self.controller.u1); if output < self.controller.parameters.output_min.into() { output = self.controller.parameters.output_min.into(); } if output > self.controller.parameters.output_max.into() { output = self.controller.parameters.output_max.into(); } self.controller.x2 = self.controller.x1; self.controller.x1 = input as f64; self.controller.u1 = setpoint as f64; self.controller.y1 = output; Some(output) } pub fn get_adc(&self) -> Option { Some(self.adc_calibration.convert_data(self.adc_data?)) } /// Get `SENS[01]` input resistance pub fn get_sens(&self) -> Option { let r_inner = ElectricalResistance::new::(R_INNER); let vref = ElectricPotential::new::(VREF_SENS); let adc_input = self.get_adc()?; let r = r_inner * adc_input / (vref - adc_input); Some(r) } pub fn get_temperature(&self) -> Option { let r = self.get_sens()?; let temperature = self.sh.get_temperature(r); Some(temperature) } pub fn apply_pid_params(&mut self, pid_params: Parameters){ self.controller.parameters = pid_params; } pub fn set_pid_params(&mut self, param: PidSettings, val: f32){ match param { PidSettings::Kp => { self.controller.parameters.kp = val; } PidSettings::Ki => { self.controller.parameters.ki = val; } PidSettings::Kd => { self.controller.parameters.kd = val; } PidSettings::Min => { self.controller.parameters.output_min = val; } PidSettings::Max => { self.controller.parameters.output_max = val; } } } pub fn reset_pid_state(&mut self){ self.controller.u1 = 0.0; self.controller.x1 = 0.0; self.controller.x2 = 0.0; self.controller.y1 = 0.0; } pub fn set_pid_setpoint(&mut self, temperature: ThermodynamicTemperature){ self.set_point = temperature; } pub fn get_pid_setpoint(&mut self) -> ThermodynamicTemperature { self.set_point } pub fn set_sh_t0(&mut self, t0: ThermodynamicTemperature){ self.sh.t0 = t0 } pub fn set_sh_r0(&mut self, r0: ElectricalResistance){ self.sh.r0 = r0 } pub fn set_sh_beta(&mut self, beta: f32){ self.sh.b = beta } pub fn set_adc_calibration(&mut self, adc_cal: ad7172::ChannelCalibration){ self.adc_calibration = adc_cal; } pub fn set_pid_engaged(&mut self, pid_engaged: bool){ self.pid_engaged = pid_engaged; } pub fn get_pid_engaged(&mut self) -> bool { self.pid_engaged } pub fn get_pid_settings(&mut self) -> Parameters { self.controller.parameters } pub fn get_sh(&mut self) -> sh::Parameters { self.sh } pub fn apply_sh(&mut self, sh: sh::Parameters) { self.sh = sh; } }