Added in the Pid and Motor Code

Pid was added to rnavp, since it can be used in alot of different things.

Cargo.lock

@@ -158,6 +158,11 @@ version = "0.1.0"
 [[package]]
 name = "rnavp_logic"
 version = "0.1.0"
+dependencies = [
+ "rnavp",
+ "serde",
+ "uom",
+]
 
 [[package]]
 name = "rustc_version"

Cargo.toml

@@ -1,4 +1,4 @@
 [workspace]
 members = ["rnavp"
-, "rnavp_communication", "rnavp_logic"]
+, "rnavp_communication", "rnavp_controller"]
 resolver = "3"

rnavp/src/lib.rs

@@ -1,4 +1,5 @@
 mod direction;
 pub mod error;
 pub mod motor;
+pub mod pid;
 pub mod positional;

rnavp/src/pid.rs

@@ -0,0 +1,129 @@
+use serde::{Deserialize, Serialize};
+
+///The Config for the PID system used by the controller
+#[derive(Serialize, Deserialize, Debug, Default, Clone, Copy, PartialEq)]
+pub struct Config {
+    ///The Proportional value for the controller
+    pub kp: f32,
+    ///The Integral value for the controller
+    pub ki: f32,
+    ///The Derivative value for the controller
+    pub kd: f32,
+    ///The Feedforward value for the controller
+    pub ff: f32,
+    ///The delta time Value used for the controller
+    pub dt: f32,
+    ///The max accel change from the controller, 0 means no limit
+    pub accel: f32,
+}
+
+///A PID, the config field is used to handle all the needed configuration.
+/// This PID using pid_step() then attempts to output a value between -1.0 and 1.0 to attempt to make the "current_value" passed in, match the "set_point"
+#[derive(Debug, PartialEq, Clone, Copy)]
+pub struct PID {
+    pub config: Config,
+
+    integral: f32,
+    prev_error: f32,
+
+    set_point: f32,
+
+    output: f32,
+}
+
+impl PID {
+    pub fn new(config: Config) -> Self {
+        PID {
+            config: config,
+            integral: 0.0,
+            prev_error: 0.0,
+            set_point: 0.0,
+            output: 0.0,
+        }
+    }
+}
+
+impl Default for PID {
+    fn default() -> Self {
+        PID::new(Config {
+            kp: 0.0,
+            ki: 0.0,
+            kd: 0.0,
+            ff: 0.0,
+            dt: 0.0,
+            accel: 0.0,
+        })
+    }
+}
+
+impl PID {
+    ///Calculuates a single step of the PID+FF based on the config given to it.
+    ///Will output a float between -1.0 and 1.0
+    ///Current value is the current "position" of the system NOT the goal
+    /// Use set_point() method to change the "goal position" for the system
+    pub async fn pid_step(&mut self, current_value: f32) -> f32 {
+        //If the values
+        let error = self.set_point - current_value;
+
+        //If the error is equal to 0.0, just return the output from before. All of this math will just end up not affecting anything
+        if error == 0.0 {
+            return self.output;
+        }
+
+        //Calculate the integral
+        self.integral += error * self.config.dt;
+        let i = self.integral * self.config.ki;
+
+        //Calculate the derivative in a manner that is safe for 0.0 dt
+        let d;
+        if self.config.dt != 0.0 {
+            let derivative = (error - self.prev_error) / self.config.dt;
+            d = derivative * self.config.kd;
+        } else {
+            d = 0.0;
+        }
+
+        //store the original error
+        self.prev_error = error;
+
+        //Calculate the feedforward
+        let f = self.set_point * self.config.ff;
+        //Calculate the proportional factor
+        let p = error * self.config.kp;
+
+        //Calculate and store the output
+        let output = match self.config.accel {
+            0.0 => (f + p + i + d) * 0.0001,
+            _ => {
+                let pre_output = (f + p + i + d) * 0.0001;
+                let change = pre_output - self.output;
+                let change = change.clamp(-self.config.accel, self.config.accel);
+
+                let accel_output = self.output + change;
+                //Validate that there is a ki value
+                if self.config.ki != 0.0 {
+                    let excess = pre_output - accel_output;
+
+                    self.integral += excess / self.config.ki;
+                }
+
+                accel_output
+            }
+        };
+
+        self.output = output.clamp(-1.0, 1.0);
+
+        //Return the output information
+        return self.output;
+    }
+
+    pub async fn set_point(&mut self, set_point: f32) {
+        if set_point != self.set_point {
+            //Update all internal fields
+            self.integral = 0.0;
+            self.prev_error = 0.0;
+
+            self.set_point = set_point;
+        }
+    }
+}

