Skip to content

Field oriented driving #11

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
DylanTaylor29 merged 5 commits into from
Jan 13, 2026
Merged

Field oriented driving #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
9b19f39
split odometry & vision pose estimates
nlaverdure Jan 13, 2026
8e10b98
Changed methods to be either getOdometryPose() or getVisionPose()
DylanTaylor29 Jan 13, 2026
3f0a829
changed autonomous selector ports
DylanTaylor29 Jan 13, 2026
7538257
changed command for resetting robot heading
DylanTaylor29 Jan 13, 2026
116b85d
reset orientation based on alliance
nlaverdure Jan 13, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,10 +65,10 @@ public static final class KrakenX60Constants {
}

public static final class AutoConstants {
public static final int kAllianceColorSelectorPort = 10;
public static final int kAllianceColorSelectorPort = 3;

// max length is 8
public static final int[] kAutonomousModeSelectorPorts = {11, 12, 13, 18, 19};
public static final int[] kAutonomousModeSelectorPorts = {0, 1, 2};
}

public static final class OIConstants {
Expand Down
27 changes: 15 additions & 12 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import static edu.wpi.first.units.Units.Meters;
import static frc.robot.subsystems.vision.VisionConstants.*;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand Down Expand Up @@ -97,7 +96,7 @@ public Robot() {
vision =
new Vision(
drive::addVisionMeasurement,
drive::getPose,
drive::getVisionPose,
new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera),
new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera),
new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera),
Expand All @@ -119,15 +118,15 @@ public Robot() {
vision =
new Vision(
drive::addVisionMeasurement,
drive::getPose,
drive::getOdometryPose,
new VisionIOPhotonVisionSim(
cameraFrontRightName, robotToFrontRightCamera, drive::getPose),
cameraFrontRightName, robotToFrontRightCamera, drive::getOdometryPose),
new VisionIOPhotonVisionSim(
cameraFrontLeftName, robotToFrontLeftCamera, drive::getPose),
cameraFrontLeftName, robotToFrontLeftCamera, drive::getOdometryPose),
new VisionIOPhotonVisionSim(
cameraBackRightName, robotToBackRightCamera, drive::getPose),
cameraBackRightName, robotToBackRightCamera, drive::getOdometryPose),
new VisionIOPhotonVisionSim(
cameraBackLeftName, robotToBackLeftCamera, drive::getPose));
cameraBackLeftName, robotToBackLeftCamera, drive::getOdometryPose));
break;

case REPLAY: // Replaying a log
Expand All @@ -149,7 +148,7 @@ public Robot() {
vision =
new Vision(
drive::addVisionMeasurement,
drive::getPose,
drive::getOdometryPose,
new VisionIO() {},
new VisionIO() {},
new VisionIO() {},
Expand Down Expand Up @@ -286,8 +285,10 @@ public void bindZorroDriver(int port) {
.onTrue(
Commands.runOnce(
() ->
drive.setPose(
new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)),
drive.setOdometryRotation(
allianceSelector.fieldRotated()
? Rotation2d.k180deg
: Rotation2d.kZero),
drive)
.ignoringDisable(true));

Expand Down Expand Up @@ -318,8 +319,10 @@ public void bindXboxDriver(int port) {
.onTrue(
Commands.runOnce(
() ->
drive.setPose(
new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)),
drive.setOdometryRotation(
allianceSelector.fieldRotated()
? Rotation2d.k180deg
: Rotation2d.kZero),
drive)
.ignoringDisable(true));

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/auto/AutoMode.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@ public abstract class AutoMode {
public AutoMode(Drive drivetrain) {
autoFactory =
new AutoFactory(
drivetrain::getPose,
drivetrain::setPose,
drivetrain::getOdometryPose,
drivetrain::setOdometryPose,
drivetrain::followTrajectory,
false,
drivetrain);
Expand Down
25 changes: 13 additions & 12 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,8 +99,8 @@ public static Command joystickDrive(
ChassisSpeeds.fromFieldRelativeSpeeds(
speeds,
fieldRotatedSupplier.getAsBoolean()
? drive.getRotation().plus(Rotation2d.kPi)
: drive.getRotation());
? drive.getOdometryRotation().plus(Rotation2d.kPi)
: drive.getOdometryRotation());
}

drive.runVelocity(speeds);
Expand Down Expand Up @@ -140,8 +140,8 @@ public static Command fieldRelativeJoystickDrive(
ChassisSpeeds.fromFieldRelativeSpeeds(
speeds,
fieldRotatedSupplier.getAsBoolean()
? drive.getRotation().plus(Rotation2d.kPi)
: drive.getRotation()));
? drive.getOdometryRotation().plus(Rotation2d.kPi)
: drive.getOdometryRotation()));
},
drive)
.withName("Field Relative Joystick Drive");
Expand Down Expand Up @@ -209,7 +209,8 @@ public static Command joystickDriveAtFixedOrientation(
// Calculate angular speed
double omega =
angleController.calculate(
drive.getRotation().getRadians(), rotationSupplier.get().getRadians());
drive.getOdometryRotation().getRadians(),
rotationSupplier.get().getRadians());

