From b6a03465e5e40fa94044086925dba7017717cf86 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Mon, 21 Oct 2024 18:45:37 +0000 Subject: [PATCH 1/5] Add rudimentary tuning-amp mode --- src/main/java/frc/robot/RobotContainer.java | 42 +++++++++++++++++++ .../frc/robot/constants/ScoringConstants.java | 2 + 2 files changed, 44 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d81e291..182bdb9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -427,6 +428,7 @@ private void configureModes() { testModeChooser.addOption("Shooter Tuning", "tuning-shooter"); testModeChooser.addOption("Aimer Tuning", "tuning-aimer"); testModeChooser.addOption("Shot Tuning", "tuning-shot"); + testModeChooser.addOption("Amp Tuning", "tuning-amp"); SmartDashboard.putData("Test Mode Chooser", testModeChooser); } @@ -651,6 +653,46 @@ public void testInit() { 0))) .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0))); break; + case "tuning-amp": + SmartDashboard.putNumber( + "Test-Mode/amp/aimerSetpointPosition", + ScoringConstants.ampAimerAngleRotations); + + // Let us drive + CommandScheduler.getInstance().cancelAll(); + setUpDriveWithJoysticks(); + + // Reset bindings + masher = new CommandXboxController(2); + + // Let us intake + masher.b() + .onTrue(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.INTAKE))) + .onFalse(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.NONE))); + + // Let us reverse intake (in case a note gets jammed during amp tuning) + masher.a() + .onTrue(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.REVERSE))) + .onFalse(new InstantCommand(() -> intakeSubsystem.run(IntakeAction.NONE))); + + masher.rightTrigger() + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.runToPosition( + SmartDashboard.getNumber( + "Test-Mode/amp/aimerSetpointPosition", + 0.0), + 0))) + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.setAction( + ScoringAction.TEMPORARY_SETPOINT))) + .onFalse( + new InstantCommand( + () -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + break; } } diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index a8cbe69..77ea855 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -61,6 +61,8 @@ public class ScoringConstants { public static final double aimAngleTolerance = 0.015; + public static final double ampAimerAngleRotations = 0.0; // TODO: Tune this value + public static final double maxAimIntake = 0.0; public static final double minAimIntake = 0.0; From f5e2fab27e65a393e9f440c7c8e7b11569edb826 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Mon, 21 Oct 2024 22:53:46 +0000 Subject: [PATCH 2/5] Add right bumper to manually shoot in tuning-amp mode --- src/main/java/frc/robot/RobotContainer.java | 12 ++++++++++++ .../robot/subsystems/scoring/ScoringSubsystem.java | 5 +++++ 2 files changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 182bdb9..64002ec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -692,6 +692,18 @@ public void testInit() { .onFalse( new InstantCommand( () -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + masher.rightBumper() + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.setOverrideKickerVoltsDirectly( + -12.0))) + .onFalse( + new InstantCommand( + () -> + scoringSubsystem.setOverrideKickerVoltsDirectly( + -12.0))); break; } } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 377e0ef..45c53e0 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -773,4 +773,9 @@ public boolean readyToShoot() { public void setAimerStatorCurrentLimit(double limit) { aimerIo.setStatorCurrentLimit(limit); } + + public void setOverrideKickerVoltsDirectly(double volts) { + /* Immediately sets kicker voltage, fully ignoring scoring state */ + shooterIo.setKickerVolts(volts); + } } From 0498db326a06a3c9ff83e7502bf88e760ffdd265 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 22 Oct 2024 14:50:09 +0000 Subject: [PATCH 3/5] Update amp aimer rotations constant --- .../java/frc/robot/constants/ScoringConstants.java | 2 +- .../robot/subsystems/scoring/ScoringSubsystem.java | 11 ++++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index 77ea855..f8400d3 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -61,7 +61,7 @@ public class ScoringConstants { public static final double aimAngleTolerance = 0.015; - public static final double ampAimerAngleRotations = 0.0; // TODO: Tune this value + public static final double ampAimerAngleRotations = 0.14; public static final double maxAimIntake = 0.0; public static final double minAimIntake = 0.0; diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 45c53e0..b852f3b 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -301,7 +301,9 @@ private void ampPrime() { // shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM); // TODO: Test this out aimerIo.setAimAngleRot( - 0.25); // This is actually in rotations and not radians, I really need to rename all + ScoringConstants + .ampAimerAngleRotations); // This is actually in rotations and not radians, + // I really need to rename all // of the aimer IO functions if (action != ScoringAction.SHOOT && action != ScoringAction.AMP_AIM) { state = ScoringState.IDLE; @@ -374,6 +376,13 @@ private void shoot() { private void ampShoot() { shooterIo.setKickerVolts(-12); // TODO: Test if this kicks note forward or backward + // Revert to amp prime after we shoot the note + // This will keep the arm up and allow the driver to drive away from the amp before the arm + // comes down + if (!hasNote() && shootTimer.get() > 2.0) { + state = ScoringState.AMP_PRIME; + } + // I see no reason why amp should not shoot within 1 second but if it takes longer than // that, we should probably let it From cc6a6eef00679e3967d659e4efbee22aa9d53b8e Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Wed, 23 Oct 2024 16:13:32 +0000 Subject: [PATCH 4/5] Remove outdated comments, fix kicker voltage in amp tuning mode --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../java/frc/robot/subsystems/scoring/ScoringSubsystem.java | 6 +----- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 64002ec..80eef34 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -698,12 +698,12 @@ public void testInit() { new InstantCommand( () -> scoringSubsystem.setOverrideKickerVoltsDirectly( - -12.0))) + 12.0))) .onFalse( new InstantCommand( () -> scoringSubsystem.setOverrideKickerVoltsDirectly( - -12.0))); + 0.0))); break; } } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index b852f3b..1103b19 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -300,11 +300,7 @@ private void prime() { private void ampPrime() { // shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM); // TODO: Test this out - aimerIo.setAimAngleRot( - ScoringConstants - .ampAimerAngleRotations); // This is actually in rotations and not radians, - // I really need to rename all - // of the aimer IO functions + aimerIo.setAimAngleRot(ScoringConstants.ampAimerAngleRotations); if (action != ScoringAction.SHOOT && action != ScoringAction.AMP_AIM) { state = ScoringState.IDLE; } else if (action == ScoringAction.SHOOT) { From 7a8027649460807ec2739bfe1037296f06edf5fd Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Wed, 23 Oct 2024 19:28:15 -0400 Subject: [PATCH 5/5] Update amp align angle, fix amp logic to be more seamlessTM --- src/main/java/frc/robot/commands/ShootWithGamepad.java | 3 ++- src/main/java/frc/robot/constants/FieldConstants.java | 2 +- .../java/frc/robot/subsystems/scoring/ScoringSubsystem.java | 3 ++- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShootWithGamepad.java b/src/main/java/frc/robot/commands/ShootWithGamepad.java index 55a83c6..a86681f 100644 --- a/src/main/java/frc/robot/commands/ShootWithGamepad.java +++ b/src/main/java/frc/robot/commands/ShootWithGamepad.java @@ -59,7 +59,8 @@ public void execute() { // ScoringAction.SHOOT will shoot when ready. // Therefore, when shoot is pressed, check if we've transitioned to aim and then // transition to shoot. - if (scoring.getCurrentAction() == ScoringAction.AIM) { + if (scoring.getCurrentAction() == ScoringAction.AIM + || scoring.getCurrentAction() == ScoringAction.AMP_AIM) { scoring.setAction(ScoringAction.SHOOT); } diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 6f76e38..cc25c6d 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -12,7 +12,7 @@ public final class FieldConstants { public static final double midfieldLowThresholdM = 5.87; public static final double midfieldHighThresholdM = 10.72; - public static final Rotation2d ampHeading = new Rotation2d(-Math.PI / 2); + public static final Rotation2d ampHeading = new Rotation2d(Math.PI / 2); public static final Rotation2d blueUpHeading = Rotation2d.fromRadians(0.0); public static final Rotation2d blueDownHeading = Rotation2d.fromRadians(Math.PI); diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 1103b19..0c18db9 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -371,11 +371,12 @@ private void shoot() { private void ampShoot() { shooterIo.setKickerVolts(-12); // TODO: Test if this kicks note forward or backward + aimerIo.setAimAngleRot(ScoringConstants.ampAimerAngleRotations); // Revert to amp prime after we shoot the note // This will keep the arm up and allow the driver to drive away from the amp before the arm // comes down - if (!hasNote() && shootTimer.get() > 2.0) { + if (action == ScoringAction.WAIT || (!hasNote() && shootTimer.get() > 1.0)) { state = ScoringState.AMP_PRIME; }