⚙️ 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: 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 unnecessary - 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.

# Conceptual Understanding

How does swerve work?

Concepts of linear algebra are heavily prevalent when formulating control vectors for the robots. There are several types of paths that robots can take. This varies from spline curves to straight lines.

How is a straight line formed?

TrajectoryConfig trajectoryConfiguration = new TrajectoryConfig(1, 1);

What does this line do? Basically, this sets the maximum speed that the robot can move with during its path to its destination.

Spline.ControlVector controlVectorStart = new Spline.ControlVector(new double[]{0, 0, 0}, new double[]{0, 0, 0});
Spline.ControlVector controlVectorEnd = new Spline.ControlVector(new double[]{0, 0, 0}, new double[]{0, 0, 0});

These lines of code just establish the starting an ending positions for the robot during its path. The first bracket in the starting control vector with 0,0,0 represent the starting x position, x velocity, and x rotation of the robot. The second bracket with 0,0,0 in the control vector start represents the initial y-position, y-velocity, and y-rotation. The same applied for the control vector end.

controlVectorStart.x = new double[]{currentPose2d.getX(), 1, 0};
controlVectorStart.y = new double[]{currentPose2d.getY(), 1, 0};
controlVectorEnd.x = new double[]{BlueATarget.getX(), 1, 0};
controlVectorEnd.y = new double[]{BlueATarget.getY(), 0, 0};

For controlVectorStart.x, the code is setting the x-position of the starting position vector to the current x-position of the robot, which is mathematically calculated using PhotonVision systems on the robot, or limelight.

For controlVectorStart.y, the code is setting the y-position of the starting position vector to the current y-position of the robot.

For the controlVectorEnd.x, the x-position is set to the x-position of the desired target.

For the controlVectorEnd.y, the x-position is set to the y-position of the desired target.

QuinticHermiteSpline curvedPathway = new QuinticHermiteSpline(controlVectorStart.x, controlVectorStart.y, controlVectorEnd.x, controlVectorEnd.y);

The variable curvedPathway is an object, and this is basically the spline curve filled with several position points that the robot will follow.

In order for a trajectory to be made from the spline, it needs an array of several 2 dimensional points with x and y coordinates that the robot will follow.

Spline[] arrayOfSplines = {curvedPathway};
List<PoseWithCurvature> pathway = TrajectoryGenerator.splinePointsFromSplines(arrayOfSplines);

This line of code generates the list of points that the robot will go to one by one in order to move in the spline curve, and it generates the points from the spline object curvedPathway.

List<Pose2d> twoDimensionalPoints = new ArrayList<>();
for (PoseWithCurvature curvedPose : pathway) {
    twoDimensionalPoints.add(curvedPose.poseMeters);
}

In the first line of code, a list named twoDimensional points is created. These points consist of x and y coordinates. The for loop is filling the list twoDimensionalPoints with points from the spline generated earlier.

trajectory = TrajectoryGenerator.generateTrajectory(twoDimensionalPoints, trajectoryConfiguration);

This line of code generated a trajectory vector for the robot to travel to its intended destination, with its inputs being the list of points from the spline and the trajectoryConfiguration, which was the maximum velocity and acceleration the robot was set to travel with during its travel path.