From daf06fa466bb2c3a28b3dee3d342a15e40b2e65f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 21 Jan 2017 21:41:25 +0100
Subject: [PATCH] improved PID controller

---
 js/pid_controller.js | 129 +++++++++++++++++++++++++++++++++++++++++++
 js/serial_queue.js   |  46 ++++-----------
 main.html            |   1 +
 3 files changed, 140 insertions(+), 36 deletions(-)
 create mode 100644 js/pid_controller.js

diff --git a/js/pid_controller.js b/js/pid_controller.js
new file mode 100644
index 00000000..780a43f8
--- /dev/null
+++ b/js/pid_controller.js
@@ -0,0 +1,129 @@
+'use strict';
+
+var classes = classes || {};
+
+classes.PidController = function () {
+
+    var self = {},
+        privateScope = {};
+
+    /**
+     *
+     * @type {number}
+     */
+    privateScope.target = null;
+
+    /**
+     *
+     * @type {{P: null, I: null, D: null}}
+     */
+    privateScope.gains = {
+        P: null,
+        I: null,
+        D: null
+    };
+
+    /**
+     *
+     * @type {number}
+     */
+    privateScope.Iterm = 0;
+
+    /**
+     *
+     * @type {{min: number, max: number}}
+     */
+    privateScope.ItermLimit = {
+        min: -1000,
+        max: 1000
+    };
+
+    /**
+     *
+     * @type {number}
+     */
+    privateScope.previousError = 0;
+
+    /**
+     *
+     * @type {{min: number, max: number, minThreshold: number}}
+     */
+    privateScope.output = {
+        min: null,
+        max: null,
+        minThreshold: null
+    };
+
+    /**
+     *
+     * @param {number} value
+     */
+    self.setTarget = function (value) {
+        privateScope.target = value;
+    };
+
+    /**
+     * @param {number} Pgain
+     * @param {number} Igain
+     * @param {number} Dgain
+     */
+    self.setGains = function (Pgain, Igain, Dgain) {
+        privateScope.gains.P = Pgain;
+        privateScope.gains.I = Igain;
+        privateScope.gains.D = Dgain;
+    };
+
+    /**
+     * Sets min and max value for output
+     * @param {number} min
+     * @param {number} max
+     * @param {number} minThreshold if output is below this value, [min] is returned
+     */
+    self.setOutput = function (min, max, minThreshold) {
+        privateScope.output.min = min;
+        privateScope.output.max = max;
+        privateScope.output.minThreshold = minThreshold;
+    };
+
+    /**
+     * Sets upper and lower limit for Iterm accumulator
+     * @param {number} min
+     * @param {number} max
+     */
+    self.setItermLimit = function (min, max) {
+        privateScope.ItermLimit.min = min;
+        privateScope.ItermLimit.max = max;
+    };
+
+    /**
+     * Executes PID controller based on current value and target
+     * @param {number} current
+     * @returns {number}
+     */
+    self.run = function (current) {
+        var error = current - privateScope.target,
+            Pterm = error * privateScope.gains.P,
+            Dterm = (error - privateScope.previousError) * privateScope.gains.D,
+            output;
+
+        privateScope.previousError = error;
+
+        privateScope.Iterm += error * privateScope.gains.I;
+        if (privateScope.Iterm > privateScope.ItermLimit.max) {
+            privateScope.Iterm = privateScope.ItermLimit.max;
+        } else if (privateScope.Iterm < privateScope.ItermLimit.min) {
+            privateScope.Iterm = privateScope.ItermLimit.min;
+        }
+
+        output = Pterm + privateScope.Iterm + Dterm;
+        if (output < privateScope.output.minThreshold) {
+            output = privateScope.output.min;
+        } else if (output > privateScope.output.max) {
+            output = privateScope.output.max;
+        }
+
+        return output;
+    };
+
+    return self;
+};
\ No newline at end of file
diff --git a/js/serial_queue.js b/js/serial_queue.js
index 15dbd290..19121dd5 100644
--- a/js/serial_queue.js
+++ b/js/serial_queue.js
@@ -19,46 +19,20 @@ helper.mspQueue = (function (serial, MSP) {
 
     privateScope.currentLoad = 0;
 
-    privateScope.loadPid = {
-        gains: {
-            P: 10,
-            I: 4,
-            D: 2
-        },
-        Iterm: 0,
-        ItermLimit: 85,
-        previousError: 0,
-        output: {
-            min: 0,
-            max: 97,
-            minThreshold: 0
-        }
-    };
+    /**
+     * PID controller used to perform throttling
+     * @type {classes.PidController}
+     */
+    privateScope.loadPidController = new classes.PidController();
+    privateScope.loadPidController.setTarget(privateScope.targetLoad);
+    privateScope.loadPidController.setOutput(0, 97, 0);
+    privateScope.loadPidController.setGains(16, 6, 4);
+    privateScope.loadPidController.setItermLimit(0, 90);
 
     privateScope.dropRatio = 0;
 
     publicScope.computeDropRatio = function () {
-        var error = privateScope.currentLoad - privateScope.targetLoad;
-
-        var Pterm = error * privateScope.loadPid.gains.P,
-            Dterm = (error - privateScope.loadPid.previousError) * privateScope.loadPid.gains.D;
-
-        privateScope.loadPid.previousError = error;
-
-        privateScope.loadPid.Iterm += error * privateScope.loadPid.gains.I;
-        if (privateScope.loadPid.Iterm > privateScope.loadPid.ItermLimit) {
-            privateScope.loadPid.Iterm = privateScope.loadPid.ItermLimit;
-        } else if (privateScope.loadPid.Iterm < -privateScope.loadPid.ItermLimit) {
-            privateScope.loadPid.Iterm = -privateScope.loadPid.ItermLimit;
-        }
-
-        privateScope.dropRatio = Pterm + privateScope.loadPid.Iterm + Dterm;
-        if (privateScope.dropRatio < privateScope.loadPid.output.minThreshold) {
-            privateScope.dropRatio = privateScope.loadPid.output.min;
-        }
-        if (privateScope.dropRatio > privateScope.loadPid.output.max) {
-            privateScope.dropRatio = privateScope.loadPid.output.max;
-        }
+        privateScope.dropRatio = privateScope.loadPidController.run(publicScope.getLoad());
     };
 
     publicScope.getDropRatio = function () {
diff --git a/main.html b/main.html
index 63c6a465..fe2aaa61 100755
--- a/main.html
+++ b/main.html
@@ -52,6 +52,7 @@
 <script type="text/javascript" src="./js/injected_methods.js"></script>
 <script type="text/javascript" src="./js/intervals.js"></script>
 <script type="text/javascript" src="./js/timeouts.js"></script>
+<script type="text/javascript" src="./js/pid_controller.js"></script>
 <script type="text/javascript" src="./js/simple_smooth_filter.js"></script>
 <script type="text/javascript" src="./js/walking_average_filter.js"></script>
 <script type="text/javascript" src="./js/gui.js"></script>