From ade03f38f1b0271710e3673e4ca536396ecc1838 Mon Sep 17 00:00:00 2001 From: Lucas <75286864+bot3O3@users.noreply.github.com> Date: Fri, 5 Apr 2024 13:14:37 -0400 Subject: [PATCH] Dcm pfixing auto intaking (#174) * changed command structure off intake and raising arm command * changed auto to start after intake command during rase arm to insert it into preveous cycle. FIxed issue where the intake was "too fast" and reflective sensor was triggering seemingly earlier in auto * spot :/ --- .../autos/B-ThreePieceAutoTame.auto | 2 +- .../pathplanner/autos/B-TwoPieceFar1.auto | 2 +- .../pathplanner/autos/R-TheTwoPieceNear.auto | 2 +- .../autos/R-ThreePieceAutoTame.auto | 2 +- .../pathplanner/autos/R-TwoPieceFar1.auto | 2 +- src/main/java/frc/robot/RobotContainer.java | 33 ++++++++++--------- src/main/java/frc/robot/intake/Intake.java | 11 +++++++ 7 files changed, 34 insertions(+), 20 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/B-ThreePieceAutoTame.auto b/src/main/deploy/pathplanner/autos/B-ThreePieceAutoTame.auto index 5a0b707..d6889f7 100644 --- a/src/main/deploy/pathplanner/autos/B-ThreePieceAutoTame.auto +++ b/src/main/deploy/pathplanner/autos/B-ThreePieceAutoTame.auto @@ -65,7 +65,7 @@ { "type": "named", "data": { - "name": "raiseArmAndWaitWhileAdvancingPiece" + "name": "raiseArmAndWait" } } ] diff --git a/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto b/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto index ee6d998..924c501 100644 --- a/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto +++ b/src/main/deploy/pathplanner/autos/B-TwoPieceFar1.auto @@ -70,7 +70,7 @@ { "type": "named", "data": { - "name": "raiseArmAndWaitWhileAdvancingPiece" + "name": "raiseArmAndWait" } }, { diff --git a/src/main/deploy/pathplanner/autos/R-TheTwoPieceNear.auto b/src/main/deploy/pathplanner/autos/R-TheTwoPieceNear.auto index 55ba9be..f198853 100644 --- a/src/main/deploy/pathplanner/autos/R-TheTwoPieceNear.auto +++ b/src/main/deploy/pathplanner/autos/R-TheTwoPieceNear.auto @@ -65,7 +65,7 @@ { "type": "named", "data": { - "name": "raiseArmAndWaitWhileAdvancingPiece" + "name": "raiseArmAndWait" } } ] diff --git a/src/main/deploy/pathplanner/autos/R-ThreePieceAutoTame.auto b/src/main/deploy/pathplanner/autos/R-ThreePieceAutoTame.auto index cf126a3..1e454ed 100644 --- a/src/main/deploy/pathplanner/autos/R-ThreePieceAutoTame.auto +++ b/src/main/deploy/pathplanner/autos/R-ThreePieceAutoTame.auto @@ -65,7 +65,7 @@ { "type": "named", "data": { - "name": "raiseArmAndWaitWhileAdvancingPiece" + "name": "raiseArmAndWait" } } ] diff --git a/src/main/deploy/pathplanner/autos/R-TwoPieceFar1.auto b/src/main/deploy/pathplanner/autos/R-TwoPieceFar1.auto index a568f3a..f0dec7a 100644 --- a/src/main/deploy/pathplanner/autos/R-TwoPieceFar1.auto +++ b/src/main/deploy/pathplanner/autos/R-TwoPieceFar1.auto @@ -70,7 +70,7 @@ { "type": "named", "data": { - "name": "raiseArmAndWaitWhileAdvancingPiece" + "name": "raiseArmAndWait" } }, { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 319069a..f9f0e72 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -245,10 +245,6 @@ private void createNamedCommands() { NamedCommands.registerCommand("raiseArmAndWait", m_arm.createDeployCommand() .andThen(new WaitCommand(1.8))); - - NamedCommands.registerCommand("raiseArmAndWaitWhileAdvancingPiece", - m_arm.createDeployCommand().alongWith(m_intake.createSetVelocityCommand(0.5)) - .andThen(new WaitCommand(1.8)).andThen(m_intake.createStopIntakeCommand())); NamedCommands.registerCommand("resetArmAndIntake", m_arm.createStowCommand() @@ -259,8 +255,11 @@ private void createNamedCommands() { .withTimeout(0.7)); NamedCommands.registerCommand("intakePiece", - createAutoIntakeCommandSequence() - ); + m_arm.createStowCommand() + .andThen(m_intake.createIntakeCommand()).until(m_intake.hasNote) + .andThen(m_arm.createCarryCommand() + .alongWith(m_intake.createAdvanceAfterIntakingCommand())) + ); NamedCommands.registerCommand("stopIntake", m_intake.createStopIntakeCommand()); @@ -288,16 +287,18 @@ private void configureDriverButtonBindings() { new JoystickButton(m_driver,OIConstants.kZorroAIn) .whileTrue((new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematicsDriveFromArm, m_driver))); - Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED)); JoystickButton D_Button = new JoystickButton(m_driver, OIConstants.kZorroDIn); // Reverse intake to outake or reject intaking Note D_Button.and(armDeployed.negate()) - .whileTrue(m_arm.createStowCommand().andThen(new WaitCommand(0.2).andThen(m_intake.createOuttakeToFloorCommand()))); - // Shoot Note into Amp + .whileTrue(m_arm.createStowCommand() + .andThen(new WaitCommand(0.2) + .andThen(m_intake.createOuttakeToFloorCommand()))); + + // Shoot Note into Amp D_Button.and(armDeployed) - .whileTrue(m_intake.createOuttakeToAmpCommand()); + .whileTrue(m_intake.createOuttakeToAmpCommand()); } // spotless:on @@ -307,7 +308,6 @@ private void configureOperatorButtonBindings() { JoystickButton leftBumper = new JoystickButton(m_operator, Button.kLeftBumper.value); JoystickButton leftJoystickDown = new JoystickButton(m_operator, Button.kLeftStick.value); - Trigger hasNote = new Trigger(m_intake.eitherSensorSupplier()); Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED)); // CLIMBER @@ -333,16 +333,19 @@ private void configureOperatorButtonBindings() { // Control position of Note in intake Trigger leftStick = new Trigger(() -> Math.abs(m_operator.getLeftY()) > 0.2); //While arm is down - leftStick.and(armDeployed.negate()).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown)); + leftStick.and(armDeployed.negate()) + .whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown)); + //While arm is up - leftStick.and(armDeployed).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp)); + leftStick.and(armDeployed) + .whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp)); // Intake Note from floor - rightBumper.and(hasNote.negate()) + rightBumper.and(m_intake.hasNote.negate()) .whileTrue(m_arm.createStowCommand() .andThen(m_intake.createIntakeCommand())); - hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop()) + m_intake.hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop()) .onTrue(m_arm.createCarryCommand() .andThen(m_intake.createAdvanceAfterIntakingCommand().withInterruptBehavior(InterruptionBehavior.kCancelIncoming))); diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 1f14cc3..b03524a 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.IntakeConstants.IntakeState; import java.util.function.BooleanSupplier; @@ -61,6 +62,7 @@ public Intake() { m_velocityController.setD(IntakeConstants.kVelocityD); m_positionController.setTolerance(IntakeConstants.kPositionTolerance); + m_positionController.setGoal(17); m_relativeEncoder = m_motor.getEncoder(); m_relativeEncoder.setPosition(0.0); @@ -88,23 +90,30 @@ public Command createStopIntakeCommand() { } private void configurePositionController(double targetPosition) { + SmartDashboard.putNumber("ResetBeam", 1); m_positionController.reset(m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity()); m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition); + m_positionController.calculate(m_relativeEncoder.getPosition()); } // spotless:off private void advanceAfterIntaking(double targetPosition) { m_secondSensorTriggered.rising().ifHigh( () -> { + SmartDashboard.putNumber("RealativeEncoderReset", 0); m_positionController.reset( m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity()); + SmartDashboard.putNumber("RealativeEncoderReset", 1); m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition); + SmartDashboard.putNumber("RealativeEncoderReset", 2); }); setState(IntakeState.PROCESSING); m_motor.set(m_positionController.calculate(m_relativeEncoder.getPosition())); } // spotless:on + public Trigger hasNote = new Trigger(this.eitherSensorSupplier()); + public Command createIntakeCommand() { return this.run( () -> { @@ -182,6 +191,8 @@ public void periodic() { SmartDashboard.putNumber( "IntakeSensorRetroReflective", !m_noteSensorRetroReflective.get() ? 1d : 0d); SmartDashboard.putNumber("IntakeSensorBeamBreak", !m_noteSensorBeamBreak.get() ? 1d : 0d); + SmartDashboard.putNumber("RealativeEncoderReset", -1); + SmartDashboard.putNumber("ResetBeam", -1); if (m_state != null) SmartDashboard.putString("Intake State", m_state.name()); }