Merge branch 'bd-elevator-disable'

src/main/java/team/gif/robot/OI.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -19,7 +20,6 @@
 import team.gif.robot.commands.drivetrain.Reset0;
 import team.gif.robot.commands.toggleManualControl.ToggleManualControl;
 
-
 public class OI {
     /*
      * Instantiate all joysticks/controllers and their buttons here
@@ -134,11 +134,13 @@ public OI() {
         aA.whileTrue(new ClimberDeploy());
         aStart.and(aBack).toggleOnTrue(new ToggleManualControl());
         aRBump.onTrue(new PistonToggleState());
-        aLBump.onTrue(new SetElevatorPosition(Constants.Elevator.LEVEL_4_POSITION));
-        aDPadUp.and(aStart.negate()).onTrue(new SetElevatorPosition(Constants.Elevator.LEVEL_3_POSITION));
-        aDPadLeft.and(aStart.negate()).onTrue(new SetElevatorPosition(Constants.Elevator.LEVEL_2_POSITION));
-        aDPadDown.and(aStart.negate()).onTrue(new SetElevatorPosition(Constants.Elevator.COLLECTOR_POSITION));
-
+        // Only run the SetElevatorPosition if robot is in StandardOp mode, false condition (i.e. manual mode) still returns
+        // to elevator defalt command which is PID mode, so need to restart ElevatorManualMode
+        aLBump.onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.LEVEL_4_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+        aDPadUp.and(aStart.negate()).onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.LEVEL_3_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+        aDPadLeft.and(aStart.negate()).onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.LEVEL_2_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+        aDPadDown.and(aStart.negate()).onTrue(new ConditionalCommand(new SetElevatorPosition(Constants.Elevator.COLLECTOR_POSITION), new InstantCommand(Robot::enableRobotModeManual), Robot::isRobotInStandardOpMode));
+
         shooterSensor.debounce(Constants.DEBOUNCE_DEFAULT).onTrue(new Rumble().andThen(new WaitCommand(0.1).andThen(new Rumble())));
 
         //test sys id for elevator, delete later

src/main/java/team/gif/robot/Robot.java

@@ -225,6 +225,10 @@ public static RobotMode getRobotMode() {
         return robotMode;
     }
 
+    public static boolean isRobotInStandardOpMode() {
+        return robotMode == RobotMode.STANDARD_OP;
+    }
+
     static public void enableRobotModeManual() {
         robotMode = RobotMode.MANUAL;
 
@@ -234,5 +238,7 @@ static public void enableRobotModeManual() {
 
     static public void enableRobotModeStandardOp() {
         robotMode = RobotMode.STANDARD_OP;
+
+        elevator.enableElevator();
     }
 }

src/main/java/team/gif/robot/commands/climber/ClimberClimb.java

@@ -13,7 +13,13 @@ public ClimberClimb() {
 
     // Called when the command is initially scheduled.
     @Override
-    public void initialize() {}
+    public void initialize() {
+        // Disable the elevator when climbing preventing the aux from accidentally raising the elevator
+        // Do not re-enable at the end of the command. Want to keep elevator disabled until
+        // aux toggles manaul mode. Although Deploy called this, it is possible aux toggled manual
+        // control after deploying so make sure the elevator continues to be disabled
+        Robot.elevator.disableElevator();
+    }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
     @Override

src/main/java/team/gif/robot/commands/climber/ClimberDeploy.java

@@ -14,7 +14,12 @@ public ClimberDeploy() {
 
     // Called when the command is initially scheduled.
     @Override
-    public void initialize() {}
+    public void initialize() {
+        // Disable the elevator when climbing preventing the aux from accidentally raising the elevator
+        // Do not re-enable at the end of the command. Want to keep elevator disabled until
+        // aux toggles manaul mode.
+        Robot.elevator.disableElevator();
+    }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
     @Override

src/main/java/team/gif/robot/subsystems/Elevator.java

@@ -1,6 +1,7 @@
 package team.gif.robot.subsystems;
 
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
@@ -184,6 +185,34 @@ public void configMotionMagicDown() {
         updateMotionMagicParms(Constants.Elevator.REV_MAX_VELOCITY, Constants.Elevator.REV_MAX_ACCELERATION);
     }
 
+    /**
+     * Disable the elevator (by setting all the motion magic pid values to 0)
+     */
+    public void disableElevator() {
+        // Slot 0 (Motion Magic) PID values
+        Slot0Configs config = new Slot0Configs();
+
+        config.kP = 0;
+        config.kI = 0;
+        config.kD = 0;
+        config.kS = 0;
+        elevatorMotor.getConfigurator().apply(config);
+    }
+
+    /**
+     * Enables the elevator after the disableElevator function was called
+     */
+    public void enableElevator() {
+        // Slot 0 (Motion Magic) PID values
+        Slot0Configs config = new Slot0Configs();
+
+        config.kP = Constants.Elevator.ELEVATOR_KP;
+        config.kI = Constants.Elevator.ELEVATOR_KI;
+        config.kD = Constants.Elevator.ELEVATOR_KD;
+        config.kS = Constants.Elevator.ELEVATOR_KS;
+        elevatorMotor.getConfigurator().apply(config);
+    }
+
     /**
      * sets the class local elevator target position
      * Used for comparing current position to target position to