From: Robin Krens Date: Tue, 23 Jan 2024 15:14:35 +0000 (+0100) Subject: pid controller and menu setup X-Git-Url: https://robinkrens.nl/gitweb/?a=commitdiff_plain;h=ccb633c3842a21493420d150c92db3d546921888;p=xy-pid-controller pid controller and menu setup --- diff --git a/package.json b/package.json index af4002f..4405128 100644 --- a/package.json +++ b/package.json @@ -16,6 +16,7 @@ }, "devDependencies": { "eslint": "^8.56.0", - "parcel": "^2.11.0" + "parcel": "^2.11.0", + "tweakpane": "^4.0.3" } } diff --git a/src/app.js b/src/app.js index 628b3af..49b3bb8 100644 --- a/src/app.js +++ b/src/app.js @@ -1,23 +1,35 @@ import {Vec2, Box, World} from "planck"; +import * as MENU from "./menu.js"; +import PIDController from "./pid-controller.js"; // Get the canvas element const canvas = document.getElementById("area"); const ctx = canvas.getContext("2d"); -// Create a Planck.js world const world = World(Vec2(0, 0)); +canvas.width = 500; +canvas.height = 500; + +const PPM = 30; + +const targetX = 10; +const targetY = -2.5; + // Define the box properties -const boxWidth = 50; -const boxHeight = 50; +const boxWidth = 1; +const boxHeight = 1; // Create a dynamic body for the box -const box = world.createDynamicBody(Vec2(200, 100)); +const box = world.createDynamicBody(Vec2(1, -2.5)); box.createFixture({ - shape: Box(boxWidth / 2, boxHeight / 2), + shape: Box(boxWidth, boxHeight), density: 1.0, }); +MENU.initMenu(); +const pid = new PIDController(); + // Render the box on the canvas function render() { const position = box.getPosition(); @@ -25,14 +37,20 @@ 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); // Draw the box ctx.save(); - ctx.translate(position.x, position.y); + //ctx.translate(position.x * PPM, (-position.y * PPM)); ctx.rotate(angle); - ctx.fillStyle = "#0095DD"; - ctx.fillRect(-boxWidth / 2, -boxHeight / 2, boxWidth, boxHeight); + 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}; + box.applyForce(force, box.getWorldCenter()); } // Animation loop diff --git a/src/menu.js b/src/menu.js new file mode 100644 index 0000000..0e4e0b4 --- /dev/null +++ b/src/menu.js @@ -0,0 +1,12 @@ +import {Pane} from "tweakpane"; + +function initMenu() +{ + const pane = new Pane(); + const tab = pane.addTab({ + pages: [{title: "Creative Mode"}, {title: "Preset"}] + }); + +} + +export {initMenu}; diff --git a/src/pid-controller.js b/src/pid-controller.js new file mode 100644 index 0000000..2ec15d2 --- /dev/null +++ b/src/pid-controller.js @@ -0,0 +1,97 @@ +class PIDController { + //public enum DerivativeMeasurement { + // Velocity, + // ErrorRateOfChange + //} + constructor() { + this.PIDParams = {proportionalGain: 50, integralGain: 0.01, derivativeGain: 35, outputMin: -1, outputMax: 1, integralSaturation: false, derivativeInitialized: false}; + this.valueLast; + this.errorLast; + this.integrationStored; + this.velocity; + } + Reset() { + this.derivativeInitialized = false; + } + Update(dt, currentValue, targetValue) { + if (dt <= 0) { + return; + } + var error = targetValue - currentValue; + + var P = this.PIDParams.proportionalGain * error; + + this.integrationStored = Math.min(Math.max(this.integrationStored + (error * dt), -this.integralSaturation), -this.integralSaturation); + var I = this.integralGain * this.integrationStored; + + var errorRateOfChange = (error - this.errorLast) / dt; + this.errorLast = error; + + var valueRateOfChange = (currentValue - this.valueLast) / dt; + this.valueLast = currentValue; + this.velocity = valueRateOfChange; + + var deriveMeasure = 0; + + if (this.derivativeInitialized) { + //if (derivativeMeasurement == DerivativeMeasurement.Velocity) { + deriveMeasure = -valueRateOfChange; + //} else { + // deriveMeasure = errorRateOfChange; + //} + } else { + this.derivativeInitialized = true; + } + + var D = this.PIDParams.derivativeGain * deriveMeasure; + var result = P + D; + return result; + + //return Math.min(Math.max((result, this.outputMin), this.outputMax)); + } + + // float AngleDifference(float a, float b) { + // return (a - b + 540) % 360 - 180; //calculate modular difference, and remap to [-180, 180] + // } + + // public float UpdateAngle(float dt, float currentAngle, float targetAngle) { + // if (dt <= 0) throw new ArgumentOutOfRangeException(nameof(dt)); + // float error = AngleDifference(targetAngle, currentAngle); + + // //calculate P term + // float P = proportionalGain * error; + + // //calculate I term + // integrationStored = Mathf.Clamp(integrationStored + (error * dt), -integralSaturation, integralSaturation); + // float I = integralGain * integrationStored; + + // //calculate both D terms + // float errorRateOfChange = AngleDifference(error, errorLast) / dt; + // errorLast = error; + + // float valueRateOfChange = AngleDifference(currentAngle, valueLast) / dt; + // valueLast = currentAngle; + // velocity = valueRateOfChange; + + // //choose D term to use + // float deriveMeasure = 0; + + // if (derivativeInitialized) { + // if (derivativeMeasurement == DerivativeMeasurement.Velocity) { + // deriveMeasure = -valueRateOfChange; + // } else { + // deriveMeasure = errorRateOfChange; + // } + // } else { + // derivativeInitialized = true; + // } + + // float D = derivativeGain * deriveMeasure; + + // float result = P + I + D; + + // return Mathf.Clamp(result, outputMin, outputMax); + // } +} + +export default PIDController; diff --git a/src/styles.css b/src/styles.css index 59169e3..3ec4728 100644 --- a/src/styles.css +++ b/src/styles.css @@ -1,7 +1,15 @@ +body, html { + margin: 10px; + padding: 0; + overflow: hidden; +} + #area { - margin: auto; + margin: 0; padding: 0; display: block; + width: 500px; + height: 500px; background: #ccc; }