// Convert to field relative speeds & send command
ChassisSpeeds speeds =
Expand All @@ -221,14 +222,14 @@ public static Command joystickDriveAtFixedOrientation(
ChassisSpeeds.fromFieldRelativeSpeeds(
speeds,
fieldRotatedSupplier.getAsBoolean()
? drive.getRotation().plus(Rotation2d.kPi)
: drive.getRotation()));
? drive.getOdometryRotation().plus(Rotation2d.kPi)
: drive.getOdometryRotation()));
},
drive)
.withName("Fixed Orientation Joystick Drive")

// Reset PID controller when command starts
.beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
.beforeStarting(() -> angleController.reset(drive.getOdometryRotation().getRadians()));
}

/**
Expand Down Expand Up @@ -261,8 +262,8 @@ public static Command pointAtTarget(
ChassisSpeeds.fromFieldRelativeSpeeds(
speeds,
fieldRotatedSupplier.getAsBoolean()
? drive.getRotation().plus(Rotation2d.kPi)
: drive.getRotation()));
? drive.getOdometryRotation().plus(Rotation2d.kPi)
: drive.getOdometryRotation()));
},
drive)
.withName("Point At Target")
Expand Down Expand Up @@ -365,14 +366,14 @@ public static Command wheelRadiusCharacterization(Drive drive) {
Commands.runOnce(
() -> {
state.positions = drive.getWheelRadiusCharacterizationPositions();
state.lastAngle = drive.getRotation();
state.lastAngle = drive.getOdometryRotation();
state.gyroDelta = 0.0;
}),

// Update gyro delta
Commands.run(
() -> {
var rotation = drive.getRotation();
var rotation = drive.getOdometryRotation();
state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians());
state.lastAngle = rotation;
})
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/commands/PathCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ public static Command goToTargetPose(Drive drive, Pose2d targetPose) {
@Override
public Command get() {
List<Waypoint> waypoints =
PathPlannerPath.waypointsFromPoses(drive.getPose(), targetPose);
PathPlannerPath.waypointsFromPoses(drive.getVisionPose(), targetPose);

return AutoBuilder.followPath(createPath(waypoints, targetPose.getRotation()));
}
Expand All @@ -62,7 +62,7 @@ public static Command dockToTargetPoint(
new Supplier<Command>() {
@Override
public Command get() {
var initialPose = drive.getPose();
var initialPose = drive.getVisionPose();
var heading = targetPoint.minus(initialPose.getTranslation()).getAngle();
var finalPose = new Pose2d(targetPoint, heading);
List<Waypoint> waypoints =
Expand Down Expand Up @@ -95,7 +95,7 @@ public static Command dockToTargetPose(Drive drive, Pose2d targetPose, Distance
public Command get() {
List<Waypoint> waypoints =
PathPlannerPath.waypointsFromPoses(
drive.getPose(),
drive.getVisionPose(),
targetPose.plus(
new Transform2d(leadDistance.unaryMinus(), Meters.of(0), Rotation2d.kZero)),
targetPose);
Expand All @@ -118,7 +118,7 @@ public static Command advanceForward(Drive drive, Distance leadDistance) {
new Supplier<Command>() {
@Override
public Command get() {
var initialPose = drive.getPose();
var initialPose = drive.getVisionPose();
var waypoints =
PathPlannerPath.waypointsFromPoses(
initialPose,
Expand Down
44 changes: 29 additions & 15 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,9 @@ public class Drive extends SubsystemBase {
new SwerveModulePosition(),
new SwerveModulePosition()
};
private SwerveDrivePoseEstimator poseEstimator =
private SwerveDrivePoseEstimator odometryPose =
new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
private SwerveDrivePoseEstimator visionPose =
new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
private Boolean firstVisionEstimate = true;

Expand Down Expand Up @@ -104,8 +106,8 @@ public Drive(

// Configure AutoBuilder for PathPlanner
AutoBuilder.configure(
this::getPose,
this::setPose,
this::getVisionPose,
this::setOdometryPose,
this::getChassisSpeeds,
this::runVelocity,
new PPHolonomicDriveController(
Expand Down Expand Up @@ -190,7 +192,8 @@ public void periodic() {
}

// Apply update
poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
odometryPose.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
visionPose.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
}

// Update gyro alert
Expand Down Expand Up @@ -223,7 +226,7 @@ public void runVelocity(ChassisSpeeds speeds) {

public void followTrajectory(SwerveSample sample) {
// Get the current pose of the robot
Pose2d pose = getPose();
Pose2d pose = getVisionPose();

// Generate the next speeds for the robot
ChassisSpeeds speeds =
Expand All @@ -234,7 +237,7 @@ public void followTrajectory(SwerveSample sample) {
+ headingController.calculate(pose.getRotation().getRadians(), sample.heading));

// Apply the generated speeds
runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getRotation()));
runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getOdometryRotation()));
}

/** Runs the drive in a straight line with the specified drive output. */
Expand Down Expand Up @@ -318,19 +321,30 @@ public double getFFCharacterizationVelocity() {
}

