112 lines
3.8 KiB
JavaScript
112 lines
3.8 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 * omega;
|
|
const rotY = this.position.x * omega;
|
|
|
|
// 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) {
|
|
// 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');
|
|
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);
|
|
}); |