diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index d907843d..d2970d0d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -17,6 +19,9 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.function.Function; +import java.util.function.Supplier; +import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization; import org.littletonrobotics.frc2024.commands.auto.AutoBuilder; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision; @@ -52,8 +57,10 @@ import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; +import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.NoteVisualizer; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -62,6 +69,7 @@ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including * subsystems, commands, and button mappings) should be declared here. */ +@ExtensionMethod({GeomUtil.class}) public class RobotContainer { // Load robot state private final RobotState robotState = RobotState.getInstance(); @@ -73,8 +81,13 @@ public class RobotContainer { private Rollers rollers; private final Superstructure superstructure; - // Controller - private final CommandXboxController controller = new CommandXboxController(0); + // Controllers + private final CommandXboxController driverController = new CommandXboxController(0); + private final CommandXboxController operatorController = new CommandXboxController(1); + private final LoggedDashboardBoolean shotTuningMode = + new LoggedDashboardBoolean("Shot Tuning Mode", false); + private final LoggedDashboardBoolean autoAmpScore = + new LoggedDashboardBoolean("Allow Auto Amp Score", false); // Dashboard inputs private final LoggedDashboardChooser autoChooser = @@ -227,12 +240,25 @@ private void configureButtonBindings() { .run( () -> drive.acceptTeleopInput( - -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX())) + -driverController.getLeftY(), + -driverController.getLeftX(), + -driverController.getRightX())) .withName("Drive Teleop Input")); + // Set up toggles + Trigger inShotTuningMode = new Trigger(shotTuningMode::get); + Trigger allowingAutoAmpScore = new Trigger(autoAmpScore::get); + operatorController + .start() + .onTrue(Commands.runOnce(() -> shotTuningMode.set(!inShotTuningMode.getAsBoolean()))); + operatorController + .x() + .onTrue(Commands.runOnce(() -> autoAmpScore.set(!allowingAutoAmpScore.getAsBoolean()))); + // Aim and Rev Flywheels - controller + driverController .a() + .and(inShotTuningMode.negate()) .whileTrue( Commands.startEnd( () -> @@ -241,26 +267,49 @@ private void configureButtonBindings() { drive::clearHeadingGoal) .alongWith(superstructure.aim(), flywheels.shootCommand()) .withName("Prepare Shot")); + // Tuning mode controls + driverController + .a() + .and(inShotTuningMode) + .whileTrue( + Commands.parallel( + superstructure.diagnoseArm(), + flywheels.shootCommand(), + Commands.startEnd( + () -> + drive.setHeadingGoal( + () -> RobotState.getInstance().getAimingParameters().driveHeading()), + drive::clearHeadingGoal))); // Shoot Trigger readyToShoot = - new Trigger(() -> drive.atHeadingGoal() && superstructure.atGoal() && flywheels.atGoal()); + new Trigger(() -> drive.atHeadingGoal() && superstructure.atGoal() && flywheels.atGoal()) + .and(inShotTuningMode.negate()); readyToShoot .whileTrue( Commands.run( - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0))) + () -> driverController.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0))) .whileFalse( Commands.run( - () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); - controller + () -> driverController.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); + // Shoot at current arm and flywheel setpoint + driverController .rightTrigger() .onTrue( Commands.parallel( Commands.waitSeconds(0.5), - Commands.waitUntil(controller.rightTrigger().negate())) - .deadlineWith( - rollers.feedShooter(), superstructure.aim(), flywheels.shootCommand())); + Commands.waitUntil(driverController.rightTrigger().negate())) + .deadlineWith(rollers.feedShooter()) + .finallyDo( + () -> { + Logger.recordOutput( + "RobotState/ShootingArmAngle", superstructure.getArmAngle().getRadians()); + Logger.recordOutput( + "RobotState/ShootingEffectiveDistance", + RobotState.getInstance().getAimingParameters().effectiveDistance()); + }) + .withName("Shoot")); // Intake Floor - controller + driverController .leftTrigger() .whileTrue( superstructure @@ -269,44 +318,43 @@ private void configureButtonBindings() { Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake())) .withName("Floor Intake")); // Eject Floor - controller + driverController .leftBumper() .whileTrue( superstructure .intake() .alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor())) .withName("Eject To Floor")); - // Test rollers with amp score - controller.b().whileTrue(rollers.ampScore()); - // Tune arm look up table - controller - .x() - .whileTrue( - Commands.parallel( - superstructure.diagnoseArm(), - flywheels.shootCommand(), - Commands.startEnd( - () -> - drive.setHeadingGoal( - () -> RobotState.getInstance().getAimingParameters().driveHeading()), - drive::clearHeadingGoal))) - .and(controller.rightBumper()) - .onTrue( - Commands.parallel( - Commands.waitSeconds(0.5), - Commands.waitUntil(controller.rightBumper().negate())) - .deadlineWith(rollers.feedShooter()) - .finallyDo( - () -> { - Logger.recordOutput( - "RobotState/ShootingArmAngle", superstructure.getArmAngle().getRadians()); - Logger.recordOutput( - "RobotState/ShootingEffectiveDistance", - RobotState.getInstance().getAimingParameters().effectiveDistance()); - })); + // Amp scoring + operatorController.a().whileTrue(superstructure.amp()); + driverController.b().whileTrue(rollers.ampScore()); + + // Auto amp scoring + Pose2d goalAmpScorePose = + new Pose2d(FieldConstants.ampCenter, new Rotation2d(Math.PI / 2.0)) + .transformBy( + new Translation2d( + 0.0, + -(DriveConstants.driveConfig.bumperWidthX() / 2.0 + + Units.inchesToMeters(3.0))) + .toTransform2d()); + Supplier ampScoringPoseSupp = () -> AllianceFlipUtil.apply(goalAmpScorePose); + + // Operator controls + Function changeCompensationDegrees = + compensation -> + Commands.runOnce( + () -> { + double currentCompensation = + RobotState.getInstance().getShotCompensationDegrees(); + RobotState.getInstance() + .setShotCompensationDegrees(currentCompensation + compensation); + }); + operatorController.povDown().onTrue(changeCompensationDegrees.apply(-0.1)); + operatorController.povUp().onTrue(changeCompensationDegrees.apply(0.1)); - controller + driverController .y() .onTrue( Commands.runOnce( diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 7ca40292..f2b41874 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -24,7 +24,6 @@ import lombok.Setter; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants; -import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @@ -64,7 +63,10 @@ public record AimingParameters( armAngleMap.put(4.145, .454); } - @Setter @Getter private double shotCompensationDegrees = 0.0; + @AutoLogOutput(key = "RobotState/ShotCompensationDegrees") + @Setter + @Getter + private double shotCompensationDegrees = 0.0; private static RobotState instance; @@ -215,15 +217,11 @@ public AimingParameters getAimingParameters() { robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance - robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance; - Rotation2d armAngleToSpeaker = - new Rotation2d( - targetDistance - ArmConstants.armOrigin.getX(), - FieldConstants.Speaker.centerSpeakerOpening.getZ() - ArmConstants.armOrigin.getY()); - latestParameters = new AimingParameters( targetVehicleDirection, - Rotation2d.fromRadians(armAngleMap.get(targetDistance)), + Rotation2d.fromRadians( + armAngleMap.get(targetDistance) + Units.degreesToRadians(shotCompensationDegrees)), targetDistance, feedVelocity); Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading()); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java index 7fddd054..8a5a16d2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -109,7 +109,7 @@ public void periodic() { gamepieceState = GamepieceState.NONE; } case AMP_SCORE -> { - feeder.setGoal(Feeder.Goal.IDLING); + feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); indexer.setGoal(Indexer.Goal.EJECTING); intake.setGoal(Intake.Goal.IDLING); backpack.setGoal(Backpack.Goal.AMP_SCORING); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index c1a83eba..b7c8043b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -56,6 +56,7 @@ public void periodic() { case INTAKE -> arm.setGoal(Arm.Goal.FLOOR_INTAKE); case STATION_INTAKE -> arm.setGoal(Arm.Goal.STATION_INTAKE); case DIAGNOSTIC_ARM -> arm.setGoal(Arm.Goal.CUSTOM); + case AMP -> arm.setGoal(Arm.Goal.AMP); default -> {} // DO NOTHING ELSE } @@ -87,6 +88,10 @@ public Command intake() { .withName("Superstructure Intaking"); } + public Command amp() { + return startEnd(() -> desiredGoal = Goal.AMP, this::stow).withName("Superstructure Amping"); + } + public Command stationIntake() { return startEnd(() -> desiredGoal = Goal.STATION_INTAKE, this::stow) .withName("Superstructure Station Intaking"); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 1aba29cd..aeb4cb23 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -48,19 +48,14 @@ public class Arm { new LoggedTunableNumber("Arm/LowerLimitDegrees", minAngle.getDegrees()); private static final LoggedTunableNumber upperLimitDegrees = new LoggedTunableNumber("Arm/UpperLimitDegrees", maxAngle.getDegrees()); - private static final LoggedTunableNumber stowDegrees = - new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0); - private static final LoggedTunableNumber stationIntakeDegrees = - new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0); - private static final LoggedTunableNumber intakeDegrees = - new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); @RequiredArgsConstructor public enum Goal { - FLOOR_INTAKE(intakeDegrees), - STATION_INTAKE(stationIntakeDegrees), + FLOOR_INTAKE(new LoggedTunableNumber("Arm/IntakeDegrees", 18.0)), + STATION_INTAKE(new LoggedTunableNumber("Arm/StationIntakeDegrees", 45.0)), AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()), - STOW(stowDegrees), + STOW(new LoggedTunableNumber("Arm/StowDegrees", 10.0)), + AMP(new LoggedTunableNumber("Arm/AmpDegrees", 100.0)), CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0)); private final DoubleSupplier armSetpointSupplier;