diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e4db502..266e07e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,11 +13,15 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.util.FlippingUtil; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -92,10 +96,19 @@ public class RobotContainer { drivetrain.registerTelemetry(logger::telemeterize); } - + public Command getAutonomousCommand() { - return new SequentialCommandGroup(autoChooser.getSelected(), - new RainBow(bougie)); + return new SequentialCommandGroup(Commands.runOnce(()->{ + boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + if(flip){ + drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose())); + } + else{ + drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()); + } + }),autoChooser.getSelected(), new RainBow(bougie)); + + } }