/** Returns the current odometry pose. */
@AutoLogOutput(key = "Odometry/Robot")
public Pose2d getPose() {
return poseEstimator.getEstimatedPosition();
@AutoLogOutput(key = "Localizer/Odometry")
public Pose2d getOdometryPose() {
return odometryPose.getEstimatedPosition();
}

/** Returns the current vision pose. */
@AutoLogOutput(key = "Localizer/Vision")
public Pose2d getVisionPose() {
return visionPose.getEstimatedPosition();
}

/** Returns the current odometry rotation. */
public Rotation2d getRotation() {
return getPose().getRotation();
public Rotation2d getOdometryRotation() {
return getOdometryPose().getRotation();
}

/** Sets the current odometry orientation. */
public void setOdometryRotation(Rotation2d rotation) {
setOdometryPose(new Pose2d(getOdometryPose().getTranslation(), rotation));
}

/** Resets the current odometry pose. */
public void setPose(Pose2d pose) {
poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
public void setOdometryPose(Pose2d pose) {
odometryPose.resetPosition(rawGyroRotation, getModulePositions(), pose);
}

/** Adds a new timestamped vision measurement. */
Expand All @@ -340,11 +354,11 @@ public void addVisionMeasurement(
Matrix<N3, N1> visionMeasurementStdDevs) {
// Teleport the odometry to the first vision estimate
if (firstVisionEstimate && RobotState.isDisabled()) {
setPose(visionRobotPoseMeters);
setOdometryPose(visionRobotPoseMeters);
firstVisionEstimate = false;
}

poseEstimator.addVisionMeasurement(
visionPose.addVisionMeasurement(
visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
}

Expand Down