Improve SwerveTeleopCommand

[ErikCald]

This is the command that well read the joystick inputs from the driver and then tell the SwerveSubsystem to go a certain X speed, Y speed and rotating speed. It can be greatly improved through acceleration limiters, slow modes, etc.

Here are some good examples from here teams:

• FRC Team 6328 Mechanical Advantage
• FRC Team 3161 Tronic Titans
• Our team in 2023

[ErikCald]

List of tasks ordered by how important they are:

• [ ] Improve deadband. Previously we did deadband on X and Y individually but that's not ideal. Instead, do deadband on the magnitude of X and Y. 6328 has code you can basically copy here: https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/commands/DriveWithJoysticks.java#L114
• [ ] We already have acceleration limiters using SlewRateLimiters but they are used before multiplying by the max speed and it should instead be after. This will result in the unit of the limiter being m/s/s which will be easier to tune.
• [ ] Implement a slow mode button. A button that changes the desired max speed which will remap the joystick to go slower, giving more resolution.
• [ ] Implement an outreach mode vs competition mode which will slow down the robot (to avoid damaging people’s carpet…). Could use networktables to set comp mode vs outreach mode but if FMS is attached, force comp mode.
• [ ] After improving this command, look back at Merge’s SwerveTeleop from last year. It used clever OOP code to allow the command to be extended and the rotation to be overridden by auto code but kept the driver controls of the X and Y (or any combination of X, Y, and rot overriden)

[ErikCald]

Here's the proper bit for converting linearMagnitude and linearDirection back into X, Y speeds. I put the code from the GeomUtil into this line. I'm not sure why this piece has to be so complicated.

    // Calcaulate new linear components
    Translation2d linearVelocity =
        new Pose2d(new Translation2d(), linearDirection)
            .transformBy(new Transform2d(new Translation2d(linearMagnitude, 0.0), new Rotation2d()))
            .getTranslation();