From bcf1196c6ff349c5d4596b31ed63e9dc688cbe31 Mon Sep 17 00:00:00 2001
From: Moonlit Productions <moonlit@munebase.dev>
Date: Tue, 21 Oct 2025 14:18:35 -0400
Subject: [PATCH] Basic framework for a swerve drivebase added

---
 script.js | 59 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/script.js b/script.js
index 3a19989..574a548 100644
--- a/script.js
+++ b/script.js
@@ -1,3 +1,62 @@
+// 2D vector class to make some of the math easier
+class Vec2D {
+    constructor(x, y) {
+        this.x = x;
+        this.y = y;
+    }
+
+    magnitude() {
+        return Math.sqrt(this.x * this.x + this.y * this.y);
+    }
+
+    angle() {
+        return Math.atan2(this.y, this.x);
+    }
+}
+
+// Swerve module class to represent a single wheel
+class SwerveModule {
+    constructor(x, y, name) {
+        this.position = new Vec2D(x, y);
+        this.velocity = new Vec2D(0, 0);
+        this.angle = 0;
+        this.speed = 0;
+        this.name = name;
+    }
+
+    calculateState(velocityX, velocityY, turnSpeed) {
+        // Take the requested speed and turn rate of the robot and calculate
+        // speed and angle of this module to achieve it
+        throw new Error("Not Yet Implemented");
+    }
+}
+
+// Swerve drive class to represent the robot as a whole
+class SwerveDrive {
+    constructor(modulePositionsAndNames) {
+        // Take an array of module positions with a name and create an array of SwerveModule objects
+        this.modules = modulePositionsAndNames.map(module =>
+            new SwerveModule(module.x, module.y, module.name)
+        );
+    }
+
+    drive(velocityX, velocityY, turnSpeed, maxSpeed) {
+        // Take in a requested speeds and update every module
+        this.modules.forEach(module =>
+            module.calculateState(velocityX, velocityY, turnSpeed)
+        );
+
+        // If any speeds exceed the max speed, normalize down so we don't effect movement direction
+        const maxCalculated = Math.max(...this.modules.map(m => m.speed), 0);
+        if (maxCalculated > maxSpeed) {
+            const scale = maxSpeed / maxCalculated;
+            this.modules.forEach(module => {
+                module.speed *= scale;
+            });
+        }
+    }
+}
+
 // Get all control elements
 const vxSlider = document.getElementById('vx-slider');
 const vySlider = document.getElementById('vy-slider');