From e98fcba1accc4fc55376c0092540b64cfbd98111 Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla <49494444+BrownGenius@users.noreply.github.com> Date: Tue, 2 Apr 2024 19:12:36 -0400 Subject: [PATCH] Add support for a second intake sensor (#107) * Add support for a second intake sensor * Made changes to drive controls * Fixed drive to amp driver control * Do not control arm for prepare to score when in amp mode * Avoid using arm at all when aiming for amp * Split rightBumper to 4 separate triggers * Split ParallelCommands into separate scheduled commands * Set default scoring mode to vision-based * Recalibrate shooter camera location --------- Co-authored-by: joshuman --- .../frc/robot/config/RobotConfigInferno.java | 4 +- .../frc/robot/controls/DriverControls.java | 68 ++++++++++++------- .../subsystems/intake/IntakeIOSparkMax.java | 6 +- .../java/frc/robot/util/DevilBotState.java | 2 +- 4 files changed, 50 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/config/RobotConfigInferno.java b/src/main/java/frc/robot/config/RobotConfigInferno.java index 57e4049c..72b6fecc 100644 --- a/src/main/java/frc/robot/config/RobotConfigInferno.java +++ b/src/main/java/frc/robot/config/RobotConfigInferno.java @@ -132,8 +132,8 @@ public RobotConfigInferno() { "shooter", "1188", new Transform3d( - new Translation3d(-Units.inchesToMeters(10.5), 0, Units.inchesToMeters(13)), - new Rotation3d(0, Units.degreesToRadians(-28), Units.degreesToRadians(180))))); + new Translation3d(-Units.inchesToMeters(9.5), 0, Units.inchesToMeters(14)), + new Rotation3d(0, Units.degreesToRadians(-32), Units.degreesToRadians(180))))); cameras.add( new VisionCamera( diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index ed079013..37b8b60e 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -1,6 +1,10 @@ package frc.robot.controls; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -17,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SelectCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -198,14 +203,11 @@ private static void setupCommonControls(CommandXboxController controller) { /* Y Button = Prepare to Climb * X Button = Climber */ - controller - .y() - .onTrue( - new ParallelCommandGroup( - RobotConfig.intake.getTurnOffCommand(), - RobotConfig.shooter.getTurnOffCommand(), - RobotConfig.arm.getStowCommand(), - RobotConfig.climber.getExtendCommand())); + controller.y().onTrue(RobotConfig.intake.getTurnOffCommand()); + controller.y().onTrue(RobotConfig.shooter.getTurnOffCommand()); + controller.y().onTrue(RobotConfig.arm.getStowCommand()); + controller.y().onTrue(RobotConfig.climber.getExtendCommand()); + controller.x().onTrue(RobotConfig.climber.getRetractCommand()); /* Target Selection Controls */ @@ -288,30 +290,44 @@ public static void setupMainControls(CommandXboxController mainController) { .onTrue( new EjectPiece(RobotConfig.intake, RobotConfig.arm, RobotConfig.shooter)); // Eject Note + mainController.rightBumper().onTrue(RobotConfig.intake.getTurnOffCommand()); + mainController .rightBumper() .onTrue( - new ParallelCommandGroup( - RobotConfig.intake.getTurnOffCommand(), - new DriveToYaw(RobotConfig.drive, () -> DevilBotState.getVisionRobotYawToTarget()) - .withTimeout(DriveConstants.pidTimeoutInSeconds), - new SetShooterVelocity( - RobotConfig.shooter, () -> DevilBotState.getShooterVelocity()) - .withTimeout(ShooterConstants.pidTimeoutInSeconds), // turn on shooter - /* TODO: Use ArmToPositionTP instead of setting arm angle directly */ - new InstantCommand( + new SelectCommand<>( + Map.ofEntries( + Map.entry( + true, + AutoBuilder.pathfindToPoseFlipped( + new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)), + new PathConstraints(4.0, 3.0, 2 * Math.PI, 3 * Math.PI))), + Map.entry( + false, + new DriveToYaw( + RobotConfig.drive, () -> DevilBotState.getVisionRobotYawToTarget()) + .withTimeout(DriveConstants.pidTimeoutInSeconds))), + () -> DevilBotState.isAmpMode())); + + mainController + .rightBumper() + .onTrue( + new SetShooterVelocity(RobotConfig.shooter, () -> DevilBotState.getShooterVelocity()) + .withTimeout(ShooterConstants.pidTimeoutInSeconds) // turn on shooter + ); + + mainController + .rightBumper() + .onTrue( + new InstantCommand( () -> { - if (DevilBotState.isAmpMode()) { - RobotConfig.arm.setAngle(ArmConstants.ampScoreAngleInDegrees); - } else { - Optional armAngle = DevilBotState.getArmAngleToTarget(); - if (armAngle.isPresent()) { - RobotConfig.arm.setAngle((armAngle.get())); - } + Optional armAngle = DevilBotState.getArmAngleToTarget(); + if (armAngle.isPresent()) { + RobotConfig.arm.setAngle((armAngle.get())); } }, - RobotConfig.arm) // adjust arm angle based on vision's distance from target - )); // Aim + RobotConfig.arm) + .onlyIf(() -> !DevilBotState.isAmpMode())); mainController .rightTrigger() diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 34225e1c..d3aeaef8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -15,6 +15,7 @@ public class IntakeIOSparkMax implements IntakeIO { // Gets the NEO encoder private final RelativeEncoder encoder; DigitalInput limitSwitchIntake = new DigitalInput(1); + DigitalInput limitSwitchIntakeSecondary = new DigitalInput(2); public IntakeIOSparkMax(int id, boolean inverted) { leader = new CANSparkMax(id, MotorType.kBrushless); @@ -31,7 +32,10 @@ public IntakeIOSparkMax(int id, boolean inverted) { @Override public void updateInputs(IntakeIOInputs inputs) { inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.limitSwitchIntake = !limitSwitchIntake.get(); + inputs.limitSwitchIntake = + !limitSwitchIntake.get() + || !limitSwitchIntakeSecondary + .get(); // Assume note in intake if either sensor is triggered inputs.current = leader.getOutputCurrent(); inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); diff --git a/src/main/java/frc/robot/util/DevilBotState.java b/src/main/java/frc/robot/util/DevilBotState.java index 6a000ba8..68fb777c 100644 --- a/src/main/java/frc/robot/util/DevilBotState.java +++ b/src/main/java/frc/robot/util/DevilBotState.java @@ -137,7 +137,7 @@ public enum SpeakerShootingMode { SPEAKER_VISION_BASED, } - private static SpeakerShootingMode shootingMode = SpeakerShootingMode.SPEAKER_FROM_SUBWOOFER; + private static SpeakerShootingMode shootingMode = SpeakerShootingMode.SPEAKER_VISION_BASED; public static void setShootingMode(SpeakerShootingMode position) { DevilBotState.shootingMode = position;