Basic framework for a swerve drivebase added

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');