diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 05484f3..bef243c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -615,7 +615,7 @@ public static VisionSystemSim getSystemSim() { public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Degrees.of(123.0); + public static final Angle PICKUP_ANGLE = Degrees.of(130.0); // 123.0 public static final Angle STOW_ANGLE = Degrees.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b9c96a1..d291e49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,6 @@ package frc.robot; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.BaseStatusSignal; @@ -22,11 +21,9 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.W8.io.motor.*; -import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; @@ -281,6 +278,7 @@ public RobotContainer() { } // Set up auto routines + // LEAVE THIS UP HERE autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); // Set up SysId routines @@ -331,111 +329,55 @@ private void configureButtonBindings() { // // Switch to X pattern when X button is pressed // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - // Reset gyro to 0° when B button is pressed - // controller - // .b() - // .onTrue( - // Commands.runOnce( - // () -> - // drive.setPose( - // new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - // drive) - // .ignoringDisable(true)); - - // controller - // .x() - // .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - - // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); - - // controller - // .leftTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // Robot.fuelSim.clearFuel(); - // Robot.fuelSim.spawnStartingFuel(); - // intake.simBalls = 0; - // })); - // controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(RotationsPerSecond.of(10)))); - // controller.a().onTrue(intake.intake()); - // controller.x().onTrue(intake.stowAndStopRollers()); - - // Flywheel - controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(100))); - controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - - // Intake Rollers 11 Motor: 9 Intake - - // ALPHA KATIE REQUESTS INTAKE RIGHT TRIGGER, SHOOT LEFT TRIGGER, AUTO ALIGN "A" - - // Intake - controller.rightTrigger().whileTrue(intake.intake()); - controller.rightTrigger().onFalse(intake.stowAndStopRollers()); - // Shoot + controller.leftTrigger().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(67))); + controller.leftTrigger().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); + + // Feed controller - .leftTrigger() + .leftBumper() .whileTrue( Commands.parallel( - shooter.prepareToShoot(RotationsPerSecond.of(100), RotationsPerSecond.of(30)), - hopper.runSpindexer(RotationsPerSecond.of(15)))); - + shooter.runTower(RotationsPerSecond.of(30)), + hopper.runSpindexer(RotationsPerSecond.of(15)), + intake.intake())); controller - .leftTrigger() - .onFalse( + .leftBumper() + .whileFalse( Commands.parallel( - shooter.prepareToShoot(RotationsPerSecond.of(0), RotationsPerSecond.of(0)), - hopper.runSpindexer(RotationsPerSecond.of(0)))); - - // Align - // controller.a().onTrue(); - - // controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5)))); - // controller - // .a() - // .onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake)); + shooter.runTower(RotationsPerSecond.of(0)), + hopper.runSpindexer(RotationsPerSecond.of(0)), + Commands.run(() -> intake.stop()))); - // Spindexer 1:1 - // controller.x().whileTrue(hopper.runSpindexer(18)); - // controller.x().onFalse(hopper.runSpindexer(0)); + // Intake + Out + controller.rightTrigger().whileTrue(intake.intake()); + controller.rightTrigger().onFalse(Commands.run(() -> intake.stop())); - // Tower - 15 Motor:7 Tower + // Align + controller.a().onTrue(getAutonomousCommand()); + controller.a().onFalse(getAutonomousCommand()); - // controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(70))); - // controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); + // Jostle + controller.b().onTrue(getAutonomousCommand()); + controller.b().onFalse(getAutonomousCommand()); - // controller.b().onFalse(shooter.setHoodAngle(ShooterRotaryConstants.STARTING_ANGLE.magnitude())); - // controller.povLeft().onTrue(shooter.calibrateHood()); - // controller.povLeft().onTrue(shooter.setHoodAngle(10)); - // controller.povDown().onTrue(shooter.setHoodAngle(15)); - // controller.povRight().onTrue(shooter.setHoodAngle(20)); + // Calibrate Hood + controller.y().onTrue(shooter.calibrateHood()); - // controller.x().onFalse(intake.setPivotAngle(IntakePivotConstants.STOW_ANGLE)); - controller.x().whileTrue(intake.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); + // Stow Intake controller.y().whileTrue(intake.setPivotAngle(IntakePivotConstants.STOW_ANGLE)); - controller.b().whileTrue(intake.zeroEncoder()); - - // controller.povRight().onTrue(shooter.calibrateHood()); - - controller - .povLeft() - .whileTrue( - new RunCommand( - () -> - climber._io.runPosition( - Rotations.of(0.25), - ClimberConstants.CRUISE_VELOCITY, - ClimberConstants.ACCELERATION, - ClimberConstants.JERK, - PIDSlot.SLOT_1), - climber)); - controller.povLeft().onFalse(climber.stopClimber()); + // Climber Raise/Lower controller.povUp().whileTrue(climber.raiseClimber()); controller.povUp().onFalse(climber.stopClimber()); controller.povDown().whileTrue(climber.lowerClimber()); controller.povDown().onFalse(climber.stopClimber()); + + // Testing Commands + controller.povLeft().onTrue(shooter.calibrateHood()); + controller.povLeft().onTrue(shooter.setHoodAngle(6.8)); + controller.povDown().onTrue(shooter.setHoodAngle(12.1)); + controller.povRight().onTrue(shooter.setHoodAngle(18.6)); } /** diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 4cb36ee..f724f9b 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -144,13 +144,14 @@ public boolean hoodAtAngle() { return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE; } + // Increases Hood Angle public Command incrementHoodAngle() { Angle currentHoodAngle = _hood.getPosition().plus(Degrees.of(2.5)); return setHoodAngle(currentHoodAngle.in(Degrees)); } public Command decrementHoodAngle() { - Angle currentHoodAngle = _hood.getPosition().minus(Degrees.of(0.5)); + Angle currentHoodAngle = _hood.getPosition().minus(Degrees.of(2.5)); return setHoodAngle(currentHoodAngle.in(Degrees)); }