From 5715e52d934cd488401164140e3a00d89eeb7a0b Mon Sep 17 00:00:00 2001 From: Robin Krens Date: Wed, 24 Jan 2024 12:10:11 +0100 Subject: [PATCH] add force magnitude of x and y forces in menu --- src/app.js | 11 +++-- src/menu.js | 6 +-- src/pid-controller.js | 111 ++++++++++++++++++++++++++++++++++++++------------ 3 files changed, 94 insertions(+), 34 deletions(-) diff --git a/src/app.js b/src/app.js index 816d89d..08ae6c7 100644 --- 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 --- 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 --- 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; -- 2.7.4