1
0
Fork 0

pid: Use Thermostat's PID algo instead of IDSP

- Has lower steady state error upon testing with PID autotune
This commit is contained in:
linuswck 2024-02-15 11:00:53 +08:00
parent e9a396f001
commit ffa5f4e8ff
4 changed files with 94 additions and 88 deletions

25
Cargo.lock generated
View File

@ -100,6 +100,7 @@ checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9"
dependencies = [ dependencies = [
"bare-metal 0.2.5", "bare-metal 0.2.5",
"bitfield", "bitfield",
"critical-section",
"embedded-hal 0.2.7", "embedded-hal 0.2.7",
"volatile-register", "volatile-register",
] ]
@ -358,17 +359,6 @@ dependencies = [
"stable_deref_trait", "stable_deref_trait",
] ]
[[package]]
name = "idsp"
version = "0.14.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "8f255ee573949fb629362d10aa3abd0a97a7c4950a3b8890b435b8c7516cf38f"
dependencies = [
"num-complex 0.4.4",
"num-traits",
"serde",
]
[[package]] [[package]]
name = "ieee802_3_miim" name = "ieee802_3_miim"
version = "0.8.0" version = "0.8.0"
@ -396,7 +386,6 @@ dependencies = [
"cortex-m-rt", "cortex-m-rt",
"cortex-m-semihosting 0.5.0", "cortex-m-semihosting 0.5.0",
"fugit", "fugit",
"idsp",
"ieee802_3_miim", "ieee802_3_miim",
"log", "log",
"miniconf", "miniconf",
@ -513,7 +502,7 @@ version = "0.3.1"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
dependencies = [ dependencies = [
"num-complex 0.3.1", "num-complex",
"num-integer", "num-integer",
"num-iter", "num-iter",
"num-rational", "num-rational",
@ -529,16 +518,6 @@ dependencies = [
"num-traits", "num-traits",
] ]
[[package]]
name = "num-complex"
version = "0.4.4"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1ba157ca0885411de85d6ca030ba7e2a83a28636056c7c699b07c8b6f7383214"
dependencies = [
"num-traits",
"serde",
]
[[package]] [[package]]
name = "num-integer" name = "num-integer"
version = "0.1.45" version = "0.1.45"

View File

@ -15,7 +15,7 @@ default-target = "thumbv7em-none-eabihf"
[dependencies] [dependencies]
panic-halt = "0.2.0" panic-halt = "0.2.0"
cortex-m = "0.7.6" cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] }
cortex-m-rt = { version = "0.7.1", features = ["device"] } cortex-m-rt = { version = "0.7.1", features = ["device"] }
cortex-m-semihosting = "0.5.0" cortex-m-semihosting = "0.5.0"
log = "0.4.17" log = "0.4.17"
@ -33,7 +33,6 @@ usbd-serial = "0.1.1"
fugit = "0.3.6" fugit = "0.3.6"
rtt-target = { version = "0.3.1", features = ["cortex-m"] } rtt-target = { version = "0.3.1", features = ["cortex-m"] }
miniconf = "0.9.0" miniconf = "0.9.0"
idsp = "0.14.1"
serde = { version = "1.0.158", features = ["derive"], default-features = false } serde = { version = "1.0.158", features = ["derive"], default-features = false }
sfkv = "0.1" sfkv = "0.1"
bit_field = "0.10" bit_field = "0.10"

View File

