⚙️ Swerve Drive Implementation & Commands

Configuration: The TunerConstants Class

The TunerConstants class serves as the constants file for all physical and control parameters of the swerve drivetrain. These values are crucial for accurate motion. The TunerConstants file is also generated by CTRE's Phoenix Tuner - none of this is written manually.

• Kinematic Parameters: Defines the physical layout and gearing, including the wheel radius (kWheelRadius), drive base size (module locations like kFrontLeftXPos), and gear ratios (kDriveGearRatio, kSteerGearRatio).
• PID Gains: Separate Slot0Configs are defined for both the Steer and Drive motors, including Proportional (KP), Integral (KI), Derivative (KD), Static Feedforward (KS), and Velocity Feedforward (KV). These must be tuned to prevent the robot from being jerky, too slow, accelerating too fast, and numerous other small issues.
• Steer Gains Example: The steering motor uses a high KP (50) and a small KD (0.5) to rapidly accelerate to the requested position.
• Motor/Sensor IDs: Specifies the CAN IDs for all TalonFX drive motors, TalonFX steer motors, and CANcoder absolute encoders for each of the four modules.
• Physical Constraints: Includes the theoretical maximum speed (kSpeedAt12Volts) and the current limit at which wheels are expected to slip (kSlipCurrent).

Subsystem Logic: The SwerveDriveSubsystem

The swerve drive subsystem is implemented using the AdvantageKit abstraction, relying on the SwerveDriveIO interface. It integrates hardware control, odometry, and pathfinding setup. Refer to Subsystem IO Abstraction if you forgot how it works!

Odometry and Pose Estimation

The subsystem maintains the robot's field position using a SwerveDrivePoseEstimator.

• Core Data: It fuses data from the Gyro (rawGyroRotation) and the positions of all four Swerve Modules (lastModulePositions).
• Update Loop: The periodic() method is responsible for continuously updating the pose estimator with time-stamped data from the gyroscope and modules.
• Vision Integration: The methods addVisionMeasurement() and addAutoVisionMeasurement() allow external vision systems (like Limelight or PhotonVision) to correct the estimated pose, improving accuracy by miles - relying on odometry alone is bound to fail as the wheels can slip frequently. See this for more unneccessary - but cool to know - odometry info

PathPlanner Setup

The subsystem configures the PathPlanner AutoBuilder to enable autonomous routines. This includes defining:

• State Getters/Setters: Methods like getPose(), setPose(), getChassisSpeeds().
• Drive Command: The runVelocity() method is used to execute the calculated path speeds.
• Control Parameters: Holonomic PID controllers (PPHolonomicDriveController) for both the X/Y translation and the rotation are configured.
• Pathfinding: It uses the LocalADStarAK implementation for real-time obstacle avoidance and dynamic path generation.

Commands: The DriveCommands Utility Class

This class contains static factory methods for generating reusable drive commands.

Teleoperated Driving Commands

joystickDrive(...)

This is the standard, field-relative drive command. It maps raw joystick inputs (X, Y, and Omega) to robot speeds.

• Input Mapping: It applies a deadband (DEADBAND = 0.1) to the joystick inputs to prevent unintentional drift. It then squares the magnitude (linearMagnitude * linearMagnitude) and rotation (omega * omega) for finer low-speed control.
• Field-Relative Control: It converts the joystick-derived ChassisSpeeds from robot-relative to field-relative using the current Gyro rotation (drive.getRotation()).
• Alliance Flipping: It automatically flips the drive direction (adds 180° to what angle the robot thinks it is looking) if the robot is on the Red Alliance, ensuring the controls are always relative to the driver's perspective.

joystickDriveAtAngle(...)

A specialized command for driving while maintaining or targeting a specific field-relative rotation. You can use this if you want the robot to aim at a target while moving, for example, if it was a shooter game.

• Linear Control: Linear motion (X and Y) is still controlled by the raw joystick input.
• Angular Control: Angular velocity is controlled by a Profiled PID Controller (angleController). This controller calculates the necessary value to move the current robot angle (drive.getRotation()) toward the supplier-provided target angle (rotationSupplier.get()) using a motion profile.

Path Following and Navigation Commands

goToTransform(...)

A command that uses separate Profiled PID Controllers for the X, Y, and rotation axes to drive the robot to a target Pose2d (derived from a Transform2d) in a straight line, without using pathfinding.

• Axis Control: Three PID controllers are used: pidX, pidY, and angleController.
• Stopping Condition: The command terminates (.until(...)) when the robot is within a small positional tolerance (0.07 meters in X and Y) and angular tolerance (5 degrees) of the target.

followCurve(...) and followPoses(...)

These commands utilize the PathPlanner library to generate and follow complex paths.

• Path Generation: The logic first converts the provided control points (followCurve) or poses (followPoses) into a PathPlanner PathPlannerPath object, which includes motion constraints.
• Sequence: The final command is a sequence: first, it uses AutoBuilder.pathfindToPose to navigate dynamically from the current location to the path's start point; second, it uses AutoBuilder.followPath to precisely follow the pre-generated curve or pose path.