diff --git a/.vscode/settings.json b/.vscode/settings.json index d7d9976..821fb3c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -59,7 +59,7 @@ "edu.wpi.first.math.**.struct.*" ], "java.dependency.enableDependencyCheckup": false, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable", + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable", "java.format.settings.url": ".vscode/java-formatter.xml", "editor.formatOnSave": true, "editor.defaultFormatter": "redhat.java", diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6f86eca..7f8db9e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -77,12 +77,12 @@ public static class CANConstants * CAN IDs 1 through 13 are used by the drive subsystem and configured in * TunerConstants */ - public static final int INTAKE = 14; // Vortex - public static final int INTAKE_EXTEND = 15; // Vortex - public static final int FEEDER_MOTOR = 16; // Vortex - public static final int TURRET_MOTOR = 17; // Talon - public static final int FLYWHEEL_LEAD = 18; // Vortex - public static final int FLYWHEEL_FOLLOW = 19; // Vortex + public static final int INTAKE = 14; // Vortex + public static final int INTAKE_EXTEND = 15; // Vortex + public static final int FEEDER_MOTOR = 16; // Vortex + public static final int TURRET_MOTOR = 17; // Talon + public static final int FLYWHEEL_LEAD = 18; // Vortex + public static final int FLYWHEEL_FOLLOW = 19; // Vortex public static final int CLIMBER_EXTEND = 21; public static final int CLIMBER_ROTATE = 22; public static final int ROTOR_MOTOR = 23; // Vortex @@ -123,8 +123,8 @@ public static class DriveConstants public static class IntakeConstants { - public static final Voltage INTAKE_VOLTS = Volts.of(9.0); - public static final Voltage REVERSE_VOLTS = Volts.of(-9.0); + 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 EXTENSION_CURRENT_LIMIT = Amps.of(60); @@ -145,6 +145,7 @@ public static class IntakeConstants public static final Distance JIGGLE_LIMIT_MARGIN = Inches.of(0.0); public static final Time JIGGLE_MOVE_TIMEOUT = Seconds.of(0.60); public static final Time JIGGLE_PAUSE_TIME = Seconds.of(0.02); + public static final Time RETRACT_TIMEOUT = Seconds.of(4.0); } public static class GeneralConstants @@ -216,7 +217,19 @@ public static class ShooterConstants public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured + // release + // point + // is + // ~1 + // in + // forward + // of + // the + // pivot + // (turret + // -90), + // not // lateral public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2d06b51..8f51349 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,8 +38,8 @@ 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 Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; - private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; + // private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; + private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; public RobotContainer() { @@ -48,17 +48,17 @@ public RobotContainer() private LinearVelocity getDrive() { - return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getY())).times(_driveMultiplier), DriveConstants.TRANSLATE_DEADBAND); + return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getY())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.TRANSLATE_DEADBAND); } private LinearVelocity getStrafe() { - return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getX())).times(_driveMultiplier), DriveConstants.TRANSLATE_DEADBAND); + return MeasureUtil.applyDeadband(DriveConstants.MAX_SPEED.times(Value.of(-_driver.getX())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.TRANSLATE_DEADBAND); } private AngularVelocity getRotate() { - return MeasureUtil.applyDeadband(DriveConstants.MAX_ANGULAR_RATE.times(Value.of(-_driver.getTwist())).times(_driveMultiplier), DriveConstants.ROTATE_DEADBAND); + return MeasureUtil.applyDeadband(DriveConstants.MAX_ANGULAR_RATE.times(Value.of(-_driver.getTwist())).times((-_driver.getRawAxis(3) + 1) / 2.0), DriveConstants.ROTATE_DEADBAND); } private SwerveRequest.FieldCentric getFieldCentricRequest() @@ -79,17 +79,27 @@ private void configureBindings() final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled().whileTrue(_drive.applyRequest(() -> idle).ignoringDisable(true)); - _driver.button(1).whileTrue(Commands.parallel(_shooter.shoot(), Commands.startEnd(() -> _drive.disableVisionPoseCorrection(true), () -> _drive.disableVisionPoseCorrection(false)))); - _driver.button(2).whileTrue(Commands.startEnd(() -> _driveMultiplier = DriveConstants.SLOW_MODE_SCALE, () -> _driveMultiplier = DriveConstants.FULL_SPEED_SCALE)); - _driver.button(3).whileTrue(_drive.applyRequest(() -> _robotCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate()))); + // _driver.button(1).whileTrue(Commands.parallel(_shooter.shoot(), + // Commands.startEnd(() -> _drive.disableVisionPoseCorrection(true), () -> + // _drive.disableVisionPoseCorrection(false)))); + _driver.button(1).whileTrue(_shooter.manualShootCmd(() -> _manualFlywheelRPM)); + // _driver.button(2).whileTrue(Commands.startEnd(() -> _driveMultiplier = + // DriveConstants.SLOW_MODE_SCALE, () -> _driveMultiplier = + // DriveConstants.FULL_SPEED_SCALE)); + _driver.button(2).whileTrue(_intake.runRollersForward()); + _driver.button(3).onTrue(_intake.getExtendCmd()); + _driver.button(4).onTrue(_intake.getRetractCmd()); _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.homeTurret()); + _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()))); _operator.leftTrigger().whileTrue(_intake.runRollersForward()); _operator.leftBumper().whileTrue(_intake.runRollersReverse()); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 90a8ae5..949811e 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -104,7 +104,7 @@ public Command jiggle() public Command getRetractCmd() { - return Commands.sequence(runOnce(() -> setRollerCurrentLimitMode(RollerCurrentLimitMode.ForceActive)), startRollersForward(), setExtensionCmd(false), Commands.waitUntil(this::isRetracted)).finallyDo(() -> + return Commands.sequence(runOnce(() -> setRollerCurrentLimitMode(RollerCurrentLimitMode.ForceActive)), startRollersForward(), setExtensionCmd(false), Commands.waitUntil(this::isRetracted)).withTimeout(2.0).finallyDo(() -> { setIntakeState(IntakeState.Off); setRollerCurrentLimitMode(RollerCurrentLimitMode.AutoByExtension); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c3ec92a..f9159bf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Seconds; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; @@ -17,6 +18,8 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.shooter.TurretDirector.ShotMode; @@ -198,6 +201,16 @@ public Command setManualTurretAngle(Angle angle) return runOnce(() -> setManualTurretAngleCommand(angle)); } + public Command manualShootCmd(DoubleSupplier speed) + { + // @formatter:off + return Commands.parallel( + Commands.startEnd(() -> setManualFlywheel(speed.getAsDouble()), () -> stopManualFlywheel()), + runManualFeeder() + ); + // @formatter:on + } + public Command trackOnly() { return startEnd(() ->