rnavp_controller/Cargo.toml

@@ -0,0 +1,9 @@
+[package]
+name = "rnavp_logic"
+version = "0.1.0"
+edition = "2024"
+
+[dependencies]
+rnavp = {path = "../rnavp"}
+serde = { version = "1.0.229", default-features = false, features = ["derive"] }
+uom = { version = "0.38.0", default-features = false, features = ["si", "f32"] }

rnavp_controller/src/lib.rs

@@ -0,0 +1 @@
+pub mod motor;

rnavp_controller/src/motor.rs

@@ -0,0 +1,59 @@
+use rnavp::motor::{self, Direction};
+use rnavp::pid;
+use uom::si::f32::AngularVelocity;
+
+#[derive(Debug, Clone, Copy, PartialEq)]
+/// # Motor Controller
+/// This is a motor controller that takes in a generic type T that has implemented the needed traits.
+/// Most specifically Driver and Sensor.
+///
+/// ## Usage
+///
+/// Creating this allows for the controller a motor via a simple PID f32 signed Speed command, rather than u16 speed commands
+/// Notice that this also TAKES the motor from your manual controller, you loose access to the manual hand control you had before.
+pub struct Controller<T>
+where
+    T: motor::Driver + motor::Sensor,
+{
+    motor: T,
+    max_speed: AngularVelocity,
+    pid: pid::PID,
+    current_direction: Direction,
+}
+
+///Controller Impl for the adding the PID logic for handling the motor compiston
+impl<T: motor::Driver + motor::Sensor> Controller<T> {
+    ///Uses the given set_speed and then will handle the rest of the control logic to accurately* hit the requested speed
+    pub async fn control(&mut self, set_speed: AngularVelocity) {
+        let max_speed_rads = self.max_speed.value;
+        let set_speed_rads = set_speed.value.clamp(-max_speed_rads, max_speed_rads);
+
+        self.pid.set_point(set_speed_rads).await;
+        let pid_output = self.pid.pid_step(self.motor.get_speed().await.value).await;
+
+        self.current_direction.dir_from_f32(pid_output);
+
+        let motor_command = (pid_output * 65535.0) as u16;
+
+        self.motor
+            .set_speed_and_direction(motor_command, self.current_direction)
+            .await;
+    }
+
+    ///Retrieve the speed from the motor inside the controller.
+    /// This allows you to get a speed value from behind the move.
+    pub async fn retrieve(&self) -> AngularVelocity {
+        self.motor.get_speed().await
+    }
+
+    ///Creates a new Controller with a motor (of type T), a max speed and a pid::Config
+    /// This then allows for you to use the controller with the provided types
+    pub fn new(motor: T, max_speed: AngularVelocity, config: pid::Config) -> Self {
+        Controller {
+            motor: motor,
+            max_speed: max_speed,
+            pid: pid::PID::new(config),
+            current_direction: Direction::CCW,
+        }
+    }
+}

rnavp_logic/Cargo.toml

@@ -1,6 +0,0 @@
-[package]
-name = "rnavp_logic"
-version = "0.1.0"
-edition = "2024"
-
-[dependencies]

rnavp_logic/src/lib.rs

@@ -1,14 +0,0 @@
-pub fn add(left: u64, right: u64) -> u64 {
-    left + right
-}
-
-#[cfg(test)]
-mod tests {
-    use super::*;
-
-    #[test]
-    fn it_works() {
-        let result = add(2, 2);
-        assert_eq!(result, 4);
-    }
-}