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

index.html

@@ -16,7 +16,7 @@
     <main>
         <section class="visualization-canvas">
             <h2>Robot Visualization</h2>
-            <canvas id="swerve-canvas" width="600" height="600"></canvas>
+            <canvas id="swerve-canvas" width="800" height="800"></canvas>
         </section>
 
         <section class="control-panel">
@@ -78,6 +78,7 @@
                 </div>
 
                 <button id="generate-inputs-btn" type="button">Generate Position Inputs</button>
+                <button id="delete-inputs-btn" type="button">Remove Position Inputs</button>
 
                 <div id="module-position-inputs" class="position-inputs">
                     <!-- Dynamically generated position inputs will appear here -->
@@ -91,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
-        this.velocity.x = velocityX + rotX;
-        this.velocity.y = velocityY + rotY;
+        // Combine translation and rotation (now in robot frame)
+        this.velocity.x = robotVelX + rotX;
+        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
@@ -171,6 +193,7 @@ const maxSpeedOutput = document.getElementById('max-speed-value');
 // Get button elements
 const resetBtn = document.getElementById('reset-btn');
 const generateInputsBtn = document.getElementById('generate-inputs-btn');
+const clearInputsBtn = document.getElementById('delete-inputs-btn');
 const applyCustomBtn = document.getElementById('apply-custom-btn');
 
 // Preset buttons
@@ -266,11 +289,77 @@ preset8WheelBtn.addEventListener('click', () => {
     updateModuleDisplays(robot);
 });
 
+generateInputsBtn.addEventListener('click', () => {
+    const count = parseInt(moduleCountInput.value);
+
+    if (isNaN(count) || count < 2) {
+        alert('Please enter a valid number of modules above or equal to 2.');
+        return;
+    }
+    generateModuleInputs(count);
+    applyCustomBtn.style.display = 'block';
+});
+
+clearInputsBtn.addEventListener('click', () => {
+    generateModuleInputs(0);
+    applyCustomBtn.style.display = 'none';
+});
+
+applyCustomBtn.addEventListener('click', () => {
+    const container = document.getElementById('module-position-inputs');
+    const moduleElements = container.childNodes;
+
+    const customModules = [];
+    for (let i = 0; i < moduleElements.length; i++) {
+        const xInput = document.getElementById(`module-${i}-x`);
+        const yInput = document.getElementById(`module-${i}-y`);
+        const nameInput = document.getElementById(`module-${i}-name`);
+
+        const x = parseFloat(xInput.value);
+        const y = parseFloat(yInput.value);
+        const name = nameInput.value.trim();
+
+        customModules.push({ x, y, name });
+    }
+
+    robot.setModules(customModules);
+    robot.setName("Custom Configuration");
+    createModuleDisplays(robot);
+    updateModuleDisplays(robot);
+});
+
 /*
 * END LISTENER CODE
 * BEGIN DYNAMIC DOM FUNCTIONS
 */
 
+function generateModuleInputs(count) {
+    const container = document.getElementById('module-position-inputs');
+    container.innerHTML = ''; // Clear existing inputs
+
+    for (let i = 0; i < count; i++) {
+        const moduleFieldset = document.createElement('fieldset');
+        moduleFieldset.className = 'module-input-group';
+        moduleFieldset.innerHTML = `
+        <legend>Module ${i + 1}</legend>
+
+        <div class="control-group">
+            <label for="module-${i}-name">Module Name</label>
+            <input type="text" id="module-${i}-name" value="Module ${i + 1}" required>
+        </div>
+        <div class="control-group">
+            <label for="module-${i}-x">X Position (pixels)</label>
+            <input type="number" id="module-${i}-x" step="1" value="0" required>
+        </div>
+        <div class="control-group">
+            <label for="module-${i}-y">Y Position (pixels)</label>
+            <input type="number" id="module-${i}-y" step="0.1" value="0" required>
+        </div>
+        `;
+        container.appendChild(moduleFieldset);
+    }
+}
+
 function createModuleDisplays(robot) {
     const grid = document.getElementById('module-grid');
     grid.innerHTML = ''; // Delete any pre-existing elements before creating new ones
@@ -302,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`);
@@ -327,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;
@@ -396,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;
@@ -413,6 +510,8 @@ function drawRobot(ctx, robot) {
     ctx.stroke();
 
     modules.forEach(module => drawModule(ctx, module));
+
+    ctx.restore(); // Restore to remove rotation
 }
 
 
@@ -424,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);
@@ -444,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;
-    yGridOffset = (yGridOffset + (worldVy / offsetSpeedDivisor)) % gridSquareSize;
+    xGridOffset = (xGridOffset + (xSpeed / 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);
-    drawRobot(ctx, robot);
+    drawGrid(ctx, canvas.width * 2, gridSquareSize, xGridOffset, yGridOffset);
+    drawRobot(ctx, robot, robot.gyroHeading);
 
     // Do it all over again
     ctx.restore();

styles.css

@@ -321,4 +321,44 @@ button:hover {
   padding: var(--spacing-small);
   margin-bottom: var(--spacing-small);
   color: var(--text-secondary);
+}
+
+.control-group {
+  margin-bottom: var(--spacing-small);
+  display: flex;
+  flex-direction: column;
+}
+
+.position-inputs {
+  max-height: 400px;
+  overflow-y: auto;
+  padding: var(--spacing-small);
+  background-color: var(--background-dark);
+  border-radius: var(--border-radius-sm);
+  margin-top: var(--spacing-small);
+  display: grid;
+  grid-template-columns: repeat(auto-fit, minmax(200px, 1fr));
+}
+
+.preset-buttons {
+  display: grid;
+  grid-template-columns: repeat(auto-fit, minmax(150px, 1fr));
+}
+
+.module-display {
+  background-color: var(--background-dark);
+  border: 2px solid var(--border-color);
+  border-radius: var(--border-radius-sm);
+  padding: var(--spacing-small);
+}
+
+.readout {
+  display: flex;
+  justify-content: space-between;
+  margin-bottom: calc(var(--spacing-small) / 2);
+}
+
+.readout .value {
+  color: var(--text-light);
+  font-weight: bold;
 }