PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mode auto reset pose

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-02-17 19:15:53 -05:00
parent 33badb2f77
commit 32989c510a
1 changed files with 16 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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));
}
}