From 1ad821299b395b3b14eba0e9278e6f44d9638266 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Wed, 18 Mar 2020 21:51:30 +0100
Subject: [PATCH] build mods pid + steinhard_hart

---
 src/main.rs           |   2 +
 src/pid.rs            | 122 ++++++++++++++++++++++++++++++++++++++++++
 src/steinhart_hart.rs |  30 +++++++++++
 3 files changed, 154 insertions(+)
 create mode 100644 src/pid.rs
 create mode 100644 src/steinhart_hart.rs

diff --git a/src/main.rs b/src/main.rs
index 428ea8b..326655b 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -38,6 +38,8 @@ use session::{CHANNELS, Session, SessionOutput};
 mod command_parser;
 use command_parser::{Command, ShowCommand, PwmSetup, PwmMode};
 mod timer;
+mod pid;
+mod steinhart_hart;
 
 
 #[derive(Clone, Copy, Debug)]
diff --git a/src/pid.rs b/src/pid.rs
new file mode 100644
index 0000000..ecf858e
--- /dev/null
+++ b/src/pid.rs
@@ -0,0 +1,122 @@
+#[derive(Clone, Copy)]
+pub struct Parameters {
+    pub kp: f32,
+    pub ki: f32,
+    pub kd: f32,
+    pub output_min: f32,
+    pub output_max: f32,
+    pub integral_min: f32,
+    pub integral_max: f32
+}
+
+#[derive(Clone)]
+pub struct Controller {
+    parameters: Parameters,
+    target: f32,
+    integral: f32,
+    last_input: Option<f32>
+}
+
+impl Controller {
+    pub const fn new(parameters: Parameters) -> Controller {
+        Controller {
+            parameters: parameters,
+            target: 0.0,
+            last_input: None,
+            integral: 0.0
+        }
+    }
+
+    pub fn update(&mut self, input: f32) -> f32 {
+        let error = self.target - input;
+
+        let p = self.parameters.kp * error;
+
+        self.integral += error;
+        if self.integral < self.parameters.integral_min {
+            self.integral = self.parameters.integral_min;
+        }
+        if self.integral > self.parameters.integral_max {
+            self.integral = self.parameters.integral_max;
+        }
+        let i = self.parameters.ki * self.integral;
+
+        let d = match self.last_input {
+            None => 0.0,
+            Some(last_input) => self.parameters.kd * (last_input - input)
+        };
+        self.last_input = Some(input);
+
+        let mut output = p + i + d;
+        if output < self.parameters.output_min {
+            output = self.parameters.output_min;
+        }
+        if output > self.parameters.output_max {
+            output = self.parameters.output_max;
+        }
+        output
+    }
+
+    pub fn get_target(&self) -> f32 {
+        self.target
+    }
+
+    pub fn set_target(&mut self, target: f32) {
+        self.target = target;
+    }
+
+    pub fn get_parameters(&self) -> &Parameters {
+        &self.parameters
+    }
+
+    pub fn update_parameters<F: FnOnce(&mut Parameters)>(&mut self, f: F) {
+        f(&mut self.parameters);
+    }
+
+    #[allow(dead_code)]
+    pub fn reset(&mut self) {
+        self.integral = 0.0;
+        self.last_input = None;
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    const PARAMETERS: Parameters = Parameters {
+        kp: 0.055,
+        ki: 0.005,
+        kd: 0.04,
+        output_min: -10.0,
+        output_max: 10.0,
+        integral_min: -100.0,
+        integral_max: 100.0,
+    };
+
+    #[test]
+    fn test_controller() {
+        const DEFAULT: f32 = 0.0;
+        const TARGET: f32 = 1234.56;
+        const ERROR: f32 = 0.01;
+        const DELAY: usize = 10;
+
+        let mut pid = Controller::new(PARAMETERS.clone());
+        pid.set_target(TARGET);
+
+        let mut values = [DEFAULT; DELAY];
+        let mut t = 0;
+        let mut total_t = 0;
+        let target = (TARGET - ERROR)..=(TARGET + ERROR);
+        while !values.iter().all(|value| target.contains(value)) {
+            let next_t = (t + 1) % DELAY;
+            // Feed the oldest temperature
+            let output = pid.update(values[next_t]);
+            // Overwrite oldest with previous temperature + output
+            values[next_t] = values[t] + output;
+            t = next_t;
+            total_t += 1;
+        }
+        dbg!(values[t], total_t);
+    }
+}
diff --git a/src/steinhart_hart.rs b/src/steinhart_hart.rs
new file mode 100644
index 0000000..a585775
--- /dev/null
+++ b/src/steinhart_hart.rs
@@ -0,0 +1,30 @@
+use lexical_core::Float;
+
+/// Steinhart-Hart equation parameters
+#[derive(Clone, Debug)]
+pub struct Parameters {
+    pub a: f32,
+    pub b: f32,
+    pub c: f32,
+    /// Parallel resistance
+    ///
+    /// Not truly part of the equation but required to calculate
+    /// resistance from voltage.
+    pub parallel_r: f32,
+}
+
+impl Parameters {
+    /// Perform the voltage to temperature conversion.
+    ///
+    /// Result unit: Kelvin
+    ///
+    /// TODO: verify
+    pub fn get_temperature(&self, voltage: f32) -> f32 {
+        let r = self.parallel_r * voltage;
+        let ln_r = r.abs().ln();
+        let inv_temp = self.a +
+            self.b * ln_r +
+            self.c * ln_r * ln_r * ln_r;
+        1.0 / inv_temp
+    }
+}