Added in the Pid and Motor Code

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

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,
+        }
+    }
+}