Merge pull request #67 from grt192/driver-practice

Binded switchpad 1 to toLimitSwitch command
grt192 · Apr 2, 2024 · 59c6971 · 59c6971
2 parents d1c44bf + e702e06
commit 59c6971
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -115,7 +115,7 @@ public class RobotContainer {
     private final JoystickButton offsetDownButton = new JoystickButton(switchboard, 8);
 
     private final JoystickButton shuttleNotes = new JoystickButton(switchboard, 6);
-
+    private final JoystickButton elevatorToZero = new JoystickButton(switchboard, 1);
     private UsbCamera driverCamera;
     private MjpegServer driverCameraServer;
 
@@ -341,6 +341,7 @@ private void configureBindings() {
             }
         }, elevatorSubsystem));
 
+        elevatorToZero.onTrue(new ElevatorToZeroCommand(elevatorSubsystem));
         /* INTAKE TEST */
 
         // xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
@@ -374,8 +375,8 @@ private void configureBindings() {
                         () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow the pivot
                     // if elevator is down
                     new SequentialCommandGroup(
-                        new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).unless(intakeRollerSubsystem::getAmpSensor).andThen(// extend pivot
-                            new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
+                        new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).unless(() -> !intakeRollerSubsystem.getRockwellSensorValue()).andThen(// extend pivot
+                            new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75).unless(intakeRollerSubsystem::getAmpSensor) // run rollers to front sensor
                                     .until(() -> intakeRollerSubsystem.getFrontSensorReached()),
                         new IntakePivotSetPositionCommand(intakePivotSubsystem, 0.2),
                         new ElevatorToAmpCommand(elevatorSubsystem)

diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToLimitSwitchCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToLimitSwitchCommand.java
@@ -18,6 +18,7 @@ public void initialize() {
         try {
             if (!elevatorSubsystem.getLimitSwitch()){
                 elevatorSubsystem.setCoast();
+                elevatorSubsystem.setMotorPower(0);
             }
         }
         catch (Exception e) {

diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java
@@ -30,7 +30,7 @@ public void periodic() {
         // System.out.println("Left: " + GRTUtil.twoDecimals(leftClimbArm.getCurrentExtension())
         //               + ", Right: " + GRTUtil.twoDecimals(rightClimbArm.getCurrentExtension()));
 
-        //System.out.println(leftClimbArm.isLimitSwitchPressed() + " " + rightClimbArm.isLimitSwitchPressed());
+        // System.out.println(leftClimbArm.isLimitSwitchPressed() + " " + rightClimbArm.isLimitSwitchPressed());
 
         leftClimbArm.update();
         rightClimbArm.update();