Gyro heading added to swerve drive structure and implemented it to the visualization

index.html

@@ -92,6 +92,8 @@
             <div id="current-config-info" class="config-info">
                 Current Configuration: <strong id="config-name">4-Wheel Rectangle</strong>
                 (<span id="module-count-display">4</span> modules)
+                <br>
+                Gyro Heading: <strong id="gyro-heading-display">0.0°</strong>
             </div>
             <div class="module-grid" id="module-grid">
                 <!-- Dynamically generated module data will appear here -->

script.js

@@ -28,19 +28,27 @@ class SwerveModule {
         this.name = name;
     }
 
-    calculateState(velocityX, velocityY, turnSpeed) {
+    calculateState(velocityX, velocityY, turnSpeed, heading = 0) {
         // Take the requested speed and turn rate of the robot and calculate
         // speed and angle of this module to achieve it
 
+        // Transform field-relative velocities to robot-relative velocities
+        // by rotating the velocity vector by the negative of the robot's heading
+        const cosHeading = Math.cos(-heading);
+        const sinHeading = Math.sin(-heading);
+
+        const robotVelX = velocityX * cosHeading - velocityY * sinHeading;
+        const robotVelY = velocityX * sinHeading + velocityY * cosHeading;
+
         // Calculate rotation contribution (perpendicular to position vector)
         const rotX = -this.position.y * turnSpeed;
         const rotY = this.position.x * turnSpeed;
 
-        // Combine translation and rotation
+        // Combine translation and rotation (now in robot frame)
-        this.velocity.x = velocityX + rotX;
+        this.velocity.x = robotVelX + rotX;
-        this.velocity.y = velocityY + rotY;
+        this.velocity.y = robotVelY + rotY;
 
-        // Calculate speed and angle
+        // Calculate speed and angle (in robot frame)
         this.speed = this.velocity.magnitude();
         this.angle = this.velocity.angle();
     }
@@ -51,6 +59,7 @@ class SwerveDrive {
     constructor(modulePositionsAndNames, robotName) {
         this.setModules(modulePositionsAndNames);
         this.setName(robotName);
+        this.gyroHeading = 0; // Simulated gyro heading in radians
     }
 
     setName(robotName) {
@@ -64,10 +73,23 @@ class SwerveDrive {
         );
     }
 
-    drive(velocityX, velocityY, turnSpeed, maxModuleSpeed) {
+    updateHeading(turnSpeed, deltaTime = 0.01) {
+        // Integrate turn speed to update gyro heading
+        // turnSpeed is in radians/second, deltaTime is the time step
+        this.gyroHeading += turnSpeed * deltaTime;
+
+        // Normalize to -PI to PI range
+        while (this.gyroHeading > Math.PI) this.gyroHeading -= 2 * Math.PI;
+        while (this.gyroHeading < -Math.PI) this.gyroHeading += 2 * Math.PI;
+    }
+
+    drive(velocityX, velocityY, turnSpeed, maxModuleSpeed, deltaTime = 0.01) {
+        // Update gyro heading first
+        this.updateHeading(turnSpeed, deltaTime);
+
         // Take in a requested speeds and update every module
         this.modules.forEach(module =>
-            module.calculateState(velocityX, velocityY, turnSpeed)
+            module.calculateState(velocityX, velocityY, turnSpeed, this.gyroHeading)
         );
 
         // If any speeds exceed the max speed, normalize down so we don't effect movement direction
@@ -271,7 +293,7 @@ generateInputsBtn.addEventListener('click', () => {
     const count = parseInt(moduleCountInput.value);
 
     if (isNaN(count) || count < 2) {
-        alert('Please enter a valid number of modules between 2 and 12.');
+        alert('Please enter a valid number of modules above or equal to 2.');
         return;
     }
     generateModuleInputs(count);
@@ -369,6 +391,13 @@ function updateModuleDisplays(robot) {
     const moduleCount = document.getElementById('module-count-display');
     moduleCount.textContent = robot.modules.length;
 
+    // Update gyro heading display
+    const gyroHeadingDisplay = document.getElementById('gyro-heading-display');
+    if (gyroHeadingDisplay) {
+        const headingDeg = (robot.gyroHeading * 180 / Math.PI).toFixed(1);
+        gyroHeadingDisplay.textContent = `${headingDeg}°`;
+    }
+
     const modules = robot.modules;
     modules.forEach((module, i) => {
         const angleElement = document.getElementById(`module-${i}-angle`);
@@ -394,12 +423,9 @@ const ctx = canvas.getContext('2d');
 // Get CSS variables for use in canvas
 const rootStyles = getComputedStyle(document.documentElement);
 
-function drawGrid(ctx, sideLength, gridSquareSize, xOffset, yOffset, rotation) {
+function drawGrid(ctx, sideLength, gridSquareSize, xOffset, yOffset) {
     ctx.save();
 
-    // Apply rotation transform
-    ctx.rotate(-rotation);
-
     ctx.strokeStyle = rootStyles.getPropertyValue('--grid-color');
     ctx.lineWidth = 1;
     const startX = (-sideLength / 2) - xOffset;
@@ -463,7 +489,11 @@ function drawModule(ctx, module) {
     ctx.restore();
 }
 
-function drawRobot(ctx, robot) {
+function drawRobot(ctx, robot, heading) {
+    ctx.save(); // Save current state before rotation
+
+    ctx.rotate(heading);
+
     ctx.strokeStyle = rootStyles.getPropertyValue('--robot-frame-color')
     ctx.fillStyle = rootStyles.getPropertyValue('--robot-fill-color');
     ctx.lineWidth = 4;
@@ -480,6 +510,8 @@ function drawRobot(ctx, robot) {
     ctx.stroke();
 
     modules.forEach(module => drawModule(ctx, module));
+
+    ctx.restore(); // Restore to remove rotation
 }
 
 
@@ -491,9 +523,8 @@ createModuleDisplays(robot);
 let xSpeed = 0;
 let ySpeed = 0;
 let turnSpeed = -1;
-let robotRotation = 0; // Track cumulative robot rotation for grid display
 
-let gridSquareSize = 25;
+let gridSquareSize = 50;
 let xGridOffset = 0;
 let yGridOffset = 0;
 robot.drive(xSpeed, ySpeed, 0, 500);
@@ -511,26 +542,20 @@ function animate() {
 
     // Animate the grid with robot movement
     let offsetSpeedDivisor = (100 - gridSquareSize <= 0 ? 1 : 100 - gridSquareSize);
-    robotRotation += turnSpeed * 0.01; // Scale factor for reasonable rotation speed
-
-    // Convert robot velocities to world velocities for grid movement
-    const cosRot = Math.cos(robotRotation);
-    const sinRot = Math.sin(robotRotation);
-    const worldVx = xSpeed * cosRot - ySpeed * sinRot;
-    const worldVy = xSpeed * sinRot + ySpeed * cosRot;
 
     // Update grid offsets based on robot movement
-    xGridOffset = (xGridOffset + (worldVx / offsetSpeedDivisor)) % gridSquareSize;
+    xGridOffset = (xGridOffset + (xSpeed / offsetSpeedDivisor)) % gridSquareSize;
-    yGridOffset = (yGridOffset + (worldVy / offsetSpeedDivisor)) % gridSquareSize;
+    yGridOffset = (yGridOffset + (ySpeed / offsetSpeedDivisor)) % gridSquareSize;
 
     // Update module states before drawing the robot
+    // The drive() method will update the gyroHeading internally
     robot.drive(xSpeed, ySpeed, turnSpeed, parseFloat(maxSpeedSlider.value));
     updateModuleDisplays(robot);
 
     // Draw the robot and it's movement. Grid should be oversized so movement
     // doesn't find the edge of the grid
-    drawGrid(ctx, canvas.width * 2, gridSquareSize, xGridOffset, yGridOffset, robotRotation);
+    drawGrid(ctx, canvas.width * 2, gridSquareSize, xGridOffset, yGridOffset);
-    drawRobot(ctx, robot);
+    drawRobot(ctx, robot, robot.gyroHeading);
 
     // Do it all over again
     ctx.restore();