Skip to content

Commit

Permalink
fix: check flipping in intialize
Browse files Browse the repository at this point in the history
  • Loading branch information
mimizh2418 committed Feb 9, 2024
1 parent 61830c5 commit 7cf8dcb
Showing 1 changed file with 5 additions and 4 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@ public class SwerveDriveCommand extends Command {
private final SlewRateLimiter yLimiter = new SlewRateLimiter(2);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);

private boolean isFlipped;

public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller) {
this.drivetrain = drivetrain;
this.controller = controller;
Expand All @@ -31,6 +33,9 @@ public void initialize() {
xLimiter.reset(0);
yLimiter.reset(0);
rotLimiter.reset(0);
isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
}

@Override
Expand All @@ -49,10 +54,6 @@ public void execute() {
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();

boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red;

// Convert to field relative speeds & send command
drivetrain.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
Expand Down

0 comments on commit 7cf8dcb

Please sign in to comment.