diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 54f808a..92d2126 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -126,7 +126,7 @@ public static class IntakeConstants public static final Voltage INTAKE_VOLTS = Volts.of(4.0); public static final Voltage REVERSE_VOLTS = Volts.of(-6.0); public static final Current ROLLER_CURRENT_LIMIT_EXTENDED = Amps.of(80); - public static final Current ROLLER_CURRENT_LIMIT_ACTIVE = Amps.of(40); + public static final Current ROLLER_CURRENT_LIMIT_ACTIVE = Amps.of(60); public static final Current EXTENSION_CURRENT_LIMIT = Amps.of(60); public static final int CAMERA_DEVICE_INDEX = 0; public static final String CAMERA_NAME = "IntakeCam"; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e463808..2097a02 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,21 +11,19 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.TunerConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.MeasureUtil; -import frc.robot.NetParams; @Logged public class RobotContainer @@ -40,9 +38,9 @@ public class RobotContainer private final Intake _intake = new Intake(); private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended, _intake::isRetracted); private final Autos _autos = new Autos(_drive, _shooter, _intake); + private boolean _driverPOVReleased = true; // private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; - private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; - private NetParams _params; + private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; public RobotContainer() { @@ -51,17 +49,17 @@ public RobotContainer() private LinearVelocity getDrive() { - return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getY())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.TRANSLATE_DEADBAND); + return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getY())).times((-_driver.getRawAxis(3) + 1) * 0.375 + 0.25), DriveConstants.TRANSLATE_DEADBAND); } private LinearVelocity getStrafe() { - return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getX())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.TRANSLATE_DEADBAND); + return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getX())).times((-_driver.getRawAxis(3) + 1) * 0.375 + 0.25), DriveConstants.TRANSLATE_DEADBAND); } private AngularVelocity getRotate() { - return MeasureUtil.applyDeadband(DriveConstants.MAX_ANGULAR_RATE.times(Value.of(-_driver.getTwist())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.ROTATE_DEADBAND); + return MeasureUtil.applyDeadband(DriveConstants.MAX_ANGULAR_RATE.times(Value.of(-_driver.getTwist())).times((-_driver.getRawAxis(3) + 1) * 0.375 + 0.25), DriveConstants.ROTATE_DEADBAND); } private SwerveRequest.FieldCentric getFieldCentricRequest() @@ -69,6 +67,12 @@ private SwerveRequest.FieldCentric getFieldCentricRequest() return _fieldCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate()); } + private void setManualFlywheelRPM(double rpm) + { + _manualFlywheelRPM = Math.max(0.0, rpm); + _shooter.setManualFlywheel(_manualFlywheelRPM); + } + private void configureBindings() { _drive.setDefaultCommand(_drive.applyRequest(this::getFieldCentricRequest)); @@ -79,7 +83,7 @@ private void configureBindings() // _driver.button(1).whileTrue(Commands.parallel(_shooter.shoot(), // Commands.startEnd(() -> _drive.disableVisionPoseCorrection(true), () -> // _drive.disableVisionPoseCorrection(false)))); - _driver.button(1).whileTrue(_shooter.manualShootCmd(() -> _params.flywheelSpeed)); + _driver.button(1).whileTrue(_shooter.manualShootCmd(() -> _manualFlywheelRPM)); // _driver.button(2).whileTrue(Commands.startEnd(() -> _driveMultiplier = // DriveConstants.SLOW_MODE_SCALE, () -> _driveMultiplier = // DriveConstants.FULL_SPEED_SCALE)); @@ -89,24 +93,46 @@ private void configureBindings() _driver.button(5).whileTrue(_shooter.pass()); _driver.button(6).whileTrue(_shooter.trackOnly()); _driver.button(7).onTrue(_drive.runOnce(_drive::seedFieldCentric)); - _driver.button(8).onTrue(_shooter.setManualTurretAngle(Degrees.zero())); - _driver.button(9).onTrue(_shooter.setManualTurretAngle(Degrees.of(45.0))); - _driver.button(10).onTrue(_shooter.setManualTurretAngle(Degrees.of(-45.0))); - _driver.button(11).onTrue(_shooter.setManualTurretAngle(Degrees.of(90.0))); - _driver.button(12).onTrue(_shooter.setManualTurretAngle(Degrees.of(-90.0))); - _driver.button(13).onTrue(_shooter.setManualTurretAngle(Degrees.of(135.0))); - _driver.button(14).onTrue(_shooter.setManualTurretAngle(Degrees.of(-135.0))); + // _driver.button(8).onTrue(_shooter.setManualTurretAngle(Degrees.zero())); + // _driver.button(9).onTrue(_shooter.setManualTurretAngle(Degrees.of(45.0))); + // _driver.button(10).onTrue(_shooter.setManualTurretAngle(Degrees.of(-45.0))); + // _driver.button(11).onTrue(_shooter.setManualTurretAngle(Degrees.of(90.0))); + // _driver.button(12).onTrue(_shooter.setManualTurretAngle(Degrees.of(-90.0))); + // _driver.button(13).onTrue(_shooter.setManualTurretAngle(Degrees.of(135.0))); + // _driver.button(14).onTrue(_shooter.setManualTurretAngle(Degrees.of(-135.0))); _driver.button(16).whileTrue(_drive.applyRequest(() -> _robotCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate()))); + // Debounce the transition out of -1 by 50ms + Trigger povJustPressed = _driver.pov(-1).negate().debounce(0.05).and(() -> _driverPOVReleased); + + povJustPressed.onTrue(Commands.runOnce(() -> + { + int angle = _driver.getHID().getPOV(); + _driverPOVReleased = false; + + _shooter.setManualTurretAngleCommand(Degrees.of(switch (angle) + { + case 0 -> -90; + case 180 -> 90; + case 225 -> 45; + case 315 -> -45; + default -> 0; + })); + + })); + + _driver.pov(-1).onTrue(Commands.runOnce(() -> _driverPOVReleased = true)); + _operator.leftTrigger().whileTrue(_intake.runRollersForward()); _operator.leftBumper().whileTrue(_intake.runRollersReverse()); _operator.rightTrigger().whileTrue(_intake.jiggle()); + _operator.y().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(MANUAL_FLYWHEEL_START_RPM))); + _operator.x().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM - MANUAL_FLYWHEEL_STEP_RPM))); + _operator.b().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM + MANUAL_FLYWHEEL_STEP_RPM))); _operator.a().onTrue(Commands.runOnce(_shooter::stopManualFlywheel)); _operator.rightBumper().whileTrue(_shooter.runManualFeeder()); _operator.povDown().onTrue(_intake.getRetractCmd()); _operator.povUp().onTrue(_intake.getExtendCmd()); - - SmartDashboard.putData(_shooter.setTurretAngleCmd(() -> _params.turretAngle)); } public Command getAutonomousCommand() diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 4cb28ce..7c9ae58 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -342,7 +342,7 @@ private void beginManualControl(boolean clearTurretTarget) } } - private void setManualTurretAngleCommand(Angle angle) + public void setManualTurretAngleCommand(Angle angle) { beginManualControl(true); _turret.setManualAngle(angle);