From: Robin Krens <robin@robinkrens.nl>
Date: Wed, 24 Jan 2024 11:10:11 +0000 (+0100)
Subject: add force magnitude of x and y forces in menu
X-Git-Url: https://robinkrens.nl/gitweb/?a=commitdiff_plain;h=5715e52d934cd488401164140e3a00d89eeb7a0b;p=xy-pid-controller

add force magnitude of x and y forces in menu
---

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;