Milkybeans - More human-friendly rewrite of Climber.java

src/main/java/org/team199/robot2022/RobotContainer.java

@@ -11,8 +11,6 @@
 import org.team199.robot2022.commands.PassiveAutomaticIntake;
 import org.team199.robot2022.commands.PassiveManualIntake;
 import org.team199.robot2022.commands.Regurgitate;
-import org.team199.robot2022.commands.ResetAndExtendClimber;
-import org.team199.robot2022.commands.ResetAndRetractClimber;
 import org.team199.robot2022.commands.RetractClimber;
 import org.team199.robot2022.commands.Shoot;
 import org.team199.robot2022.commands.TeleopDrive;
@@ -149,21 +147,21 @@ public RobotContainer(Robot robot) {
   private void configureButtonBindingsLeftJoy() {
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualAddPort).whenPressed(new InstantCommand(intakeFeeder::manualAdd));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualSubtractPort).whenPressed(new InstantCommand(intakeFeeder::manualSub));
-    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetAndExtendClimberPort).whenPressed(new ResetAndExtendClimber(climber));
-    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetAndRetractClimberPort).whenPressed(new ResetAndRetractClimber(climber));
-    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetClimberEncoders). whenPressed(new InstantCommand(climber::resetEncodersToZero));
+
+    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetClimberEncoders). whenPressed(new InstantCommand(()->climber.resetEncodersTo(Climber.EncoderPos.zero,Climber.bothMotors)));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.toggleDriveMode).whenPressed(new InstantCommand( () -> {SmartDashboard.putBoolean("Field Oriented", SmartDashboard.getBoolean("Field Oriented", true) ? false : true);}));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.toggleLongShot).whenPressed(new InstantCommand(shooter::toggleLongShot));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetFieldOriented).whenPressed(new SequentialCommandGroup(new InstantCommand(() -> {SmartDashboard.putBoolean("Field Oriented", true);}), new WaitCommand(0.05), new InstantCommand(() -> {SmartDashboard.putNumber("Field Offset from North (degrees)", SmartDashboard.getNumber("Field Offset from North (degrees)", 0) - dt.getHeadingDeg() + 180);})));
   }
 
   private void configureButtonBindingsRightJoy() {
     new JoystickButton(rightJoy, Constants.OI.RightJoy.shootPort).whenPressed(new Shoot(intakeFeeder, shooter));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(climber::slowExtendLeft)).whenReleased(new InstantCommand(climber::stopLeft));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(climber::slowRetractLeft)).whenReleased(new InstantCommand(climber::stopLeft));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(climber::slowExtendRight)).whenReleased(new InstantCommand(climber::stopRight));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(climber::slowRetractRight)).whenReleased(new InstantCommand(climber::stopRight));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,Climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.leftMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.leftMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor)));
     new JoystickButton(rightJoy, Constants.OI.RightJoy.toggleShooterModePort).whenPressed(new InstantCommand(shooter::toggleDutyCycleMode));
+
     new JoystickButton(rightJoy, Constants.OI.RightJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override));
   }
 
@@ -237,7 +235,7 @@ private double getStickValue(Constants.OI.StickType stick, Constants.OI.StickDir
 
   /**
    * Processes an input from the joystick into a value between -1 and 1
-   * 
+   *
    * @param value The value to be processed.
    * @return The processed value.
    */

src/main/java/org/team199/robot2022/commands/ExtendClimber.java

@@ -15,18 +15,17 @@ public ExtendClimber(Climber climber) {
         super(
             new FunctionalCommand(
                 () -> {},
-                climber::extendLeft,
-                climber::stopLeft,
-                climber::isLeftExtended
+                () -> climber.moveMotors(Climber.MotorSpeed.extend,Climber.rightMotor),
+                (interrupted) -> climber.stopMotors(Climber.rightMotor),
+                () -> climber.isMotorExtended(Climber.rightMotor)
             ),
             new FunctionalCommand(
                 () -> {},
-                climber::extendRight,
-                climber::stopRight,
-                climber::isRightExtended
+                () -> climber.moveMotors(Climber.MotorSpeed.extend,Climber.leftMotor),
+                (interrupted) -> climber.stopMotors(Climber.leftMotor),
+                () -> climber.isMotorExtended(Climber.leftMotor)
             )
         );
         addRequirements(climber);
     }
-
 }

src/main/java/org/team199/robot2022/commands/RetractClimber.java

@@ -14,19 +14,18 @@ public class RetractClimber extends ParallelCommandGroup {
     public RetractClimber(Climber climber) {
         super(
             new FunctionalCommand(
-                () -> {},
-                climber::retractLeft,
-                climber::stopLeft,
-                climber::isLeftRetracted
+              () -> {},
+              () -> climber.moveMotors(Climber.MotorSpeed.retract,Climber.rightMotor),
+              (interrupted) -> climber.stopMotors(Climber.rightMotor),
+              () -> climber.isMotorRetracted(Climber.rightMotor)
             ),
             new FunctionalCommand(
-                () -> {},
-                climber::retractRight,
-                climber::stopRight,
-                climber::isRightRetracted
+              () -> {},
+              () -> climber.moveMotors(Climber.MotorSpeed.retract,Climber.leftMotor),
+              (interrupted) -> climber.stopMotors(Climber.leftMotor),
+              () -> climber.isMotorRetracted(Climber.leftMotor)
             )
         );
         addRequirements(climber);
     }
-
 }