From f1f5947f221bb2d31620d8a558f6d0df2560ee21 Mon Sep 17 00:00:00 2001
From: Zachary Sunforge <zachary.sunforge@bfpower.io>
Date: Tue, 4 Jun 2024 21:07:09 -0700
Subject: [PATCH] Removed implicit type conversions. Renamed some variables.

---
 src/control/pid.rs | 54 +++++++++++++++++++++-------------------------
 1 file changed, 25 insertions(+), 29 deletions(-)

diff --git a/src/control/pid.rs b/src/control/pid.rs
index ab17db2..ee80f24 100644
--- a/src/control/pid.rs
+++ b/src/control/pid.rs
@@ -109,18 +109,18 @@ pub struct Pid<T: Number> {
     /// Defines the overall output filter limit.
     pub output_limit: T,
     /// Proportional gain.
-    pub kp: T,
+    pub p_gain: T,
     /// Integral gain.
-    pub ki: T,
+    pub i_gain: T,
     /// Derivative gain.
-    pub kd: T,
+    pub d_gain: T,
     /// Limiter for the proportional term: `-p_limit <= P <= p_limit`.
     pub p_limit: T,
     /// Limiter for the integral term: `-i_limit <= I <= i_limit`.
     pub i_limit: T,
     /// Limiter for the derivative term: `-d_limit <= D <= d_limit`.
     pub d_limit: T,
-    /// Last calculated integral value if [Pid::ki] is used.
+    /// Last calculated integral value if [Pid::i_gain] is used.
     integral_term: T,
     /// Previously found measurement whilst using the [Pid::next_control_output] method.
     prev_measurement: Option<T>,
@@ -167,13 +167,13 @@ impl<T> Pid<T>
     /// - [Self::p()]: Proportional term setting
     /// - [Self::i()]: Integral term setting
     /// - [Self::d()]: Derivative term setting
-    pub fn new(setpoint: impl Into<T>, output_limit: impl Into<T>) -> Self {
+    pub fn new(setpoint: T, output_limit: T) -> Self {
         Self {
-            setpoint: setpoint.into(),
-            output_limit: output_limit.into(),
-            kp: T::zero(),
-            ki: T::zero(),
-            kd: T::zero(),
+            setpoint,
+            output_limit,
+            p_gain: T::zero(),
+            i_gain: T::zero(),
+            d_gain: T::zero(),
             p_limit: T::zero(),
             i_limit: T::zero(),
             d_limit: T::zero(),
@@ -183,44 +183,40 @@ impl<T> Pid<T>
     }
 
     /// Sets the [Self::p] term for this controller.
-    pub fn p(&mut self, gain: impl Into<T>, limit: impl Into<T>) -> &mut Self {
-        self.kp = gain.into();
-        self.p_limit = limit.into();
+    pub fn p(&mut self, gain: T, limit: T) -> &mut Self {
+        self.p_gain = gain;
+        self.p_limit = limit;
         self
     }
 
     /// Sets the [Self::i] term for this controller.
-    pub fn i(&mut self, gain: impl Into<T>, limit: impl Into<T>) -> &mut Self {
-        self.ki = gain.into();
-        self.i_limit = limit.into();
+    pub fn i(&mut self, gain: T, limit: T) -> &mut Self {
+        self.i_gain = gain;
+        self.i_limit = limit;
         self
     }
 
     /// Sets the [Self::d] term for this controller.
-    pub fn d(&mut self, gain: impl Into<T>, limit: impl Into<T>) -> &mut Self {
-        self.kd = gain.into();
-        self.d_limit = limit.into();
+    pub fn d(&mut self, gain: T, limit: T) -> &mut Self {
+        self.d_gain = gain;
+        self.d_limit = limit;
         self
     }
 
     /// Sets the [Pid::setpoint] to target for this controller.
-    pub fn setpoint(&mut self, setpoint: impl Into<T>) -> &mut Self {
-        self.setpoint = setpoint.into();
+    pub fn setpoint(&mut self, setpoint: T) -> &mut Self {
+        self.setpoint = setpoint;
         self
     }
 
     /// Given a new measurement, calculates the next [control output](ControlOutput).
-    ///
-    /// # Panics
-    ///
-    /// - If a setpoint has not been set via `update_setpoint()`.
     pub fn next_control_output(&mut self, measurement: T) -> ControlOutput<T> {
         // Calculate the error between the ideal setpoint and the current
         // measurement to compare against
         let error = self.setpoint - measurement;
 
         // Calculate the proportional term and limit to it's individual limit
-        let p_unbounded = error * self.kp;
+        let p_unbounded = error * self.p_gain;
         let p = apply_limit(self.p_limit, p_unbounded);
 
         // Mitigate output jumps when ki(t) != ki(t-1).
@@ -228,7 +224,7 @@ impl<T> Pid<T>
         // just the error (no ki), because we support ki changing dynamically,
         // we store the entire term so that we don't need to remember previous
         // ki values.
-        self.integral_term = self.integral_term + error * self.ki;
+        self.integral_term = self.integral_term + error * self.i_gain;
 
         // Mitigate integral windup: Don't want to keep building up error
         // beyond what i_limit will allow.
@@ -239,7 +235,7 @@ impl<T> Pid<T>
         let d_unbounded = -match self.prev_measurement.as_ref() {
             Some(prev_measurement) => measurement - *prev_measurement,
             None => T::zero(),
-        } * self.kd;
+        } * self.d_gain;
         self.prev_measurement = Some(measurement);
         let d = apply_limit(self.d_limit, d_unbounded);
 
@@ -253,7 +249,7 @@ impl<T> Pid<T>
             p,
             i: self.integral_term,
             d,
-            output: output,
+            output,
         }
     }