From c43db2725fef14684f5b5ed150b8f14e9641032a Mon Sep 17 00:00:00 2001
From: Ernie3 <ernestgwilsoniii@gmail.com>
Date: Sat, 30 Mar 2024 03:04:49 -0400
Subject: [PATCH] fix stop all cmd at the end of auto since I accidentally
 broke it on Friday while cleaning up, we have tested it before and I also
 personally tested it heavily using the simulator and it works fine (no
 runtime exceptions)

---
 src/main/java/frc/robot/Robot.java               | 16 +++++++---------
 .../commands/drive/HoldAngleWhileDriving.java    |  2 +-
 2 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index bd7c489..14b5050 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -33,7 +33,7 @@ public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
     DriverStation.silenceJoystickConnectionWarning(true);
-    
+
     // for debugging
     CommandScheduler.getInstance().onCommandInitialize((command) -> {
       var cmdName = command.getName();
@@ -87,7 +87,7 @@ public void disabledInit() {
   public void disabledPeriodic() {
     RobotContainer.intake.setSetpoint(RobotContainer.intake.getPosition());
     RobotContainer.arm.setSetpoint(RobotContainer.arm.getPosition());
-    
+
     SwerveDrive.kMaxAngularSpeedRadiansPerSecond = 3 * Math.PI;
     SwerveDrive.kMaxSpeedMetersPerSecond = 4.6;
   }
@@ -98,9 +98,9 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
-    autonomousRoutine = robotContainer.getAutonomousCommand();
+    var selectedAutoRoutine = robotContainer.getAutonomousCommand();
 
-    if (autonomousRoutine != null) {
+    if (selectedAutoRoutine != null) {
       // for the end of the auto routine
       var stopAllCmd = new InstantCommand(() -> {
         RobotContainer.swerveDrive.stop();
@@ -109,11 +109,9 @@ public void autonomousInit() {
         RobotContainer.intake.stop();
       }, RobotContainer.swerveDrive, RobotContainer.shooter, RobotContainer.intake);
 
-      
-      // get the auto routine as a proxy command so we are free to compose a sequential command group 
-      // using it, run the auto routine then stop all motors
-      new ProxyCommand(() ->autonomousRoutine).andThen(stopAllCmd);
-
+      // get the auto routine as a proxy command so we are free to compose a
+      // sequential command group using it, run the auto routine then stop all motors
+      autonomousRoutine = new ProxyCommand(() -> selectedAutoRoutine).andThen(stopAllCmd);
       autonomousRoutine.schedule();
     }
   }
diff --git a/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java b/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java
index bd119d3..7d9fa22 100644
--- a/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java
+++ b/src/main/java/frc/robot/commands/drive/HoldAngleWhileDriving.java
@@ -40,7 +40,7 @@ public void execute() {
     }
 
     var fieldRelative = RobotContainer.driverController.getHID().getRightTriggerAxis() > 0.5;
-    var sign = fieldRelative || RobotContainer.arm.isAtState(ArmState.kAmp) ? 1 : -1;
+    var sign = fieldRelative || RobotContainer.arm.rotationSetpoint == ArmState.kAmp.rotations ? 1 : -1;
     RobotContainer.swerveDrive.holdAngleWhileDriving(-xSpeed*sign, -ySpeed*sign, angleSetpoint, fieldRelative);
   }