add force magnitude of x and y forces in menu

diff --git a/src/app.js b/src/app.js
index 816d89d..08ae6c7 100644 (file)
--- a/src/app.js
+++ b/src/app.js
@@ -14,7 +14,7 @@ canvas.height = window.innerHeight;
 const PPM = 30;
 
 var targetX = 10;
-const targetY = -2.5;
+var targetY = -2.5;
 
 // Define the box properties
 const boxWidth = 1;
@@ -33,7 +33,8 @@ MENU.initMenu(pid);
 
 canvas.addEventListener("click", function(event) {
        targetX = (event.clientX - canvas.getBoundingClientRect().left) / PPM;
-       console.log("Updated targetX:", targetX);
+       targetY = -(event.clientY) / PPM
+       console.log("Updated target:", targetX, targetY);
 });
 
 // Render the box on the canvas
@@ -44,7 +45,7 @@ function render() {
        // Clear the canvas
        ctx.clearRect(0, 0, canvas.width, canvas.height);
        ctx.fillStyle = "white";
-       ctx.fillRect(targetX * PPM, (-position.y * PPM), 30, 30);
+       ctx.fillRect(targetX * PPM, (-targetY * PPM), 30, 30);
 
        // Draw the box
        ctx.save();
@@ -53,9 +54,7 @@ function render() {
        ctx.fillStyle = "blue";
        ctx.fillRect(position.x * PPM, (-position.y * PPM), boxWidth * PPM, boxHeight * PPM);
        ctx.restore();
-       var r = pid.Update(1/60, position.x, targetX);
-       //console.log(r);
-       var force = {x: r, y: 0};
+       var force = pid.Update(1/60, {x: position.x, y: position.y}, {x: targetX, y: targetY});
        box.applyForce(force, box.getWorldCenter());
 }
 

diff --git a/src/menu.js b/src/menu.js
index fcc58bc..df6ec57 100644 (file)
--- a/src/menu.js
+++ b/src/menu.js
@@ -27,11 +27,11 @@ function initMenu(pid)
                        velocity: "Velocity"
                }
                });
-       pane.addMonitor(pid.PIDParams, "force", {
+       pane.addMonitor(pid.PIDParams, "forceMagnitude", {
                title: "Force (N)",
                view: "graph",
-               min: -10000,
-               max: +10000,
+               min: 0,
+               max: +20000,
        });
 }
 

diff --git a/src/pid-controller.js b/src/pid-controller.js
index b284e64..1d06aa8 100644 (file)
--- a/src/pid-controller.js
+++ b/src/pid-controller.js
@@ -1,51 +1,112 @@
 class PIDController {
        constructor() {
-       this.PIDParams = {proportionalGain: 2, integralGain: 0.0, derivativeGain: 2, iMin: -1, iMax: 1, integralSaturation: false, derivativeInitialized: false, outputMax: 1, outputMin: -1, magnitude: 10, force: 0, derivativeMeasurement: "Velocity"};
-               this.valueLast; 
-               this.errorLast;
-       this.integrationStored = 0;
-       this.velocity;
+               this.PIDParams = {
+                       proportionalGain: 2,
+                       integralGain: 0.0,
+                       derivativeGain: 2,
+                       iMin: -1,
+                       iMax: 1,
+                       integralSaturation: false,
+                       derivativeInitialized: false,
+                       outputMax: 1,
+                       outputMin: -1,
+                       magnitude: 10,
+                       force: {x: 0, y: 0},
+                       forceMagnitude: 0,
+                       derivativeMeasurement: "Velocity"
+               };
+               this.valueLast = {x: 0, y: 0};
+               this.errorLast = {x: 0, y: 0};
+               this.integrationStored = {x: 0, y: 0};
+               this.velocity = {x: 0, y: 0};
        }
+
        Reset() {
                this.derivativeInitialized = false;
        }
+
        Update(dt, currentValue, targetValue) {
                if (dt <= 0) {
-                       return;
+                       return {x: 0, y: 0};
                }
-               var error = targetValue - currentValue;
-        
-               var P = this.PIDParams.proportionalGain * error;
-        
-               this.integrationStored = Math.min(Math.max(this.integrationStored + (error * dt), this.PIDParams.iMin), this.PIDParams.iMax);
-               var I = this.PIDParams.integralGain * this.integrationStored;
-
-               var errorRateOfChange = (error - this.errorLast) / dt;
+
+               const error = {
+                       x: targetValue.x - currentValue.x,
+                       y: targetValue.y - currentValue.y
+               };
+
+               const P = {
+                       x: this.PIDParams.proportionalGain * error.x,
+                       y: this.PIDParams.proportionalGain * error.y
+               };
+
+               this.integrationStored = {
+                       x: Math.min(
+                               Math.max(this.integrationStored.x + (error.x * dt), this.PIDParams.iMin),
+                               this.PIDParams.iMax
+                       ),
+                       y: Math.min(
+                               Math.max(this.integrationStored.y + (error.y * dt), this.PIDParams.iMin),
+                               this.PIDParams.iMax
+                       )
+               };
+
+               const I = {
+                       x: this.PIDParams.integralGain * this.integrationStored.x,
+                       y: this.PIDParams.integralGain * this.integrationStored.y
+               };
+
+               const errorRateOfChange = {
+                       x: (error.x - this.errorLast.x) / dt,
+                       y: (error.y - this.errorLast.y) / dt
+               };
+
                this.errorLast = error;
 
-               var valueRateOfChange = (currentValue - this.valueLast) / dt;
+               const valueRateOfChange = {
+                       x: (currentValue.x - this.valueLast.x) / dt,
+                       y: (currentValue.y - this.valueLast.y) / dt
+               };
+
                this.valueLast = currentValue;
                this.velocity = valueRateOfChange;
 
-               var deriveMeasure = 0;
+               let deriveMeasure = {x: 0, y: 0};
 
                if (this.derivativeInitialized) {
-                       if (this.PIDParams.derivativeMeasurement == "Velocity") {
-                               deriveMeasure = -valueRateOfChange;
+                       if (this.PIDParams.derivativeMeasurement === "Velocity") {
+                               deriveMeasure = {
+                                       x: -valueRateOfChange.x,
+                                       y: -valueRateOfChange.y
+                               };
                        } else {
-                          deriveMeasure = errorRateOfChange;
+                               deriveMeasure = errorRateOfChange;
                        }
                } else {
                        this.derivativeInitialized = true;
                }
-               var D = this.PIDParams.derivativeGain * deriveMeasure;
-               var result = P + I + D;
-               result *= this.PIDParams.magnitude;
-               this.PIDParams.force = result * 60; /* this routine is called ~60seconds */
+
+               const D = {
+                       x: this.PIDParams.derivativeGain * deriveMeasure.x,
+                       y: this.PIDParams.derivativeGain * deriveMeasure.y
+               };
+
+               const result = {
+                       x: P.x + I.x + D.x,
+                       y: P.y + I.y + D.y
+               };
+
+               result.x *= this.PIDParams.magnitude;
+               result.y *= this.PIDParams.magnitude;
+
+               this.PIDParams.force = {
+                       x: result.x * 60, // this routine is called ~60 seconds
+                       y: result.y * 60
+               };
+
+               this.PIDParams.forceMagnitude = Math.sqrt(result.x ** 2 + result.y ** 2) * 60;
 
                return result;
-               //return Math.min(Math.max((result, this.PIDParams.outputMin), this.PIDParams.outputMax));
        }
 }
-
 export default PIDController;