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/kinematics.rs

@@ -0,0 +1 @@
+

src/core/logic/mod.rs

@@ -1,2 +1,3 @@
 pub mod kinematics;
 pub mod motor;
+pub mod pid;

src/core/logic/motor.rs

@@ -1,24 +1,32 @@
-use crate::core::{
+use crate::core::motor;
-    motor,
+use crate::core::motor::{Direction, Speed};
-    positional,
-    units,
-};
 
+use crate::core::logic::pid;
 
-///The Config for the PID system used by the controller
+pub struct Data {
-pub struct PIDConfig {
+    pub max_speed: Speed,
-    ///The Porportional value for the controller
+    pub pid: pid::PID,
-    pub kp: f32,
+    pub current_direction: Direction,
-    ///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,
-
 }
 
-///Controller trait for motors. Motors with this trait, automatically have a PID included.
+///Controller trait for motors. Motors with this trait, automatically have controls included
 pub trait Controller: motor::Driver + motor::Sensor {
-    const CONFIG: PIDConfig;
+    ///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;
+
+    ///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);
+    }
 }

src/core/logic/pid.rs

@@ -0,0 +1,92 @@
+///The Config for the PID system used by the controller
+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,
+}
+
+pub struct PID {
+    pub config: Config,
+
+    integral: f32,
+    prev_error: f32,
+
+    set_point: f32,
+
+    output: f32,
+}
+
+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 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;
+
+        //Calculate the derivative
+        let derivative = (error - self.prev_error) / self.config.dt;
+
+        //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 the inegral factor
+        let i = self.integral * self.config.ki;
+        //Calculate the derivative
+        let d = derivative * self.config.kd;
+
+        //Calculate and store the output
+        let output = match self.config.accel {
+            0.0 => f + p + i + d,
+            _ => {
+                let pre_output = f + p + i + d;
+                let change = pre_output - self.output;
+                let change = change.clamp(-self.config.accel, self.config.accel);
+
+                let accel_output = self.output + change;
+                let excess = pre_output - accel_output;
+
+                self.integral += excess / self.config.ki;
+
+                accel_output
+            }
+        };
+
+        self.output = output.abs().clamp(-1.0, 1.0);
+
+        //Return the output information
+        return self.output;
+    }
+
+    pub 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;
+        }
+    }
+}