PID fixes.

There was no way to create a PID, becuase of the private fields

I also fixed a divide by 0.0 bug that was 100% going to bite me later if I did not.

src/core/logic/pid.rs

@@ -30,6 +30,31 @@ pub struct PID {
     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
@@ -46,9 +71,16 @@ impl PID {
 
         //Calculate the integral
         self.integral += error * self.config.dt;
+        let i = self.integral * self.config.ki;
 
-        //Calculate the derivative
-        let derivative = (error - self.prev_error) / self.config.dt;
+        //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;
@@ -57,10 +89,6 @@ impl PID {
         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 {
@@ -71,9 +99,12 @@ impl PID {
                 let change = change.clamp(-self.config.accel, self.config.accel);
 
                 let accel_output = self.output + change;
-                let excess = pre_output - accel_output;
+                //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;
+                    self.integral += excess / self.config.ki;
+                }
 
                 accel_output
             }