From 8ae27725c6fb70e75baa728ff9ac548209e46d82 Mon Sep 17 00:00:00 2001
From: linuswck <linuswck@m-labs.hk>
Date: Thu, 11 Jan 2024 12:34:07 +0800
Subject: [PATCH] Port PID Controller from thermostat firmware - Add serde
 Cargo

---
 Cargo.lock     |   1 +
 Cargo.toml     |   1 +
 src/pid/mod.rs |   1 +
 src/pid/pid.rs | 142 +++++++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 145 insertions(+)
 create mode 100644 src/pid/mod.rs
 create mode 100644 src/pid/pid.rs

diff --git a/Cargo.lock b/Cargo.lock
index 088b34c..ff8521d 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -315,6 +315,7 @@ dependencies = [
  "num-traits",
  "panic-halt",
  "rtt-target",
+ "serde",
  "smoltcp",
  "stm32-eth",
  "stm32f4xx-hal",
diff --git a/Cargo.toml b/Cargo.toml
index f334a91..4f276b6 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -32,6 +32,7 @@ usbd-serial = "0.1.1"
 fugit = "0.3.6"
 rtt-target = { version = "0.3.1", features = ["cortex-m"] }
 miniconf = "0.6.3"
+serde = { version = "1.0.158", features = ["derive"], default-features = false }
 
 [features]
 semihosting = ["cortex-m-log/semihosting"]
diff --git a/src/pid/mod.rs b/src/pid/mod.rs
new file mode 100644
index 0000000..e319c78
--- /dev/null
+++ b/src/pid/mod.rs
@@ -0,0 +1 @@
+pub mod pid;
\ No newline at end of file
diff --git a/src/pid/pid.rs b/src/pid/pid.rs
new file mode 100644
index 0000000..e491dc5
--- /dev/null
+++ b/src/pid/pid.rs
@@ -0,0 +1,142 @@
+#[macro_use]
+use miniconf::Miniconf;
+use miniconf::serde::{Serialize, Deserialize};
+
+#[derive(Clone, Debug, Serialize, Deserialize)]
+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: -2.0,
+            output_max: 2.0,
+        }
+    }
+}
+
+#[derive(Clone)]
+pub struct Controller {
+    pub parameters: Parameters,
+    pub target : f64,
+    u1 : f64,
+    x1 : f64,
+    x2 : f64,
+    pub y1 : f64,
+}
+
+impl Controller {
+    pub const fn new(parameters: Parameters) -> Controller {
+        Controller {
+            parameters: parameters,
+            target : 0.0,
+            u1 : 0.0,
+            x1 : 0.0,
+            x2 : 0.0,
+            y1 : 0.0,
+        }
+    }
+
+    // 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(&mut self, input: f64) -> f64 {
+        
+        let mut output: f64 = self.y1 - self.target * f64::from(self.parameters.ki)
+                            + input * f64::from(self.parameters.kp + self.parameters.ki + self.parameters.kd)
+                            - self.x1 * f64::from(self.parameters.kp + 2.0 * self.parameters.kd)
+                            + self.x2 * f64::from(self.parameters.kd)
+                            + f64::from(self.parameters.kp) * (self.target - self.u1);
+        if output < self.parameters.output_min.into() {
+            output = self.parameters.output_min.into();
+        }
+        if output > self.parameters.output_max.into() {
+            output = self.parameters.output_max.into();
+        }
+        self.x2 = self.x1;
+        self.x1 = input;
+        self.u1 = self.target;
+        self.y1 = output;        
+        output
+    }
+
+    pub fn summary(&self) -> Summary {
+        Summary {
+            parameters: self.parameters.clone(),
+            target: self.target,
+        }
+    }
+}
+
+#[derive(Clone, Debug, Miniconf)]
+pub struct Summary {
+    parameters: Parameters,
+    target: f64,
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    const PARAMETERS: Parameters = Parameters {
+        kp: 0.03,
+        ki: 0.002,
+        kd: 0.15,
+        output_min: -10.0,
+        output_max: 10.0,
+    };
+
+    #[test]
+    fn test_controller() {
+        // Initial and ambient temperature
+        const DEFAULT: f64 = 20.0;    
+        // Target temperature  
+        const TARGET: f64 = 40.0;   
+        // Control tolerance    
+        const ERROR: f64 = 0.01;    
+        // System response delay    
+        const DELAY: usize = 10;  
+        // Heat lost
+        const LOSS: f64 = 0.05;    
+        // Limit simulation cycle, reaching this limit before settling fails test      
+        const CYCLE_LIMIT: u32 = 1000;  
+
+        let mut pid = Controller::new(PARAMETERS.clone());
+        pid.target = TARGET;
+
+        let mut values = [DEFAULT; DELAY];
+        let mut t = 0;
+        let mut total_t = 0;
+        let mut output: f64 = 0.0;
+        let target = (TARGET - ERROR)..=(TARGET + ERROR);
+        while !values.iter().all(|value| target.contains(value)) && total_t < CYCLE_LIMIT {
+            let next_t = (t + 1) % DELAY;
+            // Feed the oldest temperature
+            output = pid.update(values[next_t]);
+            // Overwrite oldest with previous temperature - output
+            values[next_t] = values[t] - output - (values[t] - DEFAULT) * LOSS;
+            t = next_t;
+            total_t += 1;
+            println!("{}", values[t].to_string());
+        }
+        assert_ne!(CYCLE_LIMIT, total_t);
+    }
+}