@ -1,59 +1,76 @@
use miniconf::Tree;
use uom::si::{ use uom::si::{
electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, f64::{ electric_potential::volt, electrical_resistance::ohm, f64::{
ElectricCurrent, ElectricPotential, ElectricalResistance, ThermodynamicTemperature ElectricPotential, ElectricalResistance, ThermodynamicTemperature
}, thermodynamic_temperature::degree_celsius }, thermodynamic_temperature::degree_celsius
}; };
use crate::thermostat::{ use crate::thermostat::{
ad7172, ad7172,
steinhart_hart as sh, steinhart_hart as sh,
}; };
use idsp::iir::{Pid, Action, Biquad};
use crate::debug;
const R_INNER: f64 = 2.0 * 5100.0; const R_INNER: f64 = 2.0 * 5100.0;
const VREF_SENS: f64 = 3.3 / 2.0; const VREF_SENS: f64 = 3.3 / 2.0;
#[derive(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 { pub struct PidState {
adc_data: Option<u32>, adc_data: Option<u32>,
adc_calibration: ad7172::ChannelCalibration, adc_calibration: ad7172::ChannelCalibration,
pid_engaged: bool, pid_engaged: bool,
pid: Biquad<f32>,
pid_out_min: ElectricCurrent,
pid_out_max: ElectricCurrent,
xy: [f32; 4],
set_point: ThermodynamicTemperature, set_point: ThermodynamicTemperature,
settings: Pid<f32>,
sh: sh::Parameters, sh: sh::Parameters,
controller: Controller,
} }
impl Default for PidState { impl Default for PidState {
fn default() -> Self { fn default() -> Self {
const OUT_MIN: f32 = -1.0;
const OUT_MAX: f32 = 1.0;
let mut pid_settings = Pid::<f32>::default();
pid_settings
// Pid Update Period depends on the AD7172 Output Data Rate Set
.period(0.1)
.limit(Action::Kp, 10.0)
.limit(Action::Ki, 10.0)
.limit(Action::Kd, 10.0);
let mut pid: Biquad<f32> = pid_settings.build().unwrap().into();
pid.set_min(OUT_MIN);
pid.set_max(OUT_MAX);
PidState { PidState {
adc_data: None, adc_data: None,
adc_calibration: ad7172::ChannelCalibration::default(), adc_calibration: ad7172::ChannelCalibration::default(),
pid_engaged: false, pid_engaged: false,
pid: pid,
pid_out_min: ElectricCurrent::new::<ampere>(OUT_MIN as f64),
pid_out_max: ElectricCurrent::new::<ampere>(OUT_MAX as f64),
xy: [0.0; 4],
set_point: ThermodynamicTemperature::new::<degree_celsius>(0.0), set_point: ThermodynamicTemperature::new::<degree_celsius>(0.0),
settings: pid_settings,
sh: sh::Parameters::default(), sh: sh::Parameters::default(),
controller: Controller {
parameters: Parameters::default(),
u1 : 0.0,
x1 : 0.0,
x2 : 0.0,
y1 : 0.0,
},
} }
} }
} }
@ -76,16 +93,33 @@ impl PidState {
}; };
} }
/// Update PID state on ADC input, calculate new DAC output // 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<f64> { pub fn update_pid(&mut self) -> Option<f64> {
let input = (self.get_temperature()?.get::<degree_celsius>() as f32) - (self.set_point.get::<degree_celsius>() as f32); let input = self.get_temperature()?.get::<degree_celsius>();
let pid_output = self.pid.update(&mut self.xy, input); let setpoint = self.set_point.get::<degree_celsius>();
debug!("BiQuad Storage [x0, x1, y0, y1]: {:?}", self.xy); let mut output: f64 = self.controller.y1 - setpoint * f64::from(self.controller.parameters.ki)
Some(pid_output as f64) + input * 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 - 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() {
pub fn reset_pid_state(&mut self){ output = self.controller.parameters.output_max.into();
self.xy = [0.0; 4]; }
self.controller.x2 = self.controller.x1;
self.controller.x1 = input;
self.controller.u1 = setpoint;
self.controller.y1 = output;
Some(output)
} }
pub fn get_adc(&self) -> Option<ElectricPotential> { pub fn get_adc(&self) -> Option<ElectricPotential> {
@ -110,33 +144,28 @@ impl PidState {
pub fn set_pid_params(&mut self, param: PidSettings, val: f64){ pub fn set_pid_params(&mut self, param: PidSettings, val: f64){
match param { match param {
PidSettings::Kp => { PidSettings::Kp => {
self.settings.gain(Action::Kp, val as f32); self.controller.parameters.kp = val as f32;
} }
PidSettings::Ki => { PidSettings::Ki => {
self.settings.gain(Action::Ki, val as f32); self.controller.parameters.ki = val as f32;
} }
PidSettings::Kd => { PidSettings::Kd => {
self.settings.gain(Action::Kd, val as f32); self.controller.parameters.kd = val as f32;
} }
PidSettings::Min => { PidSettings::Min => {
self.pid_out_min = ElectricCurrent::new::<ampere>(val); self.controller.parameters.output_min = val as f32;
} }
PidSettings::Max => { PidSettings::Max => {
self.pid_out_max = ElectricCurrent::new::<ampere>(val); self.controller.parameters.output_max = val as f32;
} }
} }
self.update_pid_settings();
} }
pub fn set_pid_period(&mut self, period: f32){ pub fn reset_pid_state(&mut self){
self.settings.period(period); self.controller.u1 = 0.0;
self.pid = self.settings.build().unwrap().into(); self.controller.x1 = 0.0;
} self.controller.x2 = 0.0;
self.controller.y1 = 0.0;
pub fn update_pid_settings(&mut self){
self.pid = self.settings.build().unwrap().into();
self.pid.set_max(self.pid_out_max.get::<ampere>() as f32);
self.pid.set_min(self.pid_out_min.get::<ampere>() as f32);
} }
pub fn set_pid_setpoint(&mut self, temperature: ThermodynamicTemperature){ pub fn set_pid_setpoint(&mut self, temperature: ThermodynamicTemperature){
@ -171,8 +200,8 @@ impl PidState {
self.pid_engaged self.pid_engaged
} }
pub fn get_pid_settings(&mut self) -> Pid<f32> { pub fn get_pid_settings(&mut self) -> Parameters {
unimplemented!() self.controller.parameters
} }
pub fn get_sh(&mut self) -> sh::Parameters { pub fn get_sh(&mut self) -> sh::Parameters {

View File

@ -7,7 +7,6 @@ use crate::thermostat::ad7172;
use crate::thermostat::pid_state::{PidState, PidSettings}; use crate::thermostat::pid_state::{PidState, PidSettings};
use crate::thermostat::steinhart_hart; use crate::thermostat::steinhart_hart;
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
use idsp::iir::Pid;
use log::debug; use log::debug;
use uom::si::{ use uom::si::{
electric_current::ampere, electric_current::ampere,
@ -19,6 +18,8 @@ use uom::si::{
}; };
use miniconf::Tree; use miniconf::Tree;
use super::pid_state;
pub const R_SENSE: ElectricalResistance = ElectricalResistance { pub const R_SENSE: ElectricalResistance = ElectricalResistance {
dimension: PhantomData, dimension: PhantomData,
units: PhantomData, units: PhantomData,
@ -141,6 +142,7 @@ impl Thermostat{
} }
pub fn poll_adc_and_update_pid(&mut self) -> bool { pub fn poll_adc_and_update_pid(&mut self) -> bool {
let mut data_rdy = false;
self.ad7172.data_ready().unwrap().map(|_ch| { self.ad7172.data_ready().unwrap().map(|_ch| {
let data = self.ad7172.read_data().unwrap(); let data = self.ad7172.read_data().unwrap();
let state: &mut PidState = &mut self.pid_ctrl_ch0; let state: &mut PidState = &mut self.pid_ctrl_ch0;
@ -151,17 +153,14 @@ impl Thermostat{
Some(pid_output) => { Some(pid_output) => {
self.set_i(ElectricCurrent::new::<ampere>(pid_output)); self.set_i(ElectricCurrent::new::<ampere>(pid_output));
debug!("Temperature Set Point: {:?} degree", self.pid_ctrl_ch0.get_pid_setpoint().get::<degree_celsius>()); debug!("Temperature Set Point: {:?} degree", self.pid_ctrl_ch0.get_pid_setpoint().get::<degree_celsius>());
self.power_up();
} }
None => { } None => { }
} }
} else {
self.power_down();
} }
debug!("Temperature: {:?} degree", self.get_temperature().get::<degree_celsius>()); debug!("Temperature: {:?} degree", self.get_temperature().get::<degree_celsius>());
true data_rdy = true;
}); });
false data_rdy
} }
pub fn power_up(&mut self){ pub fn power_up(&mut self){
@ -296,7 +295,7 @@ impl Thermostat{
} }
} }
pub fn get_pid_settings(&mut self) -> Pid<f32> { pub fn get_pid_settings(&mut self) -> pid_state::Parameters {
self.pid_ctrl_ch0.get_pid_settings() self.pid_ctrl_ch0.get_pid_settings()
} }