Added main logic for PID system and motor system

Quite a bit of the important logic bits have been added for the Controller Trait, and the PID system.

src/core/logic/motor.rs

@@ -1,24 +1,32 @@
-use crate::core::{
-    motor,
-    positional,
-    units,
-};
+use crate::core::motor;
+use crate::core::motor::{Direction, Speed};
 
+use crate::core::logic::pid;
 
-///The Config for the PID system used by the controller
-pub struct PIDConfig {
-    ///The Porportional 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,
+pub struct Data {
+    pub max_speed: Speed,
+    pub pid: pid::PID,
+    pub current_direction: Direction,
+}
 
-} 
+///Controller trait for motors. Motors with this trait, automatically have controls included
+pub trait Controller: motor::Driver + motor::Sensor {
+    ///Create a Data field using the Data type in your motor struct, and return it here. The rest of the trait is automatic
+    fn data(&mut self) -> Data;
 
-///Controller trait for motors. Motors with this trait, automatically have a PID included.
-pub trait Controller : motor::Driver + motor::Sensor {
-    const CONFIG: PIDConfig;
-}
+    ///Uses the given set_speed and then will handle the rest of the control logic to accurately* hit the requested speed
+    fn control(&mut self, set_speed: Speed) {
+        let data: &mut Data = &mut self.data();
+        let max_speed_rads = data.max_speed.rads();
+        let set_speed_rads = set_speed.rads().clamp(-max_speed_rads, max_speed_rads);
+
+        data.pid.set_point(set_speed_rads);
+        let pid_output = data.pid.pid_step(self.get_speed().rads());
+
+        data.current_direction.dir_from_f32(pid_output);
+
+        let motor_command = (pid_output * 65535.0) as u16;
+
+        self.set_speed_and_direction(motor_command, data.current_direction);
+    }
+}