341 lines
11 KiB
JavaScript
341 lines
11 KiB
JavaScript
// 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
|
|
|
|
// 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;
|
|
|
|
// Calculate speed and angle
|
|
this.speed = this.velocity.magnitude();
|
|
this.angle = this.velocity.angle();
|
|
}
|
|
}
|
|
|
|
// Swerve drive class to represent the robot as a whole
|
|
class SwerveDrive {
|
|
constructor(modulePositionsAndNames) {
|
|
this.setModules(modulePositionsAndNames);
|
|
}
|
|
|
|
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)
|
|
);
|
|
}
|
|
|
|
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.velocity.x *= scale;
|
|
module.velocity.y *= scale;
|
|
module.speed = module.velocity.magnitude();
|
|
module.angle = module.velocity.angle();
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
// Preset robot generators
|
|
const PresetConfigs = {
|
|
twoWheel: (size) => [
|
|
{ x: 0, y: size / 2, name: "Left" },
|
|
{ x: 0, y: -size / 2, 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.7;
|
|
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;
|
|
},
|
|
|
|
eightWheel: (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;
|
|
}
|
|
};
|
|
|
|
// 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 applyCustomBtn = document.getElementById('apply-custom-btn');
|
|
|
|
// 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');
|
|
|
|
// Add event listeners for drive controls
|
|
vxSlider.addEventListener('input', (e) => {
|
|
vxOutput.textContent = parseFloat(e.target.value).toFixed(1);
|
|
});
|
|
|
|
vySlider.addEventListener('input', (e) => {
|
|
vyOutput.textContent = parseFloat(e.target.value).toFixed(1);
|
|
});
|
|
|
|
omegaSlider.addEventListener('input', (e) => {
|
|
omegaOutput.textContent = parseFloat(e.target.value).toFixed(1);
|
|
});
|
|
|
|
maxSpeedSlider.addEventListener('input', (e) => {
|
|
maxSpeedOutput.textContent = parseFloat(e.target.value).toFixed(1);
|
|
});
|
|
|
|
|
|
|
|
/*
|
|
* 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, rotation) {
|
|
ctx.save();
|
|
|
|
// Apply rotation transform
|
|
ctx.rotate(-rotation);
|
|
|
|
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) {
|
|
ctx.strokeStyle = rootStyles.getPropertyValue('--robot-frame-color')
|
|
ctx.fillStyle = rootStyles.getPropertyValue('--robot-fill-color');
|
|
ctx.lineWidth = 4;
|
|
|
|
const modules = robot.modules.sort((a, b) => Math.atan2(a.position.y, a.position.x) - Math.atan2(b.position.y, b.position.x));
|
|
|
|
ctx.beginPath();
|
|
ctx.moveTo(modules[0].position.x, modules[0].position.y);
|
|
for (let i = 1; i < modules.length; i++) {
|
|
ctx.lineTo(modules[i].position.x, modules[i].position.y);
|
|
}
|
|
ctx.closePath();
|
|
ctx.fill();
|
|
ctx.stroke();
|
|
|
|
modules.forEach(module => drawModule(ctx, module));
|
|
}
|
|
|
|
let robot = new SwerveDrive(PresetConfigs.eightWheel(200));
|
|
let xSpeed = -100;
|
|
let xDir = 1;
|
|
|
|
let ySpeed = -100;
|
|
let yDir = 0.8;
|
|
|
|
let turnSpeed = -1;
|
|
let turnDir = 0.01;
|
|
let robotRotation = 0; // Track cumulative robot rotation for grid display
|
|
|
|
let gridSquareSize = 25;
|
|
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 sliders
|
|
xSpeed = parseFloat(vxSlider.value);
|
|
ySpeed = parseFloat(vySlider.value);
|
|
turnSpeed = parseFloat(omegaSlider.value);
|
|
|
|
// 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;
|
|
|
|
// 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);
|
|
|
|
robot.drive(xSpeed, ySpeed, turnSpeed, 500);
|
|
|
|
// Do it all over again
|
|
ctx.restore();
|
|
requestAnimationFrame(animate);
|
|
}
|
|
|
|
animate(); |