diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 644c07e..2e9f4ed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -161,7 +161,7 @@ public static class FieldConstants { // x: .2472 y: 5.5946 (USE THIS) public static final Pose3d SPEAKER_POSE_BLUE = new Pose3d(0.17, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 180)); - public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.8, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0)); // TODO: fix X here + public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.4, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0)); // TODO: fix X here } public static class LEDColors { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d9aec29..90f8d2d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -164,18 +165,30 @@ private void configureBindings() { // ); // driver bindings + _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem)); _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem)); - _driveController.L1().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetPose(new Pose2d()), _swerveSubsystem)); - _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem)); - _driveController.L2().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem)); - _driveController.triangle().onTrue(Commands.runOnce(_swerveSubsystem::resetGyro, _swerveSubsystem)); + _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem)); + + _driveController.L2().whileTrue( + new AutoAim( + _shooterSubsystem, + _elevatorSubsystem, + _ledSubsystem, + _swerveSubsystem, + () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05), + () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05), + Presets.CLOSE_SHOOTER_ANGLE, + Presets.CLOSE_ELEVATOR_HEIGHT, + () -> (UtilFuncs.GetAlliance() == Alliance.Red) ? 0 : 180 + ) + ); _driveController.R2().whileTrue( new AutoAim( _shooterSubsystem, _elevatorSubsystem, - null, + _ledSubsystem, _swerveSubsystem, () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05), () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05) diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index 277951d..95fe879 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -88,6 +88,44 @@ public AutoAim( addRequirements(_swerve, _shooter, _elevator); } + /** + * Constructs an AutoAim that does NOT finish when reaching setpoints. + * It DOES NOT calculate angle/height/heading setpoints. + * + * (USE FOR TELEOP) + * + * @param shooter Shooter subsystem. + * @param elevator Elevator subsystem. + * @param leds Led subsystem. + * @param swerve Swerve subsystem. + * @param xSpeed X joystick speed. + * @param ySpeed Y joystick speed. + * + * @param shooterAngle Desired shooter angle. + * @param elevatorHeight Desired elevator height. + * @param swerveHeadingSupplier Desired swerve heading supplier. This is a supplier since heading could change based on alliance. + */ + public AutoAim( + ShooterSubsystem shooter, + ElevatorSubsystem elevator, + LEDSubsystem leds, + SwerveDriveSubsystem swerve, + DoubleSupplier xSpeed, + DoubleSupplier ySpeed, + double shooterAngle, + double elevatorHeight, + DoubleSupplier swerveHeadingSupplier + ) { + this(shooter, elevator, leds, swerve, xSpeed, ySpeed); + + _desiredShooterAngle = shooterAngle; + _desiredElevatorHeight = elevatorHeight; + _swerveHeadingSupplier = swerveHeadingSupplier; + + _overrideDesired = true; + } + + /** * Constructs an AutoAim that finishes when it reaches its initial setpoints. * It calculates angle/height/heading setpoints.