⚙️ Swerve Drive Implementation & Commands

Swerve code can be intimidating - this document details the software architecture for the swerve drive, including constants, the primary subsystem logic, and essential commands for teleoperated driving, path following, and characterization.

Configuration: TheTunerConstantsClass

TheTunerConstantsclass 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 likekFrontLeftXPos), and gear ratios (kDriveGearRatio,kSteerGearRatio).

  • PID Gains: SeparateSlot0Configsare 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 highKP(50) and a smallKD(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: TheSwerveDriveSubsystem

The swerve drive subsystem is implemented using the AdvantageKit abstraction, relying on theSwerveDriveIOinterface. It integrates hardware control, odometry, and pathfinding setup. Refer toSubsystem IO Abstractionif 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: Theperiodic()method is responsible for continuously updating the pose estimator with time-stamped data from the gyroscope and modules.
  • Vision Integration: The methodsaddVisionMeasurement()andaddAutoVisionMeasurement()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 unnecessary - but cool to know - odometry info

PathPlanner Setup

The subsystem configures the PathPlannerAutoBuilderto enable autonomous routines. This includes defining:

  • State Getters/Setters: Methods likegetPose(),setPose(),getChassisSpeeds().
  • Drive Command: TherunVelocity()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 theLocalADStarAKimplementation for real-time obstacle avoidance and dynamic path generation.

Commands: TheDriveCommandsUtility 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-derivedChassisSpeedsfrom 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 targetPose2d(derived from aTransform2d) in a straight line, without using pathfinding.

  • Axis Control: Three PID controllers are used:pidX,pidY, andangleController.
  • 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(...)andfollowPoses(...)

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 PathPlannerPathPlannerPathobject, which includes motion constraints.
  • Sequence: The final command is a sequence: first, it usesAutoBuilder.pathfindToPoseto navigate dynamically from the current location to the path’s start point; second, it usesAutoBuilder.followPathto precisely follow the pre-generated curve or pose path.