Skip to content
This repository was archived by the owner on May 19, 2024. It is now read-only.

Commit 78c62a2

Browse files
committed
refactor: Comment out untested.
1 parent 4d47b9e commit 78c62a2

File tree

2 files changed

+14
-5
lines changed

2 files changed

+14
-5
lines changed

src/main/java/frc/robot/odometry/Odometry.java

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -149,11 +149,14 @@ public void setRotation(Rotation2d rotation) {
149149
public Command tare() {
150150
return Commands.runOnce(
151151
() -> {
152-
if (AllianceFlipHelper.shouldFlip()) {
153-
setRotation(Rotation2d.fromDegrees(180));
154-
} else {
155-
setRotation(Rotation2d.fromDegrees(0));
156-
}
152+
// TODO needs testing!
153+
// if (AllianceFlipHelper.shouldFlip()) {
154+
// setRotation(Rotation2d.fromDegrees(180));
155+
// } else {
156+
// setRotation(Rotation2d.fromDegrees(0));
157+
// }
158+
159+
gyroscope.setYaw(0.0);
157160
});
158161
}
159162

src/main/java/frc/robot/swerve/DriveCommand.java

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
1212
import frc.robot.RobotConstants;
1313
import frc.robot.odometry.Odometry;
14+
import frc.lib.AllianceFlipHelper;
1415

1516
/** Drives the swerve using driver input. */
1617
public class DriveCommand extends Command {
@@ -70,6 +71,11 @@ public void execute() {
7071

7172
Rotation2d poseHeading = odometry.getPosition().getRotation();
7273

74+
// TODO test
75+
// if (AllianceFlipHelper.shouldFlip()) {
76+
// poseHeading = poseHeading.plus(Rotation2d.fromDegrees(180));
77+
// }
78+
7379
if (DriveRequest.startedDrifting(previousRequest, request)) {
7480
headingGoal = wrap(new State(poseHeading.getRotations(), 0.0), poseHeading.getRotations());
7581
}

0 commit comments

Comments
 (0)