Add Manual Mode

src/main/java/frc/robot/OperatorStates.java

@@ -0,0 +1,27 @@
+package frc.robot;
+
+/**
+ * changes operator states from normal to manual
+ */
+public class OperatorStates {
+
+    private OperatorStates() {}
+
+    public static boolean manualMode = false;
+
+    public static boolean manualModeEnabled() {
+        return manualMode;
+    }
+
+    public static void toggleManualMode() {
+        manualMode = !manualMode;
+    }
+
+    public static void enableManualMode() {
+        manualMode = true;
+    }
+
+    public static void diableManualMode() {
+        manualMode = false;
+    }
+}

src/main/java/frc/robot/RobotContainer.java

@@ -9,6 +9,8 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -61,8 +63,6 @@ public class RobotContainer {
 
     /* Controllers */
     public final CommandXboxController driver = new CommandXboxController(Constants.driverId);
-    public final CommandXboxController backUpOperator =
-        new CommandXboxController(Constants.operatorId);
     public final CommandXboxController pitController =
         new CommandXboxController(Constants.PIT_CONTROLLER_ID);
     public final CommandXboxController altOperator =
@@ -112,11 +112,13 @@ public class RobotContainer {
     private final Swerve swerve;
     private final Vision vision;
     private CoralScoring coralScoring;
+    private Climber climb;
 
     /* Triggers */
     private Trigger algaeInIntake = new Trigger(() -> algae.hasAlgae());
+    private Trigger manualMode = new Trigger(() -> OperatorStates.manualModeEnabled());
+
 
-    private Climber climb;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -160,6 +162,7 @@ public RobotContainer(RobotRunType runtimeType) {
         /* Button and Trigger Bindings */
 
         configureTriggerBindings();
+
         if (runtimeType == RobotRunType.kSimulation) {
             maybeController("Driver", driver, this::setupDriver);
         } else {
@@ -171,6 +174,7 @@ public RobotContainer(RobotRunType runtimeType) {
 
     private List<Runnable> controllerSetups = new ArrayList<>();
 
+
     private void maybeController(String name, CommandXboxController xboxController,
         Runnable setupFun) {
         Runnable runner = () -> {
@@ -193,11 +197,20 @@ public void queryControllers() {
         controllerSetups.clear();
     }
 
+    /**
+     * Use this method to vol your button->command mappings. Buttons can be created by instantiating
+     * a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or
+     * {@link XboxController}), and then passing it to a
+     * {@link edu1.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+
     private void setupDriver() {
         swerve.setDefaultCommand(swerve.teleOpDrive(driver, Constants.Swerve.isFieldRelative,
             Constants.Swerve.isOpenLoop));
         driver.y().onTrue(new InstantCommand(() -> swerve.resetFieldRelativeOffset()));
+
         driver.x().onTrue(new InstantCommand(() -> { // sim only
+
             swerve.resetOdometry(new Pose2d(7.24, 4.05, Rotation2d.kZero));
         }));
         driver.rightTrigger().whileTrue(climb.runClimberMotorCommand());
@@ -206,30 +219,42 @@ private void setupDriver() {
     }
 
     private void setupAltOperatorController() {
-        altOperator.povUp().and(HeightMode.coralMode)
+
+        altOperator.povUp().and(manualMode.negate()).and(HeightMode.coralMode)
             .onTrue(Commands.runOnce(() -> CoralHeight.incrementState()).ignoringDisable(true));
-        altOperator.povUp().and(HeightMode.algaeMode)
+        altOperator.povUp().and(manualMode.negate()).and(HeightMode.algaeMode)
             .onTrue(Commands.runOnce(() -> AlgaeHeight.incrementState()).ignoringDisable(true));
-        altOperator.povDown().and(HeightMode.algaeMode)
+        altOperator.povDown().and(manualMode.negate()).and(HeightMode.algaeMode)
             .onTrue(Commands.runOnce(() -> AlgaeHeight.decrementState()).ignoringDisable(true));
-        altOperator.povDown().and(HeightMode.coralMode)
+        altOperator.povDown().and(manualMode.negate()).and(HeightMode.coralMode)
             .onTrue(Commands.runOnce(() -> CoralHeight.decrementState()).ignoringDisable(true));
-        altOperator.povRight()
+        altOperator.povRight().and(manualMode.negate())
             .onTrue(Commands.runOnce(() -> HeightMode.decrementState()).ignoringDisable(true));
-        altOperator.povLeft()
+        altOperator.povLeft().and(manualMode.negate())
             .onTrue(Commands.runOnce(() -> HeightMode.incrementState()).ignoringDisable(true));
         altOperator.y().onTrue(elevator.home());
         altOperator.x().whileTrue(coralScoring.runScoringMotor(2));
         altOperator.rightTrigger().whileTrue(algae.setMotorVoltageCommand(Constants.Algae.VOLTAGE));
         altOperator.leftTrigger()
             .whileTrue(algae.setMotorVoltageCommand(Constants.Algae.NEGATIVE_VOLTAGE));
-
-        altOperator.a().and(HeightMode.algaeMode).and(AlgaeHeight.level1).whileTrue(elevator.p0());
-        altOperator.a().and(HeightMode.algaeMode).and(AlgaeHeight.level2).whileTrue(elevator.p2());
-        altOperator.a().and(HeightMode.coralMode).and(CoralHeight.level1).whileTrue(elevator.p0());
-        altOperator.a().and(HeightMode.coralMode).and(CoralHeight.level2).whileTrue(elevator.p1());
-        altOperator.a().and(HeightMode.coralMode).and(CoralHeight.level3).whileTrue(elevator.p3());
-        altOperator.a().and(HeightMode.coralMode).and(CoralHeight.level4).whileTrue(elevator.p4());
+        // manual mode
+        altOperator.start().onTrue(Commands.runOnce(() -> {
+            OperatorStates.toggleManualMode();
+        }).ignoringDisable(true));
+        manualMode.onTrue(elevator.manualMove(altOperator));
+
+        altOperator.a().and(manualMode.negate()).and(HeightMode.algaeMode).and(AlgaeHeight.level1)
+            .whileTrue(elevator.p0());
+        altOperator.a().and(manualMode.negate()).and(HeightMode.algaeMode).and(AlgaeHeight.level2)
+            .whileTrue(elevator.p2());
+        altOperator.a().and(manualMode.negate()).and(HeightMode.coralMode).and(CoralHeight.level1)
+            .whileTrue(elevator.p0());
+        altOperator.a().and(manualMode.negate()).and(HeightMode.coralMode).and(CoralHeight.level2)
+            .whileTrue(elevator.p1());
+        altOperator.a().and(manualMode.negate()).and(HeightMode.coralMode).and(CoralHeight.level3)
+            .whileTrue(elevator.p3());
+        altOperator.a().and(manualMode.negate()).and(HeightMode.coralMode).and(CoralHeight.level4)
+            .whileTrue(elevator.p4());
         // altOperator.a().whileTrue(elevator.moveTo(() -> {
         // switch (HeightMode.getCurrentHeightMode()) {
         // case kAlgae:
@@ -263,6 +288,7 @@ private void setupAltOperatorController() {
         // return null;
         // }
         // }));
+
     }
 
     private void setupPitController() {

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.util.viz.Viz2025;
 import frc.robot.Constants;
 
@@ -93,6 +94,10 @@ public Command moveTo(Supplier<Distance> height) {
         }).until(() -> Math.abs(inputs.position.in(Inches) - height.get().in(Inches)) < 1);
     }
 
+    public Command manualMove(CommandXboxController leftStick) {
+        return run(() -> io.setPower(leftStick.getLeftY()));
+    }
+
     public Command moveUp() {
         return runEnd(() -> io.setVoltage(SmartDashboard.getNumber("elevatorVoltage", 1.0)),
             () -> io.setVoltage(0));
@@ -101,4 +106,5 @@ public Command moveUp() {
     public Command moveDown() {
         return runEnd(() -> io.setVoltage(-1.0), () -> io.setVoltage(0));
     }
+
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java

@@ -56,4 +56,6 @@ public void resetHome() {
 
     }
 
+    public default void setPower(double power) {}
+
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorReal.java

@@ -82,6 +82,10 @@ public void setVoltage(double volts) {
         rightElevatorMotor.setVoltage(volts);
     }
 
+    public void setPower(double power) {
+        rightElevatorMotor.set(power);;
+    }
+
     public void setPositon(double position) {
         rightElevatorMotor.setControl(positionVoltage.withPosition(position));
     }