1 files changed, 55 insertions, 0 deletions
diff --git a/src/math.rs b/src/math.rs
index e266a88..4f0558c 100644
--- a/src/math.rs
+++ b/src/math.rs
@@ -2,6 +2,8 @@ extern crate rand;
 
 use self::rand::distributions::Alphanumeric;
 use self::rand::Rng;
+use crate::constants;
+use crate::modules::types::ModuleType;
 use std::iter::repeat;
 
 pub fn rand_name() -> String {
@@ -11,6 +13,59 @@ pub fn rand_name() -> String {
         .collect()
 }
 
+#[derive(Serialize, Deserialize, Debug, Clone, Default)]
+pub struct ControlSystem {
+    previous_error: f64,
+    integral: f64,
+    kp: f64,
+    ki: f64,
+    kd: f64,
+    dt: f64,
+}
+
+impl ControlSystem {
+    pub fn new(module: ModuleType) -> ControlSystem {
+        let previous_error = 0.0;
+        let integral = 0.0;
+        match module {
+            ModuleType::Tractorbeam => ControlSystem {
+                kp: constants::SHIP_TRACTORBEAM_CONTROLSYSTEM_KP,
+                ki: constants::SHIP_TRACTORBEAM_CONTROLSYSTEM_KI,
+                kd: constants::SHIP_TRACTORBEAM_CONTROLSYSTEM_KD,
+                dt: constants::SHIP_TRACTORBEAM_CONTROLSYSTEM_DT,
+                previous_error,
+                integral,
+            },
+            _ => ControlSystem {
+                kp: 1.0,
+                ki: 1.0,
+                kd: 1.0,
+                dt: 1.0,
+                previous_error,
+                integral,
+            },
+        }
+    }
+
+    pub fn compute(&mut self, strength: f64, distance: f64, desired_distance: f64) -> f64 {
+        let error = desired_distance - distance;
+        self.integral += error * self.dt;
+        let derivative = (error - self.previous_error) / self.dt;
+        let output = self.kp * error + self.ki * self.integral + self.kd * derivative;
+        self.previous_error = error;
+
+        if output.abs() > strength {
+            if output.is_sign_positive() {
+                strength
+            } else {
+                strength * -1.0
+            }
+        } else {
+            output
+        }
+    }
+}
+
 #[derive(Serialize, Deserialize, Debug, Clone, Default, PartialEq)]
 pub struct Vector {
     pub x: f64,