878 lines
29 KiB
JavaScript
878 lines
29 KiB
JavaScript
/*
|
|
* BEGIN CLASS DECLARATIONS
|
|
*/
|
|
|
|
import GrahamScan from "./vendor/lucio/graham-scan.mjs";
|
|
|
|
// 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, 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 (now in robot frame)
|
|
this.velocity.x = robotVelX + rotX;
|
|
this.velocity.y = robotVelY + rotY;
|
|
|
|
// Calculate speed and angle (in robot frame)
|
|
this.speed = this.velocity.magnitude();
|
|
this.angle = this.velocity.angle();
|
|
}
|
|
}
|
|
|
|
// Swerve drive class to represent the robot as a whole
|
|
class SwerveDrive {
|
|
constructor(modulePositionsAndNames, robotName) {
|
|
this.setModules(modulePositionsAndNames);
|
|
this.setName(robotName);
|
|
this.gyroHeading = 0; // Simulated gyro heading in radians
|
|
}
|
|
|
|
setName(robotName) {
|
|
this.name = robotName;
|
|
}
|
|
|
|
setModules(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)
|
|
);
|
|
}
|
|
|
|
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) {
|
|
// Store the requested turn speed for later calculation of actual turn speed
|
|
this.requestedTurnSpeed = turnSpeed;
|
|
|
|
// Take in a requested speeds and update every module (but don't update heading yet)
|
|
this.modules.forEach(module =>
|
|
module.calculateState(velocityX, velocityY, turnSpeed, this.gyroHeading)
|
|
);
|
|
|
|
// 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);
|
|
let scale = 1.0;
|
|
if (maxCalculated > maxModuleSpeed) {
|
|
scale = maxModuleSpeed / maxCalculated;
|
|
this.modules.forEach(module => {
|
|
module.velocity.x *= scale;
|
|
module.velocity.y *= scale;
|
|
module.speed = module.velocity.magnitude();
|
|
module.angle = module.velocity.angle();
|
|
});
|
|
}
|
|
|
|
// Update heading with the actual turn speed (scaled if modules were limited)
|
|
const actualTurnSpeed = turnSpeed * scale;
|
|
this.updateHeading(actualTurnSpeed, deltaTime);
|
|
this.actualTurnSpeed = actualTurnSpeed;
|
|
}
|
|
|
|
getActualVelocity() {
|
|
// Calculate the actual robot velocity from the average of module velocities
|
|
// This returns the velocity in robot-relative coordinates
|
|
if (this.modules.length === 0) return new Vec2D(0, 0);
|
|
|
|
let sumX = 0;
|
|
let sumY = 0;
|
|
|
|
// Average the module velocities (they're in robot frame)
|
|
this.modules.forEach(module => {
|
|
sumX += module.velocity.x;
|
|
sumY += module.velocity.y;
|
|
});
|
|
|
|
const avgX = sumX / this.modules.length;
|
|
const avgY = sumY / this.modules.length;
|
|
|
|
// Transform back to field-relative coordinates
|
|
const cosHeading = Math.cos(this.gyroHeading);
|
|
const sinHeading = Math.sin(this.gyroHeading);
|
|
|
|
const fieldVelX = avgX * cosHeading - avgY * sinHeading;
|
|
const fieldVelY = avgX * sinHeading + avgY * cosHeading;
|
|
|
|
return new Vec2D(fieldVelX, fieldVelY);
|
|
}
|
|
}
|
|
|
|
// Preset robot generators
|
|
const PresetConfigs = {
|
|
twoWheel: (size) => [
|
|
{ x: size / 2, y: 0, name: "Left" },
|
|
{ x: -size / 2, y: 0, name: "Right" }
|
|
],
|
|
|
|
threeWheel: (size) => {
|
|
const radius = size / 2;
|
|
return [
|
|
{ x: radius * Math.cos(Math.PI / 2), y: radius * Math.sin(Math.PI / 2), name: "Front" },
|
|
{ x: radius * Math.cos(Math.PI / 2 + 2 * Math.PI / 3), y: radius * Math.sin(Math.PI / 2 + 2 * Math.PI / 3), name: "Back Left" },
|
|
{ x: radius * Math.cos(Math.PI / 2 + 4 * Math.PI / 3), y: radius * Math.sin(Math.PI / 2 + 4 * Math.PI / 3), name: "Back Right" }
|
|
];
|
|
},
|
|
|
|
fourWheelSquare: (size) => {
|
|
const half = size / 2;
|
|
return [
|
|
{ x: half, y: half, name: "FL" },
|
|
{ x: half, y: -half, name: "FR" },
|
|
{ x: -half, y: half, name: "BL" },
|
|
{ x: -half, y: -half, name: "BR" }
|
|
];
|
|
},
|
|
|
|
fourWheelRectangle: (size) => {
|
|
const width = size * 0.5;
|
|
const length = size;
|
|
return [
|
|
{ x: length / 2, y: width / 2, name: "FL" },
|
|
{ x: length / 2, y: -width / 2, name: "FR" },
|
|
{ x: -length / 2, y: width / 2, name: "BL" },
|
|
{ x: -length / 2, y: -width / 2, name: "BR" }
|
|
];
|
|
},
|
|
|
|
sixWheel: (size) => {
|
|
const radius = size / 2;
|
|
const modules = [];
|
|
for (let i = 0; i < 6; i++) {
|
|
const angle = (Math.PI / 2) + (i * Math.PI / 3);
|
|
modules.push({
|
|
x: radius * Math.cos(angle),
|
|
y: radius * Math.sin(angle),
|
|
name: `Module ${i + 1}`
|
|
});
|
|
}
|
|
return modules;
|
|
},
|
|
|
|
eightWheelOctogon: (size) => {
|
|
const radius = size / 2;
|
|
const modules = [];
|
|
for (let i = 0; i < 8; i++) {
|
|
const angle = (Math.PI / 2) + (i * Math.PI / 4);
|
|
modules.push({
|
|
x: radius * Math.cos(angle),
|
|
y: radius * Math.sin(angle),
|
|
name: `Module ${i + 1}`
|
|
});
|
|
}
|
|
return modules;
|
|
},
|
|
|
|
eightWheelSquare: (size) => {
|
|
const full = size;
|
|
const half = size / 2;
|
|
return [
|
|
{ x: full, y: full, name: "Outer FL" },
|
|
{ x: full, y: -full, name: "Outer FR" },
|
|
{ x: -full, y: full, name: "Outer BL" },
|
|
{ x: -full, y: -full, name: "Outer BR" },
|
|
{ x: half, y: half, name: "Inner FL" },
|
|
{ x: half, y: -half, name: "Inner FR" },
|
|
{ x: -half, y: half, name: "Inner BL" },
|
|
{ x: -half, y: -half, name: "Inner BR" }
|
|
];
|
|
},
|
|
|
|
twelveWheelHexagon: (size) => {
|
|
const outerRadius = size;
|
|
const innerRadius = size / 2;
|
|
const modules = [];
|
|
for (let i = 0; i < 6; i++) {
|
|
const angle = (Math.PI / 2) + (i * Math.PI / 3);
|
|
modules.push({
|
|
x: outerRadius * Math.cos(angle),
|
|
y: outerRadius * Math.sin(angle),
|
|
name: `Module ${i + 1}`
|
|
});
|
|
|
|
modules.push({
|
|
x: innerRadius * Math.cos(angle),
|
|
y: innerRadius * Math.sin(angle),
|
|
name: `Module ${i + 7}`
|
|
});
|
|
}
|
|
return modules;
|
|
},
|
|
|
|
sixteenWheelOctogon: (size) => {
|
|
const outerRadius = size;
|
|
const innerRadius = size / 2;
|
|
const modules = [];
|
|
for (let i = 0; i < 8; i++) {
|
|
const angle = (Math.PI / 2) + (i * Math.PI / 4);
|
|
modules.push({
|
|
x: outerRadius * Math.cos(angle),
|
|
y: outerRadius * Math.sin(angle),
|
|
name: `Module ${i + 1}`
|
|
});
|
|
|
|
modules.push({
|
|
x: innerRadius * Math.cos(angle),
|
|
y: innerRadius * Math.sin(angle),
|
|
name: `Module ${i + 9}`
|
|
});
|
|
}
|
|
return modules;
|
|
},
|
|
};
|
|
|
|
/*
|
|
* END CLASS DECLARATIONS
|
|
* BEGIN DOM VARIABLES
|
|
*/
|
|
|
|
// Get all control elements
|
|
const vxSlider = document.getElementById('vx-slider');
|
|
const vySlider = document.getElementById('vy-slider');
|
|
const omegaSlider = document.getElementById('omega-slider');
|
|
const maxSpeedSlider = document.getElementById('max-speed-slider');
|
|
const moduleCountInput = document.getElementById('module-count');
|
|
|
|
// Get all output elements
|
|
const vxOutput = document.getElementById('vx-value');
|
|
const vyOutput = document.getElementById('vy-value');
|
|
const omegaOutput = document.getElementById('omega-value');
|
|
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');
|
|
|
|
// Get control mode elements
|
|
const controlModeToggle = document.getElementById('control-mode-toggle');
|
|
const sliderControls = document.getElementById('slider-controls');
|
|
const keyboardControls = document.getElementById('keyboard-controls');
|
|
const keyboardMaxSpeed = document.getElementById('keyboard-max-speed');
|
|
const keyboardMaxSpeedOutput = document.getElementById('keyboard-max-speed-value');
|
|
const keyboardMaxRotation = document.getElementById('keyboard-max-rotation');
|
|
const keyboardMaxRotationOutput = document.getElementById('keyboard-max-rotation-value');
|
|
|
|
// Preset buttons
|
|
const preset2WheelBtn = document.getElementById('preset-2wheel');
|
|
const preset3WheelBtn = document.getElementById('preset-3wheel');
|
|
const preset4WheelBtn = document.getElementById('preset-4wheel');
|
|
const preset4RectBtn = document.getElementById('preset-4rect');
|
|
const preset6WheelBtn = document.getElementById('preset-6wheel');
|
|
const preset8WheelBtn = document.getElementById('preset-8wheel');
|
|
const preset8SquareBtn = document.getElementById('preset-8square');
|
|
const preset12HexBtn = document.getElementById('preset-12hex');
|
|
const preset16OctBtn = document.getElementById('preset-16oct');
|
|
|
|
/*
|
|
* END DOM VARIABLES
|
|
* BEGIN CONTROL MODE AND INPUT DEVICE VARIABLES
|
|
*/
|
|
|
|
// Control mode state
|
|
let isManualInputMode = false; // true = keyboard/gamepad mode, false = slider mode
|
|
|
|
// Keyboard state tracking
|
|
const keyState = {
|
|
w: false, // Forward
|
|
a: false, // Left
|
|
s: false, // Backward
|
|
d: false, // Right
|
|
q: false, // Counter-clockwise
|
|
e: false // Clockwise
|
|
};
|
|
|
|
// Current manual input velocities (from keyboard or gamepad)
|
|
let manualInputVelX = 0;
|
|
let manualInputVelY = 0;
|
|
let manualInputOmega = 0;
|
|
|
|
/*
|
|
* END CONTROL MODE AND INPUT DEVICE VARIABLES
|
|
* BEGIN LISTENER CODE
|
|
*/
|
|
|
|
maxSpeedSlider.addEventListener('input', (e) => {
|
|
maxSpeedOutput.textContent = e.target.value;
|
|
});
|
|
maxSpeedOutput.textContent = maxSpeedSlider.value;
|
|
|
|
resetBtn.addEventListener('click', (e) => {
|
|
vxSlider.value = 0;
|
|
vySlider.value = 0;
|
|
omegaSlider.value = 0;
|
|
|
|
vxOutput.textContent = parseFloat(vxSlider.value);
|
|
vyOutput.textContent = parseFloat(vySlider.value);
|
|
omegaOutput.textContent = parseFloat(omegaSlider.value);
|
|
});
|
|
|
|
// Keyboard control sliders
|
|
keyboardMaxSpeed.addEventListener('input', (e) => {
|
|
keyboardMaxSpeedOutput.textContent = parseFloat(e.target.value);
|
|
});
|
|
keyboardMaxSpeedOutput.textContent = parseFloat(keyboardMaxSpeed.value);
|
|
|
|
keyboardMaxRotation.addEventListener('input', (e) => {
|
|
keyboardMaxRotationOutput.textContent = parseFloat(e.target.value);
|
|
});
|
|
keyboardMaxRotationOutput.textContent = parseFloat(keyboardMaxRotation.value);
|
|
|
|
// Control mode toggle
|
|
controlModeToggle.addEventListener('click', () => {
|
|
isManualInputMode = !isManualInputMode;
|
|
|
|
if (isManualInputMode) {
|
|
// Switch to manual input mode (keyboard/gamepad)
|
|
sliderControls.style.display = 'none';
|
|
keyboardControls.style.display = 'block';
|
|
controlModeToggle.textContent = 'Switch to Slider Controls';
|
|
|
|
// Reset slider values when switching to manual input
|
|
vxSlider.value = 0;
|
|
vySlider.value = 0;
|
|
omegaSlider.value = 0;
|
|
vxOutput.textContent = '0';
|
|
vyOutput.textContent = '0';
|
|
omegaOutput.textContent = '0';
|
|
} else {
|
|
// Switch to slider mode
|
|
sliderControls.style.display = 'block';
|
|
keyboardControls.style.display = 'none';
|
|
controlModeToggle.textContent = 'Switch to Keyboard Controls (WASD + QE)';
|
|
|
|
// Reset manual input state
|
|
Object.keys(keyState).forEach(key => keyState[key] = false);
|
|
manualInputVelX = 0;
|
|
manualInputVelY = 0;
|
|
manualInputOmega = 0;
|
|
}
|
|
});
|
|
|
|
// Keyboard event listeners
|
|
document.addEventListener('keydown', (e) => {
|
|
if (!isManualInputMode) return;
|
|
|
|
const key = e.key.toLowerCase();
|
|
if (key in keyState) {
|
|
keyState[key] = true;
|
|
e.preventDefault();
|
|
updateManualInputVelocities();
|
|
}
|
|
});
|
|
|
|
document.addEventListener('keyup', (e) => {
|
|
if (!isManualInputMode) return;
|
|
|
|
const key = e.key.toLowerCase();
|
|
if (key in keyState) {
|
|
keyState[key] = false;
|
|
e.preventDefault();
|
|
updateManualInputVelocities();
|
|
}
|
|
});
|
|
|
|
// Function to update velocities based on manual input devices (keyboard/gamepad)
|
|
function updateManualInputVelocities() {
|
|
const maxSpeed = parseFloat(keyboardMaxSpeed.value);
|
|
const maxRotation = parseFloat(keyboardMaxRotation.value);
|
|
|
|
// Calculate translation velocities from keyboard input
|
|
manualInputVelX = 0;
|
|
manualInputVelY = 0;
|
|
|
|
if (keyState.d) manualInputVelX += maxSpeed; // Right
|
|
if (keyState.a) manualInputVelX -= maxSpeed; // Left
|
|
if (keyState.w) manualInputVelY += maxSpeed; // Forward
|
|
if (keyState.s) manualInputVelY -= maxSpeed; // Backward
|
|
|
|
// Calculate rotation velocity from keyboard input
|
|
manualInputOmega = 0;
|
|
if (keyState.e) manualInputOmega += maxRotation; // Clockwise
|
|
if (keyState.q) manualInputOmega -= maxRotation; // Counter-clockwise
|
|
|
|
// TODO: Add gamepad input processing here
|
|
}
|
|
|
|
// Preset button event listeners
|
|
preset2WheelBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.twoWheel(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("2-Wheel Differential");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset3WheelBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.threeWheel(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("3-Wheel Triangle");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset4WheelBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.fourWheelSquare(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("4-Wheel Square");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset4RectBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.fourWheelRectangle(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("4-Wheel Rectangle");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset6WheelBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.sixWheel(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("6-Wheel Hexagon");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset8WheelBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.eightWheelOctogon(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("8-Wheel Octogon");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset8SquareBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.eightWheelSquare(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("8-Wheel Square");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset12HexBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.twelveWheelHexagon(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("12-Wheel Hexagon");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
preset16OctBtn.addEventListener('click', () => {
|
|
const positions = PresetConfigs.sixteenWheelOctogon(robotSize);
|
|
robot.setModules(positions);
|
|
robot.setName("16-Wheel Octogon");
|
|
createModuleDisplays(robot);
|
|
updateModuleDisplays(robot);
|
|
});
|
|
|
|
|
|
|
|
generateInputsBtn.addEventListener('click', () => {
|
|
const count = parseInt(moduleCountInput.value);
|
|
|
|
if (isNaN(count) || count < 2 || count > 64) {
|
|
alert('Please enter a valid number of modules between 2 and 64.');
|
|
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 to calculate evenly spaced module positions in circular layers
|
|
function calculateModulePositions(count) {
|
|
if (count <= 0) return [];
|
|
|
|
const baseRadius = 50; // Base radius for the first layer
|
|
const positions = [];
|
|
let remainingModules = count;
|
|
let numberOfLayers = Math.ceil(count / 6);
|
|
let modulesPerLayer = Math.ceil(count / numberOfLayers);
|
|
let angleOffset = 0;
|
|
let currentLayer = 0;
|
|
let moduleIndex = 0;
|
|
|
|
while (remainingModules > 0) {
|
|
// Determine modules for this layer
|
|
let modulesInThisLayer;
|
|
|
|
if (remainingModules <= modulesPerLayer) {
|
|
// Last layer gets all remaining modules
|
|
modulesInThisLayer = remainingModules;
|
|
} else {
|
|
// All other layers: try to distribute evenly with max modulesPerLayer
|
|
const remainingLayers = Math.ceil(remainingModules / modulesPerLayer);
|
|
modulesInThisLayer = Math.min(modulesPerLayer, Math.ceil(remainingModules / remainingLayers));
|
|
}
|
|
|
|
// Calculate radius for this layer
|
|
const radius = currentLayer === 0 ? baseRadius : baseRadius * (1 + currentLayer * 0.75);
|
|
|
|
// Calculate positions for modules in this layer
|
|
for (let i = 0; i < modulesInThisLayer; i++) {
|
|
const angle = (angleOffset + 2 * Math.PI * i) / modulesInThisLayer;
|
|
const x = Math.round(radius * Math.cos(angle));
|
|
const y = Math.round(radius * Math.sin(angle));
|
|
|
|
positions.push({
|
|
x: x,
|
|
y: y,
|
|
name: `Layer ${currentLayer + 1} Module ${(moduleIndex % modulesPerLayer) + 1}`
|
|
});
|
|
|
|
moduleIndex++;
|
|
}
|
|
angleOffset += Math.PI;
|
|
|
|
remainingModules -= modulesInThisLayer;
|
|
currentLayer++;
|
|
}
|
|
|
|
return positions;
|
|
}
|
|
|
|
function generateModuleInputs(count) {
|
|
const container = document.getElementById('module-position-inputs');
|
|
container.innerHTML = ''; // Clear existing inputs
|
|
|
|
// Calculate evenly spaced positions
|
|
const positions = calculateModulePositions(count);
|
|
|
|
for (let i = 0; i < count; i++) {
|
|
const position = positions[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="${position.name}" 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="${position.x}" 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="${position.y}" 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
|
|
|
|
const modules = robot.modules;
|
|
modules.forEach((module, i) => {
|
|
const article = document.createElement('article');
|
|
article.className = 'module-display';
|
|
const name = module.name;
|
|
|
|
article.innerHTML = `
|
|
<h3>${name}</h3>
|
|
<div class="readout">
|
|
<span class="label">Angle:</span>
|
|
<span id="module-${i}-angle" class="value">0.0°</span>
|
|
</div>
|
|
<div class="readout">
|
|
<span class="label">Speed:</span>
|
|
<span id="module-${i}-speed" class="value">0.00 pixels/s</span>
|
|
</div>
|
|
`;
|
|
grid.appendChild(article);
|
|
});
|
|
}
|
|
|
|
function updateModuleDisplays(robot) {
|
|
const configName = document.getElementById('config-name');
|
|
configName.textContent = robot.name;
|
|
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`);
|
|
const speedElement = document.getElementById(`module-${i}-speed`);
|
|
|
|
if (angleElement && speedElement) {
|
|
const angleDeg = (module.angle * 180 / Math.PI).toFixed(1);
|
|
angleElement.textContent = `${angleDeg}°`;
|
|
speedElement.textContent = `${module.speed.toFixed(2)} pixels/s`;
|
|
}
|
|
});
|
|
}
|
|
|
|
/*
|
|
* END DYNAMIC DOM FUNCTIONS
|
|
* BEGIN ANIMATION CODE
|
|
*/
|
|
|
|
// Get the canvas and context as constants
|
|
const canvas = document.getElementById('swerve-canvas');
|
|
const ctx = canvas.getContext('2d');
|
|
|
|
// Get CSS variables for use in canvas
|
|
const rootStyles = getComputedStyle(document.documentElement);
|
|
|
|
function drawGrid(ctx, sideLength, gridSquareSize, xOffset, yOffset) {
|
|
ctx.save();
|
|
|
|
ctx.strokeStyle = rootStyles.getPropertyValue('--grid-color');
|
|
ctx.lineWidth = 1;
|
|
const startX = (-sideLength / 2) - xOffset;
|
|
const endX = (sideLength / 2) - xOffset;
|
|
const startY = (-sideLength / 2) - yOffset;
|
|
const endY = (sideLength / 2) - yOffset;
|
|
|
|
// Draw vertical lines
|
|
for (let i = startX; i <= endX; i += gridSquareSize) {
|
|
ctx.beginPath();
|
|
ctx.moveTo(i, -sideLength / 2);
|
|
ctx.lineTo(i, sideLength / 2);
|
|
ctx.stroke();
|
|
}
|
|
|
|
// Draw horizontal lines
|
|
for (let i = startY; i <= endY; i += gridSquareSize) {
|
|
ctx.beginPath();
|
|
ctx.moveTo(-sideLength / 2, i);
|
|
ctx.lineTo(sideLength / 2, i);
|
|
ctx.stroke();
|
|
}
|
|
|
|
ctx.restore();
|
|
}
|
|
|
|
function drawModule(ctx, module) {
|
|
const x = module.position.x;
|
|
const y = module.position.y;
|
|
const arrowLength = Math.max(module.speed / 2, 5);
|
|
|
|
ctx.save();
|
|
ctx.translate(x, y);
|
|
|
|
ctx.fillStyle = rootStyles.getPropertyValue('--swerve-fill-color');
|
|
ctx.beginPath();
|
|
ctx.arc(0, 0, 10, 0, Math.PI * 2);
|
|
ctx.fill();
|
|
|
|
ctx.strokeStyle = rootStyles.getPropertyValue('--swerve-module-color');
|
|
ctx.lineWidth = 4;
|
|
ctx.stroke();
|
|
|
|
// Draw velocity arrow if module is moving
|
|
if (module.speed > 0.01) {
|
|
ctx.strokeStyle = rootStyles.getPropertyValue('--swerve-arrow-color');
|
|
ctx.fillStyle = rootStyles.getPropertyValue('--swerve-arrow-color');
|
|
ctx.lineWidth = 4;
|
|
|
|
const endX = arrowLength * Math.cos(module.angle);
|
|
const endY = arrowLength * Math.sin(module.angle);
|
|
|
|
// Arrow line
|
|
ctx.beginPath();
|
|
ctx.moveTo(0, 0);
|
|
ctx.lineTo(endX, endY);
|
|
ctx.stroke();
|
|
}
|
|
|
|
|
|
ctx.restore();
|
|
}
|
|
|
|
|
|
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;
|
|
|
|
let hull = [];
|
|
// Get the convex hull of the robot if there are more than 3 modules
|
|
if (robot.modules.length > 3) {
|
|
const grahamScan = new GrahamScan();
|
|
grahamScan.setPoints(robot.modules.map((module) => [module.position.x, module.position.y]));
|
|
hull = grahamScan.getHull();
|
|
} else {
|
|
hull = robot.modules.map((module) => [module.position.x, module.position.y]);
|
|
}
|
|
|
|
|
|
// Draw the convex hull as the robot frame
|
|
ctx.beginPath();
|
|
ctx.moveTo(hull[0][0], hull[0][1]);
|
|
for (let i = 1; i < hull.length; i++) {
|
|
ctx.lineTo(hull[i][0], hull[i][1]);
|
|
}
|
|
ctx.closePath();
|
|
ctx.fill();
|
|
ctx.stroke();
|
|
|
|
// Draw all modules (not just hull modules)
|
|
robot.modules.forEach(module => drawModule(ctx, module, heading));
|
|
|
|
ctx.restore(); // Restore to remove rotation
|
|
}
|
|
|
|
|
|
// Initialize Variables
|
|
const robotSize = 200;
|
|
const defaultModulePositions = PresetConfigs.fourWheelSquare(robotSize);
|
|
const robot = new SwerveDrive(defaultModulePositions, "4-Wheel Square");
|
|
createModuleDisplays(robot);
|
|
let xSpeed = 0;
|
|
let ySpeed = 0;
|
|
let turnSpeed = -1;
|
|
|
|
let gridSquareSize = 50;
|
|
let xGridOffset = 0;
|
|
let yGridOffset = 0;
|
|
robot.drive(xSpeed, ySpeed, 0, 500);
|
|
|
|
function animate() {
|
|
// Clear and set up canvas
|
|
ctx.clearRect(0, 0, canvas.width, canvas.height);
|
|
ctx.save();
|
|
ctx.translate(canvas.width / 2, canvas.height / 2);
|
|
|
|
// Update speeds based on control mode
|
|
if (isManualInputMode) {
|
|
xSpeed = manualInputVelX;
|
|
ySpeed = -manualInputVelY; // Negative because canvas Y axis is inverted
|
|
turnSpeed = manualInputOmega;
|
|
} else {
|
|
xSpeed = parseFloat(vxSlider.value);
|
|
ySpeed = -parseFloat(vySlider.value);
|
|
turnSpeed = parseFloat(omegaSlider.value);
|
|
}
|
|
|
|
// 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);
|
|
|
|
// Get the actual robot velocity (after scaling to max module speed) for grid animation
|
|
const actualVelocity = robot.getActualVelocity();
|
|
|
|
// Update control outputs with actual speeds
|
|
if (isManualInputMode) {
|
|
// In manual input mode (keyboard/gamepad), show the current values
|
|
keyboardMaxSpeedOutput.textContent = `Max: ${keyboardMaxSpeed.value} | Current: ${Math.max(Math.abs(actualVelocity.x), Math.abs(actualVelocity.y)).toFixed(1)}`;
|
|
keyboardMaxRotationOutput.textContent = `Max: ${keyboardMaxRotation.value} | Current: ${Math.abs(robot.actualTurnSpeed || 0).toFixed(2)}`;
|
|
} else {
|
|
// In slider mode, show requested vs actual
|
|
vxOutput.textContent = `Requested: ${vxSlider.value} | Actual: ${actualVelocity.x.toFixed(2)}`;
|
|
vyOutput.textContent = `Requested: ${vySlider.value} | Actual: ${-actualVelocity.y.toFixed(2)}`;
|
|
omegaOutput.textContent = `Requested: ${omegaSlider.value} | Actual: ${(robot.actualTurnSpeed || 0).toFixed(2)}`;
|
|
}
|
|
|
|
// Animate the grid
|
|
let offsetSpeedDivisor = (100 - gridSquareSize <= 0 ? 1 : 100 - gridSquareSize);
|
|
|
|
// Update grid offsets based on robot movement
|
|
xGridOffset = (xGridOffset + (actualVelocity.x / offsetSpeedDivisor)) % gridSquareSize;
|
|
yGridOffset = (yGridOffset + (actualVelocity.y / offsetSpeedDivisor)) % gridSquareSize;
|
|
|
|
// 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);
|
|
drawRobot(ctx, robot, robot.gyroHeading);
|
|
|
|
// Do it all over again
|
|
ctx.restore();
|
|
requestAnimationFrame(animate);
|
|
}
|
|
|
|
animate(); |