Drive to Point

A Pose2d bundles three numbers: X in meters, Y in meters, and a Rotation2dheading. The robot's current position and the place you are sending it are both written this way. 0° points down the field.

Every command so far came out of a factory: mechanism.run(...) or Command.sequence(...), one expression with .named(...) on the end. This one does four things, and only one of them repeats.

1public class DriveToPoint extends ClassicCommand {
2 
3  public DriveToPoint(DriveMechanism drivetrain, Pose2d targetPose) {
4    super("DriveToPoint", drivetrain); // command name + required mechanisms
5  }
6 
7  @Override
8  protected void initialize() {}               // once, when the command starts
9 
10  @Override
11  protected void execute() {}                  // every loop, while it is active
12 
13  @Override
14  protected boolean isFinished() {             // every loop, right after execute
15    return false;                              // true = finish now
16  }
17 
18  @Override
19  protected void end(boolean interrupted) {}   // once, when it ends either way
20}

super(...) calls the constructor of the class you extended, here ClassicCommand. It takes the command name first, then every mechanism this command owns while it runs. That is where the telemetry name comes from. .named(...) belongs to the builder run(...) hands back, so calling it on a finished Command will not compile.

ClassicCommand is a file, not a framework class: 123 lines that turn your four methods into an ordinary Command. Before anything else, paste the branch's copy into src/main/java/frc/robot/utils/ClassicCommand.java and leave it alone. It is in the PR diff below.

One new file, commands/DriveToPoint.java. Six field declarations go in at the top, two of them plain: the drivetrain and the target pose. Three are PIDController fields with kP of 10 on X and Y and 7 on heading. The sixth is one SwerveRequest.ApplyFieldVelocity, built once and reused every loop. Copy them from the file at the end of this section.

Three controllers, because a swerve drivetrain moves in three directions at once. Heading gets its own, so the robot turns to face the right way while still driving.

The request is blue-relative because the pose is. The joystick request uses the operator perspective, which flips on red so forward matches what the driver sees. Your controllers already work in field coordinates, and re-rotating their output would drive the robot the wrong way on one alliance. OpenLoopVoltage means no wheel PID underneath yours.

1  /**
2   * @param drivetrain the swerve drive to command
3   * @param targetPose the field pose (blue-origin) to drive to, including the goal heading
4   */
5  public DriveToPoint(DriveMechanism drivetrain, Pose2d targetPose) {
6    super("DriveToPoint", drivetrain); // command name + required mechanism
7    this.drivetrain = drivetrain;
8    this.targetPose = targetPose;
9 
10    // Wrap heading error to [-pi, pi] so the robot turns the short way around.
11    headingController.enableContinuousInput(-Math.PI, Math.PI);
12  }

Then the loop. initialize() calls reset() on all three controllers, because a PIDController is a field here and remembers its error between runs.

1  @Override
2  protected void execute() {
3    Pose2d currentPose = drivetrain.getPose();
4 
5    double vx = xController.calculate(currentPose.getX(), targetPose.getX());
6    double vy = yController.calculate(currentPose.getY(), targetPose.getY());
7    double omega =
8        headingController.calculate(
9            currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
10 
11    drivetrain.setControl(driveRequest.withVelocity(new ChassisVelocities(vx, vy, omega)));
12  }

calculate(measurement, setpoint) takes where you are, then where you want to be. With kI and kD at zero, out comes kP times the error. ChassisVelocities holds the three results: vx and vy in meters per second, omega in radians per second. It was called ChassisSpeeds until recently, so an example using that name targets an older WPILib.

isFinished() returns false, so the command runs until the driver lets go. That keeps it out of Command.sequence(...). A sequence handed a command that never ends sticks on that leg forever, so no autonomous routine can use it.

The branch leaves the other option in a comment on that line: xController.atSetpoint() && yController.atSetpoint() && headingController.atSetpoint(). atSetpoint() asks whether the latest error is inside a tolerance. The default is 0.05, in whatever unit that controller works in: 5 cm on X and Y, 0.05 radians on heading, about 2.9°. setTolerance(...) changes it.

end(boolean interrupted) sends new SwerveRequest.Idle(). It runs whether the command finished or something took the drivetrain away, and the flag tells you which. No exit skips it, so that is where the stop belongs.

Canceling does not stop a motor: it ends the command, and the hardware carries on doing what it was last told. Teleop would forgive the omission, since the joystick default takes the drivetrain back and asks for nothing.

That default belongs to one OpMode. Schedule the command anywhere without it and nothing claims the drivetrain, so Mechanism.idle() takes over at the lowest priority. It sends no output at all, and Phoenix keeps applying the last velocity.

Two bindings go in the TeleopOpMode constructor, under the left-bumper binding already there. whileTrue, because the command never finishes on its own: release and it is canceled.

1import frc.robot.commands.DriveToPoint;
2import org.wpilib.math.geometry.Pose2d;
3import org.wpilib.math.geometry.Rotation2d;
4 
5// ... inside the constructor, after the seedFieldCentric binding:
6 
7    // Hold A or B to drive straight to a fixed spot on the field. Let go to stop.
8    driver.a().whileTrue(new DriveToPoint(drivetrain, Pose2d.kZero));
9    driver
10        .b()
11        .whileTrue(new DriveToPoint(drivetrain, new Pose2d(3, 2, Rotation2d.fromDegrees(180))));

These three controllers are not the Slot 0 gains you tuned in Tuner X. Those run on the TalonFX itself, in rotations of one mechanism, and put out volts for one motor. These run in your code once a loop, in meters and radians, and command a velocity for the whole chassis.

So kP = 10 reads as ten meters per second of commanded speed for every meter of error. On heading, kP = 7 reads as seven radians per second for every radian. Do the arithmetic. Three meters out, execute() asks for 30 m/s, against the 4.54 m/s in TunerConstants.kSpeedAt12Volts. Nothing in the file trims it.

The robot lurches away at full power, then crawls in over the last half meter as the error shrinks. It works, and it looks bad.

1. Enable Teleop in the simulator, drive a few meters from where the robot started, and turn it. Now hold A: it should drive back and rotate to 0° together, not spin first and drive second.
2. Keep holding A and push the left stick. Nothing should happen: DriveToPoint requires the drivetrain, so it outranks the joystick default until you release.
3. Release A halfway through the trip with the sticks centered. The robot should stop, not coast on at its last speed.
4. Hold B. The robot drives to (3, 2), turns to 180°, then sits on the target making small corrections for as long as you hold.
5. Graph Drivetrain/Pose and Drivetrain/TranslationSpeedMps for that run. X settles near 3, Y near 2, heading near 180°. Speed jumps to whatever the drivetrain can do, holds, then falls off steeply.

It drives off confidently in the wrong direction. Suspect the pose, not the gains. The robot is driving correctly toward where it believes the target is, so graph Drivetrain/Pose before touching a number. If the pose is right and it still slides sideways, check the request has ForwardPerspectiveValue.BlueAlliance.

It gets close, creeps in, and never arrives. Expected on this branch. One centimeter of error asks for 10 cm/s, and at some point the request cannot overcome friction. There is no I term to grind out the last bit.

It will not compile. Usually one of three. A ChassisSpeeds where ChassisVelocities belongs, a .named(...) call on the new command, or a super(...) that is not the first line of the constructor.

Profiled Drive to Point changes this one file and both faults go away. It plans the whole trip before the robot moves, so PID becomes a small correction and the command gets a finish line.