pid controller and menu setup

diff --git a/package.json b/package.json
index af4002f..4405128 100644 (file)
--- 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 (file)
--- 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 <link>Planck.js</link> 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 (file)
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 (file)
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 (file)
--- 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;
 }