diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 87c9ed8e..227f3691 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -151,7 +151,10 @@ private void configureDrivebaseBindings() { // Stop running this method return; } - + // if the solo controller is connected, use it for driving + // gets set when robot code starts running for the first time so usally is false and isnt + // currently in use + boolean soloCC = soloController.isConnected(); // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. @@ -379,9 +382,6 @@ private void configureSuperStructureBindings() { .asProxy() : Commands.none()) .withName("Automatic Intake")); - } - - if (sensors.armSensor != null) { sensors .armSensor .inClaw() @@ -394,12 +394,7 @@ private void configureSuperStructureBindings() { case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; } }) - .withName("Set solo scoring mode")); - - sensors - .armSensor - .inClaw() - .and(RobotModeTriggers.teleop()) + .withName("Set solo scoring mode")) .onFalse( Commands.runOnce( () -> { @@ -718,7 +713,7 @@ private void configureSpinnyClawBindings() { } else { return switch (scoringMode) { case CORAL -> Commands.none().withName("No manual spit - Coral mode"); - case ALGAE -> s.spinnyClawSubsytem.algaeExtakePower(); + case ALGAE -> s.spinnyClawSubsytem.algaeExtakeProcessorPower(); }; } })); @@ -884,7 +879,7 @@ public void vibrateCoDriveController(double vibration) { } } - private double getJoystickInput(double input) { + private double getSoloJoystickInput(double input) { if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { return 0; // stop driving if either stick is pressed } @@ -898,21 +893,21 @@ private double getJoystickInput(double input) { private double getSoloDriveX() { // Joystick +Y is back // Robot +X is forward - return getJoystickInput(-soloController.getLeftY()) * MaxSpeed; + return getSoloJoystickInput(-soloController.getLeftY()) * MaxSpeed; } // takes the Y value from the joystick, and applies a deadband and input scaling private double getSoloDriveY() { // Joystick +X is right // Robot +Y is left - return getJoystickInput(-soloController.getLeftX()) * MaxSpeed; + return getSoloJoystickInput(-soloController.getLeftX()) * MaxSpeed; } // takes the rotation value from the joystick, and applies a deadband and input scaling private double getSoloDriveRotate() { // Joystick +X is right // Robot +angle is CCW (left) - return getJoystickInput(-soloController.getRightX()) * MaxSpeed; + return getSoloJoystickInput(-soloController.getRightX()) * MaxSpeed; } private void configureSoloControllerBindings() { @@ -920,43 +915,79 @@ private void configureSoloControllerBindings() { soloController .leftTrigger() .onTrue( - Commands.runOnce( - () -> { - Command scoreCommand; - switch (soloScoringMode) { - case CORAL_IN_CLAW -> { - scoreCommand = getSoloCoralBranchHeightCommand(); - } - case ALGAE_IN_CLAW -> { - Command bargeScoreCommand = - BargeAlign.bargeScore( + Commands.deferredProxy( + () -> { + Command scoreCommand; + switch (soloScoringMode) { + case CORAL_IN_CLAW -> { + scoreCommand = + getSoloCoralBranchHeightCommand() + .until( + () -> + soloController.a().getAsBoolean() + || soloController.b().getAsBoolean() + || soloController.x().getAsBoolean() + || soloController.y().getAsBoolean()); + } + + case ALGAE_IN_CLAW -> { + Command bargeScoreCommand = + BargeAlign.bargeScore( s.drivebaseSubsystem, superStructure, () -> getSoloDriveX(), () -> getSoloDriveY(), () -> getSoloDriveRotate(), - soloController.rightBumper()) - .withName("Algae score then intake"); - scoreCommand = - Commands.sequence( - bargeScoreCommand, - Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); - } - case NO_GAME_PIECE -> { - scoreCommand = - Commands.parallel( - Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) - .alongWith(scoringModeSelectRumble()) - .withName("Algae Scoring Mode"), - AutoAlgaeHeights.autoAlgaeIntakeCommand( - s.drivebaseSubsystem, superStructure, this) - .until(() -> sensors.armSensor.booleanInClaw())); - } - default -> scoreCommand = Commands.none(); - } - CommandScheduler.getInstance().schedule(scoreCommand); - })); + soloController.rightBumper()); + + if (sensors.intakeSensor.booleanInIntake()) { + Command holdGroundIntakeOut = + superStructure.holdGroundIntakeOut(soloController.povUp()); + + Command quickHandoff = + Commands.sequence( + superStructure + .quickGroundIntake(() -> true) + .withName("Quick Gound intake"), + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.CORAL_IN_CLAW)) + .withName("Quick Handoff After Algae Score"); + + scoreCommand = + holdGroundIntakeOut + .deadlineFor(bargeScoreCommand) + .andThen(quickHandoff) + .withName("Algae Score + Ground Handoff"); + } else { + scoreCommand = + Commands.sequence( + bargeScoreCommand, + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + .withName("Barge Algae Score"); + } + } + + case NO_GAME_PIECE -> { + scoreCommand = + Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) + .alongWith(scoringModeSelectRumble()) + .withName("Algae Scoring Mode"), + AutoAlgaeHeights.autoAlgaeIntakeCommand( + s.drivebaseSubsystem, superStructure, this) + .until(() -> sensors.armSensor.booleanInClaw())); + } + + default -> { + scoreCommand = Commands.none(); + } + } + + return scoreCommand; + }) + .withName("Solo Left Trigger Action")); + soloController .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) @@ -966,7 +997,7 @@ private void configureSoloControllerBindings() { soloController .rightTrigger() .onTrue( - Commands.runOnce( + Commands.deferredProxy( () -> { Command scoreCommand = switch (soloScoringMode) { @@ -974,7 +1005,6 @@ private void configureSoloControllerBindings() { case ALGAE_IN_CLAW -> Commands.sequence( superStructure.algaeProcessorScore( soloController.rightBumper()), - Commands.waitSeconds(0.7), Commands.runOnce( () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) .withName("Processor score"); @@ -985,7 +1015,7 @@ private void configureSoloControllerBindings() { superStructure.coralPreIntake(), s.climbPivotSubsystem.toStow()); }; - CommandScheduler.getInstance().schedule(scoreCommand); + return scoreCommand; }) .withName("score")); soloController @@ -993,14 +1023,26 @@ private void configureSoloControllerBindings() { .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); - // Ground Intake soloController .leftBumper() .onTrue( - Commands.parallel( - Commands.runOnce(() -> intakeMode = ScoringMode.CORAL), - superStructure.quickGroundIntake(soloController.povUp())) - .withName("Quick Gound intake")); + Commands.runOnce( + () -> { + Command intakeCommand; + switch (soloScoringMode) { + case CORAL_IN_CLAW, NO_GAME_PIECE -> intakeCommand = + superStructure + .quickGroundIntake(soloController.povUp()) + .withName("Quick Ground Intake") + .alongWith(Commands.runOnce(() -> intakeMode = ScoringMode.CORAL)); + case ALGAE_IN_CLAW -> intakeCommand = + superStructure + .holdGroundIntakeOut(soloController.povUp()) + .withName("Hold Ground Intake Out"); + default -> intakeCommand = Commands.none(); + } + CommandScheduler.getInstance().schedule(intakeCommand); + })); // Scoring levels coral and algae intake heights soloController .y() diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 5d77eddc..f528043c 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -59,4 +59,9 @@ public Trigger inIntake() { }) .debounce(0.1); } + + public boolean booleanInIntake() { + double distance = getSensorDistance().in(Meters); + return distance > INTAKE_LOWER_LIMIT && distance < INTAKE_UPPER_LIMIT; + } } diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index e8c15b5f..04886fd5 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -160,8 +160,7 @@ public Command algaeIntakePower() { return setPower(ALGAE_INTAKE_SPEED).withName("Algae intake power"); } - public Command - algaeExtakePower() { // can change to net extake maybe? also bound as general extake though + public Command algaeExtakeNetPower() { return setPower(ALGAE_EXTAKE_SPEED).withName("Algae extake power"); } diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..484e469b 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -203,6 +203,21 @@ public Command groundIntake(BooleanSupplier retract) { } } + public Command holdGroundIntakeOut(BooleanSupplier retract) { + if (groundSpinny == null || groundArm == null) { + return Commands.none().withName("hold ground intake disabled"); + } + + return Commands.sequence( + Commands.parallel( + groundArm + .moveToPosition(GroundArm.GROUND_POSITION) + .andThen(groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE)) + .withDeadline(Commands.waitUntil(retract)), + groundSpinny.setGroundIntakePower())) + .withName("Hold Ground Intake Out"); + } + // This is the actual version in use. It moves the coral directly into the claw. public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph if (groundSpinny == null || groundArm == null || intakeSensor == null) { @@ -421,7 +436,7 @@ public Command algaeNetScore(BooleanSupplier score) { armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), spinnyClaw.algaeIntakePower()), Commands.waitUntil(score), - spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), + spinnyClaw.algaeExtakeNetPower(), Commands.waitSeconds(0.7), armPivot.moveToPosition( ArmPivot.CORAL_PRESET_UP), // added to prevent hitting the barge after scoring net