From 7c06d5265c29deb1980a09c0a44a80641295d0dc Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Wed, 23 Oct 2024 19:08:34 -0400 Subject: [PATCH 1/7] Pass (#60) * add guess of blue and red pass zone translations and add to align targets * move pov down to pass rather than endgame * add align to scoring * add pass states to scoring subsystem * add more constants * fix build error * Update ShootWithGamepad to make passing work, shooting less jank, and fix centering motor polarity --------- Co-authored-by: Jack Lingle --- src/main/java/frc/robot/RobotContainer.java | 8 +-- .../frc/robot/commands/ShootWithGamepad.java | 16 +++-- .../frc/robot/constants/FieldConstants.java | 6 ++ .../constants/PhoenixDriveConstants.java | 3 +- .../frc/robot/constants/ScoringConstants.java | 4 ++ .../robot/subsystems/drive/PhoenixDrive.java | 13 ++++ .../subsystems/intake/IntakeNEOVortex.java | 1 + .../subsystems/scoring/ScoringSubsystem.java | 69 ++++++++++++++++++- 8 files changed, 108 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48209b8..d81e291 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -375,7 +375,7 @@ private void configureBindings() { .onTrue(new InstantCommand(() -> drive.setAlignTarget(AlignTarget.SOURCE))); masher.povDown() - .onTrue(new InstantCommand(() -> drive.setAlignTarget(AlignTarget.ENDGAME))); + .onTrue(new InstantCommand(() -> drive.setAlignTarget(AlignTarget.PASS))); leftJoystick .trigger() @@ -404,7 +404,7 @@ private void configureBindings() { rightJoystick .povRight() - .onTrue(new InstantCommand(() -> drive.setAlignTarget(AlignTarget.ENDGAME))); + .onTrue(new InstantCommand(() -> drive.setAlignTarget(AlignTarget.PASS))); } } // spotless:on @@ -525,9 +525,7 @@ public void testInit() { () -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); masher.leftBumper() - .onTrue( - new InstantCommand( - () -> scoringSubsystem.setTuningKickerVolts(-12))) + .onTrue(new InstantCommand(() -> scoringSubsystem.setTuningKickerVolts(12))) .onFalse( new InstantCommand(() -> scoringSubsystem.setTuningKickerVolts(0))); break; diff --git a/src/main/java/frc/robot/commands/ShootWithGamepad.java b/src/main/java/frc/robot/commands/ShootWithGamepad.java index 9141197..55a83c6 100644 --- a/src/main/java/frc/robot/commands/ShootWithGamepad.java +++ b/src/main/java/frc/robot/commands/ShootWithGamepad.java @@ -54,14 +54,19 @@ public void execute() { if (driverShoot.getAsBoolean() || masherShoot.getAsBoolean()) { warmupSwitch(); + + // ScoringAction.AIM will never transition automatically to shoot + // 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) { + scoring.setAction(ScoringAction.SHOOT); + } + if (getDriveMode.get() != AlignTarget.SOURCE && getDriveMode.get() != AlignTarget.NONE) { boolean force = masherForceShoot.getAsBoolean(); scoring.setOverrideShoot(force); - - if (driverShoot.getAsBoolean() || masherShoot.getAsBoolean() || force) { - scoring.setAction(ScoringAction.SHOOT); - } } } else if (masherForceShoot.getAsBoolean()) { scoring.setAction(ScoringAction.SHOOT); @@ -90,6 +95,9 @@ public void warmupSwitch() { case ENDGAME: scoring.setAction(ScoringAction.ENDGAME); break; + case PASS: + scoring.setAction(ScoringAction.PASS); + break; default: scoring.setAction(ScoringAction.WAIT); break; diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 9e8cd6a..6f76e38 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -34,6 +34,12 @@ public final class FieldConstants { public static final Translation2d fieldToBlueSpeaker = new Translation2d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42)); + public static final Translation2d fieldToBluePass = + new Translation2d(Units.inchesToMeters(25), Units.inchesToMeters(230)); + + public static final Translation2d fieldToRedPass = + new Translation2d(Units.inchesToMeters(625), Units.inchesToMeters(230)); + public static final Pose2d robotAgainstBlueSpeaker = new Pose2d(1.39, 5.56, Rotation2d.fromDegrees(180)); diff --git a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java index c8f46a1..7d4bee9 100644 --- a/src/main/java/frc/robot/constants/PhoenixDriveConstants.java +++ b/src/main/java/frc/robot/constants/PhoenixDriveConstants.java @@ -20,7 +20,8 @@ public enum AlignTarget { UP, DOWN, LEFT, - RIGHT + RIGHT, + PASS } // Both sets of gains need to be tuned to your individual robot. diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index d8234a3..a8cbe69 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -84,6 +84,10 @@ public static HashMap getAimerMap() { return map; } + public static final double passLocationRot = -0.14; + public static final double passAngleToleranceRot = 0.005; + public static final double passShooterRpm = 3000.0; + public static final double aimerStaticOffset = 0.0; // NOTE - This should be monotonically increasing diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java index 35400e1..019928c 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java @@ -368,6 +368,19 @@ public Optional getAlignment() { } else { return Optional.of(FieldConstants.redRightHeading); } + case PASS: + if (!DriverStation.getAlliance().isEmpty() + && DriverStation.getAlliance().get() == Alliance.Blue) { + return Optional.of( + getTargetHeading( + new Pose2d(FieldConstants.fieldToBluePass, new Rotation2d()), + false)); + } else { + return Optional.of( + getTargetHeading( + new Pose2d(FieldConstants.fieldToRedPass, new Rotation2d()), + true)); + } default: // no pose to align to so set target to none this.setAlignTarget(AlignTarget.NONE); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java b/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java index 59506dc..8aa870b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeNEOVortex.java @@ -23,6 +23,7 @@ public IntakeNEOVortex() { intakeMotor.setInverted(true); centeringMotor.setSmartCurrentLimit(60); + centeringMotor.setInverted(true); } @Override diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 20bd46f..377e0ef 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -75,7 +75,9 @@ public enum ScoringState { ENDGAME, TUNING, OVERRIDE, - TEMPORARY_SETPOINT + TEMPORARY_SETPOINT, + PASS_PRIME, + PASS } public enum ScoringAction { @@ -90,7 +92,8 @@ public enum ScoringAction { TUNING, OVERRIDE, TEMPORARY_SETPOINT, - TRAP_SCORE + TRAP_SCORE, + PASS } private ScoringState state = ScoringState.IDLE; @@ -163,6 +166,8 @@ private void idle() { state = ScoringState.PRIME; // aimerIo.setAimAngleRad(aimerInputs.aimAngleRad + 0.001); shooterIo.setShooterVelocityRPM(2000); + } else if (action == ScoringAction.PASS) { + state = ScoringState.PASS_PRIME; } else if (action == ScoringAction.AMP_AIM) { state = ScoringState.AMP_PRIME; } else if (action == ScoringAction.ENDGAME || action == ScoringAction.TRAP_SCORE) { @@ -308,6 +313,47 @@ private void ampPrime() { } } + private void passPrime() { + aimerIo.setAimAngleRot(ScoringConstants.passLocationRot); + shooterIo.setShooterVelocityRPM(ScoringConstants.passShooterRpm); + + boolean notePresent = overrideBeamBreak ? true : hasNote(); + + boolean shooterReady = + shooterInputs.shooterLeftVelocityRPM + < (shooterOutputs.shooterLeftGoalVelocityRPM + + ScoringConstants.shooterUpperVelocityMarginRPM) + && shooterInputs.shooterLeftVelocityRPM + > (shooterOutputs.shooterLeftGoalVelocityRPM + - ScoringConstants.shooterLowerVelocityMarginRPM); + boolean aimReady = + Math.abs(aimerInputs.aimAngleRot - ScoringConstants.passLocationRot) + < ScoringConstants.passAngleToleranceRot + && Math.abs(aimerInputs.aimVelocityErrorRotPerSec) + < ScoringConstants.aimAngleVelocityMargin; + + boolean driveReady = driveAlignedSupplier.get(); + + boolean primeReady = shooterReady && aimReady && driveReady /*&& fieldLocationReady*/; + readyToShoot = primeReady && notePresent; + + if (action == ScoringAction.PASS && (readyToShoot || overrideShoot)) { + state = ScoringState.PASS; + + shootTimer.reset(); + shootTimer.start(); + } else if (action == ScoringAction.WAIT) { + state = ScoringState.IDLE; + } + + Logger.recordOutput("scoring/shooterReady", shooterReady); + Logger.recordOutput("scoring/aimReady", aimReady); + Logger.recordOutput("scoring/driverReady", driveReady); + Logger.recordOutput("scoring/notePresent", notePresent); + Logger.recordOutput("scoring/primeReady", primeReady); + Logger.recordOutput("scoring/readyToShoot", readyToShoot); + } + private void shoot() { double distancetoGoal = findDistanceToGoal(); @@ -339,6 +385,19 @@ private void ampShoot() { } */ } + private void passShoot() { + aimerIo.setAimAngleRot(ScoringConstants.passLocationRot); + shooterIo.setShooterVelocityRPM(ScoringConstants.passShooterRpm); + + shooterIo.setKickerVolts(12); + + if (shootTimer.get() > 0.5) { // TODO: Tune time + state = ScoringState.PASS_PRIME; + + shootTimer.stop(); + } + } + private void endgame() { aimerIo.setAimAngleRot(Math.PI / 2); // shooterIo.setShooterVelocityRPM(ScoringConstants.shooterAmpVelocityRPM); @@ -518,6 +577,12 @@ public void periodic() { case OVERRIDE: override(); break; + case PASS_PRIME: + passPrime(); + break; + case PASS: + passShoot(); + break; case TEMPORARY_SETPOINT: temporarySetpoint(); break; From ba0aceebf1dbe1593fb90b558c0a9626706d372d Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Wed, 23 Oct 2024 19:30:19 -0400 Subject: [PATCH 2/7] Seamless amp (#63) * Add rudimentary tuning-amp mode * Add right bumper to manually shoot in tuning-amp mode * Update amp aimer rotations constant * Remove outdated comments, fix kicker voltage in amp tuning mode * Update amp align angle, fix amp logic to be more seamlessTM --- src/main/java/frc/robot/RobotContainer.java | 54 +++++++++++++++++++ .../frc/robot/commands/ShootWithGamepad.java | 3 +- .../frc/robot/constants/FieldConstants.java | 2 +- .../frc/robot/constants/ScoringConstants.java | 2 + .../subsystems/scoring/ScoringSubsystem.java | 17 ++++-- 5 files changed, 73 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d81e291..80eef34 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,58 @@ 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))); + + masher.rightBumper() + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.setOverrideKickerVoltsDirectly( + 12.0))) + .onFalse( + new InstantCommand( + () -> + scoringSubsystem.setOverrideKickerVoltsDirectly( + 0.0))); + break; } } 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/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index a8cbe69..f8400d3 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.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 377e0ef..0c18db9 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -300,9 +300,7 @@ private void prime() { 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 - // 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) { @@ -373,6 +371,14 @@ 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 (action == ScoringAction.WAIT || (!hasNote() && shootTimer.get() > 1.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 @@ -773,4 +779,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 de170bbe4980898fd9864df0a6ab50c0194a65d7 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Thu, 24 Oct 2024 07:50:58 -0400 Subject: [PATCH 3/7] Ignore isDriveAligned when against subwoofer (#65) * Scoring believes drive is ready if drive is aligned OR if distance to goal is below a threshold * Make auto shoot preload and do other stuff decently --- .wpilib/wpilib_preferences.json | 2 +- .../pathplanner/autos/Amp Side - 2 Note.auto | 85 +++++++++++++++---- src/main/deploy/pathplanner/paths/C1-W1.path | 52 ++++++++++++ .../deploy/pathplanner/paths/W1-Wing.path | 52 ++++++++++++ .../deploy/pathplanner/paths/baseline-C1.path | 18 ++-- src/main/java/frc/robot/RobotContainer.java | 14 +-- .../frc/robot/constants/ScoringConstants.java | 6 +- .../robot/subsystems/drive/PhoenixDrive.java | 4 +- .../subsystems/scoring/AimerIORoboRio.java | 1 + .../subsystems/scoring/ScoringSubsystem.java | 4 +- 10 files changed, 199 insertions(+), 39 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/C1-W1.path create mode 100644 src/main/deploy/pathplanner/paths/W1-Wing.path diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 658c171..666f98a 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 9998 + "teamNumber": 401 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Amp Side - 2 Note.auto b/src/main/deploy/pathplanner/autos/Amp Side - 2 Note.auto index 8dd8fa2..fd5ad1f 100644 --- a/src/main/deploy/pathplanner/autos/Amp Side - 2 Note.auto +++ b/src/main/deploy/pathplanner/autos/Amp Side - 2 Note.auto @@ -1,6 +1,12 @@ { "version": 1.0, - "startingPose": null, + "startingPose": { + "position": { + "x": 0.7071301441350655, + "y": 6.711356821640298 + }, + "rotation": -118.17859010995917 + }, "command": { "type": "sequential", "data": { @@ -18,15 +24,9 @@ } }, { - "type": "named", - "data": { - "name": "waitForAlignment" - } - }, - { - "type": "named", + "type": "wait", "data": { - "name": "stopAlignToSpeaker" + "waitTime": 1.0 } }, { @@ -40,9 +40,28 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "intakeNote" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] } } ] @@ -51,25 +70,57 @@ { "type": "named", "data": { - "name": "alignToSpeaker" + "name": "shootNoteAtSpeaker" } }, { - "type": "named", + "type": "wait", "data": { - "name": "shootNoteAtSpeaker" + "waitTime": 1.0 } }, { - "type": "named", + "type": "parallel", "data": { - "name": "waitForAlignment" + "commands": [ + { + "type": "path", + "data": { + "pathName": "C1-W1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "W1-Wing" } }, { "type": "named", "data": { - "name": "stopAlignToSpeaker" + "name": "shootNoteAtSpeaker" } } ] diff --git a/src/main/deploy/pathplanner/paths/C1-W1.path b/src/main/deploy/pathplanner/paths/C1-W1.path new file mode 100644 index 0000000..8d5c049 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C1-W1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.909982492594682, + "y": 7.0050704681015805 + }, + "prevControl": null, + "nextControl": { + "x": 4.3932364072241565, + "y": 7.401583890824312 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.62271291626662, + "y": 7.445640937793504 + }, + "prevControl": { + "x": 7.62271291626662, + "y": 7.445640937793504 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.6360724683971, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -154.88516511385532, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/W1-Wing.path b/src/main/deploy/pathplanner/paths/W1-Wing.path new file mode 100644 index 0000000..7dcedce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/W1-Wing.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.593341551620492, + "y": 7.460326620116567 + }, + "prevControl": null, + "nextControl": { + "x": 6.302375109222491, + "y": 7.342841161532054 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.451979136516414, + "y": 6.241414987302248 + }, + "prevControl": { + "x": 5.700262133976863, + "y": 7.328155479208992 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.6360724683971, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/baseline-C1.path b/src/main/deploy/pathplanner/paths/baseline-C1.path index 3ba4581..91bd85d 100644 --- a/src/main/deploy/pathplanner/paths/baseline-C1.path +++ b/src/main/deploy/pathplanner/paths/baseline-C1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.8246156027195783, - "y": 7.019756150424644 + "x": 0.7071301441350655, + "y": 6.711356821640298 }, "prevControl": null, "nextControl": { - "x": 1.8246156027195841, - "y": 7.019756150424644 + "x": 1.707130144135071, + "y": 6.711356821640298 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.4694120229027585, - "y": 7.019756150424644 + "x": 2.8806111279485536, + "y": 6.990384785778517 }, "prevControl": { - "x": 1.4694120229027585, - "y": 7.019756150424644 + "x": 1.8806111279485536, + "y": 6.990384785778517 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -179.09061955080088, + "rotation": -119.74488129694224, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 80eef34..d2f1eca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,6 @@ 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; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; @@ -86,12 +85,13 @@ private void configureNamedCommands() { NamedCommands.registerCommand( "intakeNote", // intakes to start, ends by setting action to NONE when intake subsystem has note - new FunctionalCommand( - () -> intakeSubsystem.run(IntakeAction.INTAKE), - () -> {}, - interrupted -> intakeSubsystem.run(IntakeAction.NONE), - () -> scoringSubsystem.hasNote(), - intakeSubsystem)); + new InstantCommand( + () -> { + intakeSubsystem.run(IntakeAction.INTAKE); + scoringSubsystem.setAction(ScoringAction.INTAKE); + }, + intakeSubsystem, + scoringSubsystem)); NamedCommands.registerCommand( "waitForAlignment", diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index f8400d3..cdc4fda 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -4,8 +4,8 @@ import java.util.HashMap; public class ScoringConstants { - public static final double aimerkP = 85.0; - public static final double aimerkI = 0.0; // 5.0 + public static final double aimerkP = 100.0; + public static final double aimerkI = 2.0; // 5.0 public static final double aimerkD = 0.0; public static final double aimerkS = 0.3; // 0.25; @@ -52,6 +52,8 @@ public class ScoringConstants { public static final double aimAngleMarginRotations = Units.degreesToRotations(1); public static final double aimAngleVelocityMargin = 2.0; + public static final double minDistanceAlignmentNeeded = 1.3; // TODO: Tune this value + public static final double intakeAngleToleranceRotations = 0.05; // Todo: tune this value public static final double aimerAmpPositionRot = 0.14; diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java index 019928c..7741265 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java @@ -161,8 +161,8 @@ public void configurePathPlanner() { this.setGoalSpeeds(speeds, false); }, new HolonomicPathFollowerConfig( - new PIDConstants(1), - new PIDConstants(1, 0, 0), + new PIDConstants(0.1), + new PIDConstants(0.1, 0, 0), PhoenixDriveConstants.kSpeedAt12VoltsMps, driveBaseRadius, new ReplanningConfig(false, false)), diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index 0ffd730..9265234 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -76,6 +76,7 @@ public AimerIORoboRio() { talonFXConfigs.Feedback.SensorToMechanismRatio = ScoringConstants.aimerEncoderToMechanismRatio; talonFXConfigs.Feedback.RotorToSensorRatio = ScoringConstants.aimerRotorToSensorRatio; + talonFXConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; Slot0Configs slot0Configs = talonFXConfigs.Slot0; slot0Configs.GravityType = GravityTypeValue.Arm_Cosine; diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 0c18db9..3bf6919 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -250,7 +250,9 @@ private void prime() { < aimerAngleTolerance.getValue(distanceToGoal) && Math.abs(aimerInputs.aimVelocityErrorRotPerSec) < ScoringConstants.aimAngleVelocityMargin; - boolean driveReady = driveAlignedSupplier.get(); + boolean driveReady = + (driveAlignedSupplier.get() + || distanceToGoal < ScoringConstants.minDistanceAlignmentNeeded); boolean fieldLocationReady = true; if (!DriverStation.getAlliance().isPresent()) { From 3ed491ffdcd9d804778999e98628716da11da34d Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Sat, 26 Oct 2024 08:33:03 -0400 Subject: [PATCH 4/7] add led indicators for drivers (#72) * unfinished * Automated commit at 10/21/2024 19:01:33 * Set vision working supplier for LEDs * Hopefully change nothing but format --------- Co-authored-by: minhnguyenbhs Co-authored-by: avidraccoon --- src/main/java/frc/robot/RobotContainer.java | 20 +++ .../frc/robot/constants/FeatureFlags.java | 1 + .../frc/robot/constants/LEDConstants.java | 8 ++ .../frc/robot/constants/ScoringConstants.java | 2 +- src/main/java/frc/robot/subsystems/LED.java | 133 ++++++++++++++++++ 5 files changed, 163 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/constants/LEDConstants.java create mode 100644 src/main/java/frc/robot/subsystems/LED.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2f1eca..9a8b2cd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ import frc.robot.constants.PhoenixDriveConstants.AlignTarget; import frc.robot.constants.ScoringConstants; import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.LED; import frc.robot.subsystems.drive.PhoenixDrive; import frc.robot.subsystems.drive.PhoenixDrive.SysIdRoutineType; import frc.robot.subsystems.intake.IntakeIO; @@ -53,6 +54,7 @@ public class RobotContainer { ScoringSubsystem scoringSubsystem; IntakeSubsystem intakeSubsystem; + LED leds; CommandJoystick leftJoystick = new CommandJoystick(0); CommandJoystick rightJoystick = new CommandJoystick(1); @@ -115,6 +117,12 @@ private void configureSubsystems() { if (FeatureFlags.runIntake) { initIntake(); } + if (FeatureFlags.runScoring + && FeatureFlags.runIntake + && FeatureFlags.runVision + && FeatureFlags.runLEDS) { + initLEDs(); + } } private void initDrive() { @@ -206,6 +214,10 @@ private void initScoring() { } } + private void initLEDs() { + leds = new LED(scoringSubsystem, intakeSubsystem); + } + private void configureSuppliers() { if (FeatureFlags.runScoring) { Supplier poseSupplier; @@ -247,6 +259,14 @@ private void configureSuppliers() { tagVision.setCameraConsumer((m) -> {}); } } + + if (FeatureFlags.runLEDS) { + if (FeatureFlags.runVision) { + leds.setVisionWorkingSupplier(() -> tagVision.coprocessorConnected()); + } else { + leds.setVisionWorkingSupplier(() -> false); + } + } } private void configureBindings() { diff --git a/src/main/java/frc/robot/constants/FeatureFlags.java b/src/main/java/frc/robot/constants/FeatureFlags.java index 3e14c0c..6058022 100644 --- a/src/main/java/frc/robot/constants/FeatureFlags.java +++ b/src/main/java/frc/robot/constants/FeatureFlags.java @@ -8,4 +8,5 @@ public final class FeatureFlags { public static final boolean runLocalizer = false; public static final boolean runIntake = true; public static final boolean runScoring = true; + public static final boolean runLEDS = true; } diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java new file mode 100644 index 0000000..309b8af --- /dev/null +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -0,0 +1,8 @@ +package frc.robot.constants; + +public class LEDConstants { + + public static final int ledPort = 0; + + public static final int ledLength = 30; +} diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index cdc4fda..bad11d6 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -34,7 +34,7 @@ public class ScoringConstants { public static final double aimerCurrentLimit = 60; public static final int aimerEncoderId = 13; - ; + public static final double aimerEncoderOffset = -0.087402; // Armencoder is zeroed public static final double aimerEncoderToMechanismRatio = 1.0; diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java new file mode 100644 index 0000000..5d2d89b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LED.java @@ -0,0 +1,133 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.LEDConstants; +import frc.robot.constants.ModeConstants; +import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.scoring.ScoringSubsystem; +import java.util.function.Supplier; + +public class LED extends SubsystemBase { + + private final AddressableLED led; + private final AddressableLEDBuffer ledBuffer; + + private Timer timer; + + private boolean enabled = true; + + private final int ledcount = LEDConstants.ledLength; + private final int ledBrightness = 100; + + private int offset = 0; + private final int rainbowSpeed = 4; + private final int rainbowScale = 2; + + private ScoringSubsystem scoringSubsystem; + private IntakeSubsystem intakeSubsystem; + + private Supplier visionWorkingSupplier = () -> true; + + public LED(ScoringSubsystem scoringSubsystem, IntakeSubsystem intakeSubsystem) { + led = new AddressableLED(LEDConstants.ledPort); + ledBuffer = new AddressableLEDBuffer(ledcount); + led.setLength(ledBuffer.getLength()); + timer = new Timer(); + this.scoringSubsystem = scoringSubsystem; + this.intakeSubsystem = intakeSubsystem; + + led.setData(ledBuffer); + led.start(); + } + + public void setEnabled(boolean enabled) { + this.enabled = enabled; + } + + @Override + public void periodic() { + + clear(); + + if (!enabled) { + // LEDs left cleared if not enabled + } else if (DriverStation.isDisabled()) { + if (ModeConstants.currentMode == ModeConstants.Mode.REAL) { + rainbow(); + + if (!visionWorkingSupplier.get()) { + for (int i = 0; i < ledcount - 10; i++) { + ledBuffer.setRGB(i, 0, 0, 255 / 3); + } + } + } + + } else { + if ((scoringSubsystem != null && scoringSubsystem.hasNote()) + || (intakeSubsystem != null && intakeSubsystem.hasNote())) { + for (int i = 0; i < ledcount; i++) { + ledBuffer.setRGB(i, 245 / 3, 117 / 3, 66 / 3); + } + } + + if (DriverStation.getMatchTime() < 20 && DriverStation.getMatchTime() > 17) { + for (int i = 0; i < ledcount; i++) { + ledBuffer.setRGB(i, 255 / 3, 0, 0); + } + } + + if (!visionWorkingSupplier.get()) { + for (int i = 0; i < ledcount - 10; i++) { + ledBuffer.setRGB(i, 0, 0, 255 / 3); + } + } + } + + if (ModeConstants.currentMode == ModeConstants.Mode.REAL) { + led.setData(ledBuffer); + } + } + + public void setVisionWorkingSupplier(Supplier visionWorkingSupplier) { + this.visionWorkingSupplier = visionWorkingSupplier; + } + + private void clear() { + for (int i = 0; i < ledcount; i++) { + ledBuffer.setRGB(i, 0, 0, 0); + } + } + + private void setSolidColorSection(int lower, int upper, int[] rgbCode) { + for (int i = lower; i <= upper; i++) { + ledBuffer.setRGB(i, rgbCode[0], rgbCode[1], rgbCode[2]); + } + } + + private void setFlashingColorSection(int lower, int upper, int[] rgbCode1, int[] rgbCode2) { + if ((Math.floor(timer.get()) / 5.0) % 2 == 0) { + for (int i = lower; i <= upper; i++) { + ledBuffer.setRGB(i, rgbCode1[0], rgbCode1[1], rgbCode1[2]); + } + } else { + for (int i = lower; i <= upper; i++) { + ledBuffer.setRGB(i, rgbCode2[0], rgbCode2[1], rgbCode2[2]); + } + } + } + + private void rainbow() { + for (int i = 0; i < ledcount; i++) { + + int hue = (offset + i * rainbowScale) % 180; + ledBuffer.setHSV(i, hue, 255, ledBrightness); + } + + offset += rainbowSpeed; + offset %= 180; + } +} From 65e19930d059a5e567a87fee9c2be6698d359891 Mon Sep 17 00:00:00 2001 From: 401-minh <93294337+minhnguyenbhs@users.noreply.github.com> Date: Sat, 26 Oct 2024 08:45:08 -0400 Subject: [PATCH 5/7] Set aimer voltage to small negative when near the hard stop (#47) * is this what you were looking for? * sorry edited for A * was this what you were looking for? - added small negative voltage to aimer motor when goalAngleRad = minimumAngleRadians - added current limit changes * formatting * lockNegativeAtHome state applied to intake only * Update logic so that aimer locks when intaking and only when intaking --------- Co-authored-by: aidnem <99768676+aidnem@users.noreply.github.com> --- .../frc/robot/constants/ScoringConstants.java | 1 + .../frc/robot/subsystems/scoring/AimerIO.java | 2 ++ .../subsystems/scoring/AimerIORoboRio.java | 30 +++++++++++++++++-- .../robot/subsystems/scoring/AimerIOSim.java | 2 ++ .../subsystems/scoring/ScoringSubsystem.java | 8 +++-- 5 files changed, 38 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index bad11d6..66a9e7b 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -60,6 +60,7 @@ public class ScoringConstants { public static final double aimMaxAngleRotations = 0.361328 - 0.184570; public static final double aimMinAngleRotations = -0.217285; + public static final double aimLockVoltage = -0.5; public static final double aimAngleTolerance = 0.015; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java index 285f61b..e737deb 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java @@ -35,6 +35,8 @@ public default void setOverrideMode(boolean override) {} public default void setOverrideVolts(double volts) {} + public default void setNegativeHomeLockMode(boolean lock) {} + public default void setPID(double p, double i, double d) {} public default void resetPID() {} diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index 9265234..5acdb97 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -28,6 +28,7 @@ public class AimerIORoboRio implements AimerIO { private final TalonFX aimerMotor = new TalonFX(ScoringConstants.aimerMotorId); + MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0); private ControlRequest request; private final CANcoder aimerEncoder = new CANcoder(ScoringConstants.aimerEncoderId); @@ -53,6 +54,9 @@ public class AimerIORoboRio implements AimerIO { double controlSetpoint = 0.0; boolean motorDisabled = false; + boolean lockNegativeAtHome = false; + + TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); public AimerIORoboRio() { aimerMotor.setNeutralMode(NeutralModeValue.Brake); @@ -69,14 +73,14 @@ public AimerIORoboRio() { aimerEncoder.getConfigurator().apply(cancoderConfiguration); - TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); - talonFXConfigs.Feedback.FeedbackRemoteSensorID = aimerEncoder.getDeviceID(); talonFXConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; talonFXConfigs.Feedback.SensorToMechanismRatio = ScoringConstants.aimerEncoderToMechanismRatio; talonFXConfigs.Feedback.RotorToSensorRatio = ScoringConstants.aimerRotorToSensorRatio; talonFXConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; + talonFXConfigs.CurrentLimits.StatorCurrentLimitEnable = true; + talonFXConfigs.CurrentLimits.StatorCurrentLimit = ScoringConstants.aimerCurrentLimit; Slot0Configs slot0Configs = talonFXConfigs.Slot0; slot0Configs.GravityType = GravityTypeValue.Arm_Cosine; @@ -101,6 +105,7 @@ public void resetPID() {} @Override public void setAimAngleRot(double goalAngleRot) { + setNegativeHomeLockMode(false); this.goalAngleRot = goalAngleRot; } @@ -142,6 +147,11 @@ public void setOverrideVolts(double volts) { overrideVolts = volts; } + @Override + public void setNegativeHomeLockMode(boolean lock) { + lockNegativeAtHome = lock; + } + @Override public void setPID(double p, double i, double d) { Slot0Configs slot0Configs = new Slot0Configs(); @@ -235,6 +245,22 @@ public void applyOutputs(AimerOutputs outputs) { request = motionMagicVoltage; + if (lockNegativeAtHome + && getEncoderPosition() + < ScoringConstants.aimMinAngleRotations + + ScoringConstants.intakeAngleToleranceRotations + && !override) { + // talonFXConfigs.CurrentLimits.StatorCurrentLimit = + // ScoringConstants.aimerCurrentLimit / 5; + // aimerMotor.getConfigurator().apply(talonFXConfigs); + request = new VoltageOut(ScoringConstants.aimLockVoltage); + } else { + talonFXConfigs.CurrentLimits.StatorCurrentLimit = ScoringConstants.aimerCurrentLimit; + // aimerMotor.getConfigurator().apply(talonFXConfigs); + motionMagicVoltage.withPosition(goalAngleRot); + request = motionMagicVoltage; + } + controlSetpoint = aimerMotor.getClosedLoopReference().getValueAsDouble(); if (override) { appliedVolts = overrideVolts; diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java index b4f31c1..4e10338 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java @@ -98,6 +98,8 @@ public void setOverrideVolts(double volts) { appliedVolts = volts; } + public void setNegativeHomeLockMode(boolean lock) {} + @Override public void setPID(double p, double i, double d) { controller.setP(p); diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 3bf6919..6ef1a2f 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -58,6 +58,7 @@ public class ScoringSubsystem extends SubsystemBase implements Tunable { private boolean overrideShoot = false; private boolean overrideStageAvoidance = false; private boolean overrideBeamBreak = false; + private boolean lockNegativeAtHome = false; private Mechanism2d mechanism; private MechanismRoot2d rootMechanism; @@ -182,9 +183,8 @@ private void idle() { } private void intake() { - if (!aimerAtIntakePosition()) { - aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations); - } + aimerIo.setAimAngleRot(ScoringConstants.aimMinAngleRotations); + aimerIo.setNegativeHomeLockMode(true); if (!hasNote()) { shooterIo.setKickerVolts(ScoringConstants.kickerIntakeVolts); @@ -506,6 +506,8 @@ public void enabledInit() { @Override public void periodic() { + aimerIo.setNegativeHomeLockMode(false); // This should be false in all states but intake + // Intake will set it to true later in the loop. if (!SmartDashboard.containsKey("Aimer Offset")) { SmartDashboard.putNumber("Aimer Offset", ScoringConstants.aimerStaticOffset); From 887dcd8056bcc5876edc51dc8272dc2ddcb955a7 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Sat, 26 Oct 2024 15:23:43 -0400 Subject: [PATCH 6/7] Add OrchestraSubsystem to play music when robot is disabled (#68) * Add OrchestraSubsystem and required infrastructure * Add mii channel music and update robot container to only add motors from featureflag enabled subsystem. Also stop creating new TalonFX objects for drivetrain motors and get the TalonFXs from drive subsystem instead. --- src/main/deploy/music/mii_channel.chrp | Bin 0 -> 1560 bytes src/main/java/frc/robot/RobotContainer.java | 34 +++++ .../frc/robot/constants/FeatureFlags.java | 1 + .../robot/subsystems/OrchestraSubsystem.java | 125 ++++++++++++++++++ .../frc/robot/subsystems/scoring/AimerIO.java | 7 + .../subsystems/scoring/AimerIORoboRio.java | 10 ++ .../subsystems/scoring/ScoringSubsystem.java | 8 ++ .../robot/subsystems/scoring/ShooterIO.java | 7 + .../subsystems/scoring/ShooterIOTalonFX.java | 11 ++ 9 files changed, 203 insertions(+) create mode 100644 src/main/deploy/music/mii_channel.chrp create mode 100644 src/main/java/frc/robot/subsystems/OrchestraSubsystem.java diff --git a/src/main/deploy/music/mii_channel.chrp b/src/main/deploy/music/mii_channel.chrp new file mode 100644 index 0000000000000000000000000000000000000000..824de20738e86a5ee2a596e78074bd83be3d48c5 GIT binary patch literal 1560 zcmXYxdra147=|xUFe1ehfm0(85hV@3Kg1fi=FsLk%~^Iodt$qG-}`;{`@GM40tgCX6$66;*xx%M zUp_1t){7?jZk?c9n2!#Dt->;KU7%jr*n|gQmoT3c27840sdHwU! z$&dNiwxN(S%>DJpVQ#YfcIkrQ?r%vKYm2Ri;1RG|m=8^XwZeS(NGK60@-eB9INJAT zurx5n_t5m9u>DVc3YvwT%h+-1QJ8;rJbWuGll2TlrFoqh(zgHo3+eESurr!H0e%(c zUwaO23-bjT@VhX-Y$E(E3bj9PPlDj*VWa#SX_rjjlVa&Zlf8#BX^$-TRa2nvRPU#5 z8pKccv)d^hneAggmp+-J`=pxRy#$kTb$*)n2kDeq-or)d%-OyVSEMh#3?0hcm;>|Y zdj6*L_B_x0CM}o`AC~WOhSHeYMXS#9?yjKJl!4RLM05&Jy&n2+@Jx*~w!|~_(u7ja zd@M~W^L^ecO)iH;>Y=Fup4jN~wrqmd&Hi>w)86+U&PX#Vb&oaIIceqx{zfis(L1g3 zxvolcx572Oo%U+o{|~*N4r$&t&)ktN-R_ycq|0mkF5Q#9RqH;qPVYikorrp<5Y}h! z2B;Kv?tOMZwXn=1JE2yD%MaKE4Z_~<&`+RIw5jJ2yR{BsnNfS-sIbhKz0e}+l^Nd% zr$n*lny?Sfifs9;{qVD}%!~u7Cv0raLAWZc=Bz_-UD)?|pX!`NR_6cf|@+ z9UazMlvySVHV9`$N){Q|^yQQ@?h9Ogi?o&Y=0<`~Lybm_*9} literal 0 HcmV?d00001 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a8b2cd..9a7f8a0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.constants.ScoringConstants; import frc.robot.constants.VisionConstants; import frc.robot.subsystems.LED; +import frc.robot.subsystems.OrchestraSubsystem; import frc.robot.subsystems.drive.PhoenixDrive; import frc.robot.subsystems.drive.PhoenixDrive.SysIdRoutineType; import frc.robot.subsystems.intake.IntakeIO; @@ -56,6 +57,8 @@ public class RobotContainer { IntakeSubsystem intakeSubsystem; LED leds; + OrchestraSubsystem orchestraSubsystem; + CommandJoystick leftJoystick = new CommandJoystick(0); CommandJoystick rightJoystick = new CommandJoystick(1); CommandXboxController masher = new CommandXboxController(2); @@ -123,6 +126,9 @@ private void configureSubsystems() { && FeatureFlags.runLEDS) { initLEDs(); } + if (FeatureFlags.runOrchestra) { + initOrchestra(); + } } private void initDrive() { @@ -218,6 +224,34 @@ private void initLEDs() { leds = new LED(scoringSubsystem, intakeSubsystem); } + private void initOrchestra() { + switch (ModeConstants.currentMode) { + case REAL: + orchestraSubsystem = + new OrchestraSubsystem( + "music/mii_channel.chrp"); // TODO: Add music files to deploy! + if (FeatureFlags.runScoring) { + orchestraSubsystem.addInstruments( + scoringSubsystem.getAimerIO().getOrchestraMotors()); + orchestraSubsystem.addInstruments( + scoringSubsystem.getShooterIO().getOrchestraMotors()); + } + + if (FeatureFlags.runDrive) { + orchestraSubsystem.addInstrument(drive.getModule(0).getDriveMotor()); + orchestraSubsystem.addInstrument(drive.getModule(0).getSteerMotor()); + orchestraSubsystem.addInstrument(drive.getModule(1).getDriveMotor()); + orchestraSubsystem.addInstrument(drive.getModule(1).getSteerMotor()); + orchestraSubsystem.addInstrument(drive.getModule(2).getDriveMotor()); + orchestraSubsystem.addInstrument(drive.getModule(2).getSteerMotor()); + orchestraSubsystem.addInstrument(drive.getModule(3).getDriveMotor()); + orchestraSubsystem.addInstrument(drive.getModule(3).getSteerMotor()); + } + default: + break; + } + } + private void configureSuppliers() { if (FeatureFlags.runScoring) { Supplier poseSupplier; diff --git a/src/main/java/frc/robot/constants/FeatureFlags.java b/src/main/java/frc/robot/constants/FeatureFlags.java index 6058022..4cd1cda 100644 --- a/src/main/java/frc/robot/constants/FeatureFlags.java +++ b/src/main/java/frc/robot/constants/FeatureFlags.java @@ -9,4 +9,5 @@ public final class FeatureFlags { public static final boolean runIntake = true; public static final boolean runScoring = true; public static final boolean runLEDS = true; + public static final boolean runOrchestra = true; } diff --git a/src/main/java/frc/robot/subsystems/OrchestraSubsystem.java b/src/main/java/frc/robot/subsystems/OrchestraSubsystem.java new file mode 100644 index 0000000..376763f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/OrchestraSubsystem.java @@ -0,0 +1,125 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.Orchestra; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.AudioConfigs; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.List; + +public class OrchestraSubsystem extends SubsystemBase { + private Orchestra orchestra = new Orchestra(); + + private StatusCode status; + + enum PlayState { + PLAY, + PAUSE, + STOP, + } + + String defaultMusic; + String currentMusic; + + PlayState playState = PlayState.STOP; + + public OrchestraSubsystem(String defaultMusic) { + loadMusic(defaultMusic); + this.defaultMusic = defaultMusic; + + SmartDashboard.putBoolean("Orchestra/play", false); + SmartDashboard.putString("Orchestra/music", defaultMusic); + } + + AudioConfigs allowMusicDurDisableCfg = new AudioConfigs().withAllowMusicDurDisable(true); + + @Override + public void periodic() { + // If robot is enabled, don't waste processing power on any logic. + // We're only playing music when robot is disabled anyway + if (DriverStation.isEnabled()) { + stop(); + return; + } + + boolean play = SmartDashboard.getBoolean("Orchestra/play", false); + String music = SmartDashboard.getString("Orchestra/music", defaultMusic); + + if (music != currentMusic) { + loadMusic(music); + } + + if (play) { + play(); + } else { + pause(); + } + } + + private void logStatusIfNotOk(String whatAreWeDoing) { + if (status.isOK()) { + System.out.println( + "Orchestra Error While " + whatAreWeDoing + ": " + status.toString()); + } + } + + public void loadMusic(String musicFilename) { + stop(); + + status = orchestra.loadMusic(musicFilename); + logStatusIfNotOk("Loading Music File " + musicFilename); + + currentMusic = musicFilename; + } + + public void addInstrument(TalonFX motor) { + status = motor.getConfigurator().apply(allowMusicDurDisableCfg); + logStatusIfNotOk("Applying withAllowMusicDurDisable to a motor"); + orchestra.addInstrument(motor); + } + + public void addInstrumentById(int id) { + TalonFX motor = new TalonFX(id); + + addInstrument(motor); + } + + public void addInstruments(List motors) { + for (TalonFX motor : motors) { + addInstrument(motor); + } + } + + public void play() { + if (playState != PlayState.PLAY && status.isOK()) { + status = orchestra.play(); + logStatusIfNotOk("Playing"); + + playState = PlayState.PLAY; + } + } + + public void stop() { + if (playState != PlayState.STOP) { + status = orchestra.stop(); + logStatusIfNotOk("Stopping Playback"); + + playState = PlayState.STOP; + } + } + + public void pause() { + if (playState != PlayState.PAUSE) { + status = orchestra.stop(); + logStatusIfNotOk("Pausing"); + + playState = PlayState.PAUSE; + } + } + + public StatusCode getStatus() { + return status; + } +} diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java index e737deb..15415e0 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.scoring; +import com.ctre.phoenix6.hardware.TalonFX; +import java.util.ArrayList; +import java.util.List; import org.littletonrobotics.junction.AutoLog; public interface AimerIO { @@ -50,4 +53,8 @@ public default void setBrakeMode(boolean brake) {} public default void setStatorCurrentLimit(double limit) {} public default void setMotorDisabled(boolean disabled) {} + + public default List getOrchestraMotors() { + return new ArrayList(); + } } diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index 5acdb97..688b0f8 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -23,6 +23,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.ScoringConstants; +import java.util.ArrayList; +import java.util.List; import org.littletonrobotics.junction.Logger; public class AimerIORoboRio implements AimerIO { @@ -275,4 +277,12 @@ && getEncoderPosition() aimerMotor.setVoltage(0.0); } } + + @Override + public List getOrchestraMotors() { + ArrayList motors = new ArrayList(); + motors.add(aimerMotor); + + return motors; + } } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 6ef1a2f..25e3a72 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -788,4 +788,12 @@ public void setOverrideKickerVoltsDirectly(double volts) { /* Immediately sets kicker voltage, fully ignoring scoring state */ shooterIo.setKickerVolts(volts); } + + public ShooterIO getShooterIO() { + return shooterIo; + } + + public AimerIO getAimerIO() { + return aimerIo; + } } diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java index 2c559cb..c43d1d1 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.scoring; +import com.ctre.phoenix6.hardware.TalonFX; +import java.util.ArrayList; +import java.util.List; import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { @@ -49,4 +52,8 @@ public default void setMaxAcceleration(double maxAcceleration) {} public default void setMaxJerk(double maxJerk) {} public default void setFF(double kS, double kV, double kA) {} + + public default List getOrchestraMotors() { + return new ArrayList(); + } } diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java index 4495319..f7a2f0f 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalonFX.java @@ -11,6 +11,8 @@ import com.revrobotics.SparkLimitSwitch; import frc.robot.constants.ConversionConstants; import frc.robot.constants.ScoringConstants; +import java.util.ArrayList; +import java.util.List; public class ShooterIOTalonFX implements ShooterIO { private final CANSparkFlex kicker = @@ -155,4 +157,13 @@ public void applyOutputs(ShooterOutputs outputs) { kicker.setVoltage(outputs.kickerGoalVolts); } + + @Override + public List getOrchestraMotors() { + ArrayList motors = new ArrayList(); + motors.add(shooterLeft); + motors.add(shooterRight); + + return motors; + } } From 7fd85f6c61f4d88b44a56da17b83fed591baa2df Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Wed, 30 Oct 2024 19:25:09 -0400 Subject: [PATCH 7/7] Pass less hard+ Rumble Comp Changes (#75) * Fix amp side auto path * Increase pass shot rpm by 250 * Ignore isDriveAligned when against subwoofer (#65) * Scoring believes drive is ready if drive is aligned OR if distance to goal is below a threshold * Make auto shoot preload and do other stuff decently * add led indicators for drivers (#72) * unfinished * Automated commit at 10/21/2024 19:01:33 * Set vision working supplier for LEDs * Hopefully change nothing but format --------- Co-authored-by: minhnguyenbhs Co-authored-by: avidraccoon * Set aimer voltage to small negative when near the hard stop (#47) * is this what you were looking for? * sorry edited for A * was this what you were looking for? - added small negative voltage to aimer motor when goalAngleRad = minimumAngleRadians - added current limit changes * formatting * lockNegativeAtHome state applied to intake only * Update logic so that aimer locks when intaking and only when intaking --------- Co-authored-by: aidnem <99768676+aidnem@users.noreply.github.com> * Increase pass shot rpm by 250 * Update wing auto and add preload autos for every start positoin * Add arnit's auto * Fix alignment and other comp tuning * Tweak an auto, remove all cameras but the best one, make drive log position in advantagescope, making aimer motor coast * Remove vision because we caused java itself to segfault lol --------- Co-authored-by: minhnguyenbhs Co-authored-by: avidraccoon Co-authored-by: 401-minh <93294337+minhnguyenbhs@users.noreply.github.com> --- .../pathplanner/autos/Amp Side Preload.auto | 31 ++++ .../pathplanner/autos/Center - 4 Note.auto | 111 ++++++------ .../pathplanner/autos/Center Preload.auto | 31 ++++ .../autos/Source Side - 3 Note.auto | 167 ++++++++++++++++++ .../autos/Source Side Preload.auto | 31 ++++ src/main/deploy/pathplanner/paths/C1-W1.path | 8 +- src/main/deploy/pathplanner/paths/C2-C1.path | 16 +- src/main/deploy/pathplanner/paths/C3-C2.path | 16 +- .../deploy/pathplanner/paths/Wing-M2.path | 52 ++++++ .../pathplanner/paths/baseline - m1.path | 4 +- .../deploy/pathplanner/paths/baseline-C3.path | 8 +- .../deploy/pathplanner/paths/m1-Wing.path | 52 ++++++ .../deploy/pathplanner/paths/m2-Wing.path | 52 ++++++ src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/constants/FeatureFlags.java | 2 +- .../frc/robot/constants/ScoringConstants.java | 19 +- .../frc/robot/constants/VisionConstants.java | 88 ++++----- .../robot/subsystems/drive/PhoenixDrive.java | 19 +- .../subsystems/scoring/AimerIORoboRio.java | 4 +- 19 files changed, 567 insertions(+), 151 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Amp Side Preload.auto create mode 100644 src/main/deploy/pathplanner/autos/Center Preload.auto create mode 100644 src/main/deploy/pathplanner/autos/Source Side - 3 Note.auto create mode 100644 src/main/deploy/pathplanner/autos/Source Side Preload.auto create mode 100644 src/main/deploy/pathplanner/paths/Wing-M2.path create mode 100644 src/main/deploy/pathplanner/paths/m1-Wing.path create mode 100644 src/main/deploy/pathplanner/paths/m2-Wing.path diff --git a/src/main/deploy/pathplanner/autos/Amp Side Preload.auto b/src/main/deploy/pathplanner/autos/Amp Side Preload.auto new file mode 100644 index 0000000..339c7b5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Amp Side Preload.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7218158264581296, + "y": 6.711356821640298 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "alignToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "shootNoteAtSpeaker" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center - 4 Note.auto b/src/main/deploy/pathplanner/autos/Center - 4 Note.auto index 9495373..101dd9c 100644 --- a/src/main/deploy/pathplanner/autos/Center - 4 Note.auto +++ b/src/main/deploy/pathplanner/autos/Center - 4 Note.auto @@ -24,15 +24,9 @@ } }, { - "type": "named", - "data": { - "name": "waitForAlignment" - } - }, - { - "type": "named", + "type": "wait", "data": { - "name": "stopAlignToSpeaker" + "waitTime": 0.5 } }, { @@ -46,20 +40,27 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "intakeNote" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] } } ] } }, - { - "type": "named", - "data": { - "name": "alignToSpeaker" - } - }, { "type": "named", "data": { @@ -67,15 +68,9 @@ } }, { - "type": "named", - "data": { - "name": "waitForAlignment" - } - }, - { - "type": "named", + "type": "wait", "data": { - "name": "stopAlignToSpeaker" + "waitTime": 0.5 } }, { @@ -89,20 +84,27 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "intakeNote" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] } } ] } }, - { - "type": "named", - "data": { - "name": "alignToSpeaker" - } - }, { "type": "named", "data": { @@ -110,15 +112,9 @@ } }, { - "type": "named", - "data": { - "name": "waitForAlignment" - } - }, - { - "type": "named", + "type": "wait", "data": { - "name": "stopAlignToSpeaker" + "waitTime": 0.5 } }, { @@ -132,37 +128,32 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "intakeNote" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] } } ] } }, - { - "type": "named", - "data": { - "name": "alignToSpeaker" - } - }, { "type": "named", "data": { "name": "shootNoteAtSpeaker" } - }, - { - "type": "named", - "data": { - "name": "waitForAlignment" - } - }, - { - "type": "named", - "data": { - "name": "stopAlignToSpeaker" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Center Preload.auto b/src/main/deploy/pathplanner/autos/Center Preload.auto new file mode 100644 index 0000000..42e1e9d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Preload.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3533001663498863, + "y": 5.521816553472107 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "alignToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "shootNoteAtSpeaker" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source Side - 3 Note.auto b/src/main/deploy/pathplanner/autos/Source Side - 3 Note.auto new file mode 100644 index 0000000..de58d31 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source Side - 3 Note.auto @@ -0,0 +1,167 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6387107357548051, + "y": 4.413268821908015 + }, + "rotation": -63.78888841592257 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "alignToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "shootNoteAtSpeaker" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "baseline - m1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "stopAlignToSpeaker" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "m1-Wing" + } + }, + { + "type": "named", + "data": { + "name": "alignToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "shootNoteAtSpeaker" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Wing-M2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "stopAlignToSpeaker" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "intakeNote" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "m2-Wing" + } + }, + { + "type": "named", + "data": { + "name": "alignToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "shootNoteAtSpeaker" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source Side Preload.auto b/src/main/deploy/pathplanner/autos/Source Side Preload.auto new file mode 100644 index 0000000..b750ba2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source Side Preload.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7071301441350655, + "y": 4.361647649950041 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "alignToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "shootNoteAtSpeaker" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C1-W1.path b/src/main/deploy/pathplanner/paths/C1-W1.path index 8d5c049..abf97b0 100644 --- a/src/main/deploy/pathplanner/paths/C1-W1.path +++ b/src/main/deploy/pathplanner/paths/C1-W1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.62271291626662, - "y": 7.445640937793504 + "x": 8.725512692528069, + "y": 7.695297537285593 }, "prevControl": { - "x": 7.62271291626662, - "y": 7.445640937793504 + "x": 7.725512692528069, + "y": 7.695297537285593 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C2-C1.path b/src/main/deploy/pathplanner/paths/C2-C1.path index 13e36f9..0c72be4 100644 --- a/src/main/deploy/pathplanner/paths/C2-C1.path +++ b/src/main/deploy/pathplanner/paths/C2-C1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6162688461334, - "y": 5.580559282764361 + "x": 2.909982492594682, + "y": 5.551187918118234 }, "prevControl": null, "nextControl": { - "x": 1.323928801703758, - "y": 5.5658736004412965 + "x": 1.6176424481650398, + "y": 5.53650223579517 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6162688461334, - "y": 6.946327738809324 + "x": 3.0127822688561303, + "y": 7.0050704681015805 }, "prevControl": { - "x": 1.6162688461334, - "y": 6.946327738809324 + "x": 2.0127822688561303, + "y": 7.0050704681015805 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C3-C2.path b/src/main/deploy/pathplanner/paths/C3-C2.path index e3c3535..11989a8 100644 --- a/src/main/deploy/pathplanner/paths/C3-C2.path +++ b/src/main/deploy/pathplanner/paths/C3-C2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6162688461334, - "y": 4.067934003488759 + "x": 2.909982492594682, + "y": 4.082619685811823 }, "prevControl": null, "nextControl": { - "x": 1.323928801703758, - "y": 4.053248321165694 + "x": 1.6176424481650398, + "y": 4.067934003488758 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6162688461334, - "y": 5.551187918118234 + "x": 2.909982492594682, + "y": 5.53650223579517 }, "prevControl": { - "x": 1.6162688461334, - "y": 5.551187918118234 + "x": 1.9099824925946818, + "y": 5.53650223579517 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Wing-M2.path b/src/main/deploy/pathplanner/paths/Wing-M2.path new file mode 100644 index 0000000..ae13fd0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Wing-M2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.24, + "y": 2.63 + }, + "prevControl": null, + "nextControl": { + "x": 6.269003000968965, + "y": 1.5600276107959625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.65, + "y": 2.47 + }, + "prevControl": { + "x": 7.439614954044724, + "y": 1.6896043236530927 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 15.009958577160177, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 148.00740737423433, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/baseline - m1.path b/src/main/deploy/pathplanner/paths/baseline - m1.path index afd1f1b..0b96ad7 100644 --- a/src/main/deploy/pathplanner/paths/baseline - m1.path +++ b/src/main/deploy/pathplanner/paths/baseline - m1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.0004042543296356, - "y": 3.4687167965885233 + "x": 1.6910708597803608, + "y": 0.3377706934304757 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/baseline-C3.path b/src/main/deploy/pathplanner/paths/baseline-C3.path index 38082c5..9139819 100644 --- a/src/main/deploy/pathplanner/paths/baseline-C3.path +++ b/src/main/deploy/pathplanner/paths/baseline-C3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.528154752195015, - "y": 4.082619685811823 + "x": 2.8806111279485536, + "y": 4.097305368134887 }, "prevControl": { - "x": 1.528154752195015, - "y": 4.082619685811823 + "x": 1.8806111279485536, + "y": 4.097305368134887 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/m1-Wing.path b/src/main/deploy/pathplanner/paths/m1-Wing.path new file mode 100644 index 0000000..ba4a317 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/m1-Wing.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.65, + "y": 0.8 + }, + "prevControl": null, + "nextControl": { + "x": 7.025971740091629, + "y": 0.9499125109361337 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.24, + "y": 2.625502121374613 + }, + "prevControl": { + "x": 4.32988148994816, + "y": 1.9985812425328557 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.678076364598965, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.7746042511863881, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/m2-Wing.path b/src/main/deploy/pathplanner/paths/m2-Wing.path new file mode 100644 index 0000000..e062472 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/m2-Wing.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.65, + "y": 2.47 + }, + "prevControl": null, + "nextControl": { + "x": 7.068284438235544, + "y": 1.7654381743948677 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.24, + "y": 2.625502121374613 + }, + "prevControl": { + "x": 4.32988148994816, + "y": 1.9985812425328557 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.678076364598965, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -178.90553748421974, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a7f8a0..7a9a33f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -120,10 +120,7 @@ private void configureSubsystems() { if (FeatureFlags.runIntake) { initIntake(); } - if (FeatureFlags.runScoring - && FeatureFlags.runIntake - && FeatureFlags.runVision - && FeatureFlags.runLEDS) { + if (FeatureFlags.runScoring && FeatureFlags.runIntake && FeatureFlags.runLEDS) { initLEDs(); } if (FeatureFlags.runOrchestra) { @@ -296,7 +293,7 @@ private void configureSuppliers() { if (FeatureFlags.runLEDS) { if (FeatureFlags.runVision) { - leds.setVisionWorkingSupplier(() -> tagVision.coprocessorConnected()); + // leds.setVisionWorkingSupplier(() -> tagVision.coprocessorConnected()); } else { leds.setVisionWorkingSupplier(() -> false); } diff --git a/src/main/java/frc/robot/constants/FeatureFlags.java b/src/main/java/frc/robot/constants/FeatureFlags.java index 4cd1cda..57fcc6c 100644 --- a/src/main/java/frc/robot/constants/FeatureFlags.java +++ b/src/main/java/frc/robot/constants/FeatureFlags.java @@ -2,7 +2,7 @@ public final class FeatureFlags { public static final boolean runDrive = true; - public static final boolean runVision = true; + public static final boolean runVision = false; // NOTE: A featureflag "runLocalizer" was removed from here recently. // TODO: Figure out if we need this and add it back if necessary. public static final boolean runLocalizer = false; diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index 66a9e7b..1cfd1e4 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -56,10 +56,10 @@ public class ScoringConstants { public static final double intakeAngleToleranceRotations = 0.05; // Todo: tune this value - public static final double aimerAmpPositionRot = 0.14; + public static final double aimerAmpPositionRot = 0.145; public static final double aimMaxAngleRotations = 0.361328 - 0.184570; - public static final double aimMinAngleRotations = -0.217285; + public static final double aimMinAngleRotations = -0.212285; public static final double aimLockVoltage = -0.5; public static final double aimAngleTolerance = 0.015; @@ -79,19 +79,20 @@ public class ScoringConstants { // Value - Aimer angle in rotations public static HashMap getAimerMap() { HashMap map = new HashMap(); - map.put(1.284, -0.1); - map.put(1.7, -0.13); - map.put(2.30, -0.15); - map.put(3.32, -0.175); - map.put(4.23, -0.195); - map.put(5.811, -0.2); + // map.put(1.284, -0.1); + map.put(1.428, -0.1); + // map.put(1.7, -0.13); + // map.put(2.30, -0.15); + // map.put(3.32, -0.175); + // map.put(4.23, -0.195); + // map.put(5.811, -0.2); return map; } public static final double passLocationRot = -0.14; public static final double passAngleToleranceRot = 0.005; - public static final double passShooterRpm = 3000.0; + public static final double passShooterRpm = 2800.0; public static final double aimerStaticOffset = 0.0; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 4093a8a..56bac6f 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -4,9 +4,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; @@ -38,48 +36,50 @@ public final class VisionConstants { public static final Matrix driveUncertainty = VecBuilder.fill(0.1, 0.1, 0.1); - public static final List cameras = - List.of( - new CameraParams( - "Front-Left", // Front Right - 1280, - 960, - 50, - Rotation2d.fromDegrees(70), - new Transform3d( - new Translation3d(0.0328422, -0.3103626, 0.430911), - new Rotation3d(0, -0.261799, 0.0)), - CameraTrustZone.MIDDLE), - // new CameraParams( - // "Back-Right", // Back Right - // 1280, - // 960, - // 50, - // Rotation2d.fromDegrees(70), - // new Transform3d( - // new Translation3d(-0.3160014, -0.2327402, 0.3163316), - // new Rotation3d(0.0, -0.408546671, Math.PI)), - // CameraTrustZone.MIDDLE), - new CameraParams( - "Front-Right", // Front Left - 1280, - 960, - 50, - Rotation2d.fromDegrees(70), - new Transform3d( - new Translation3d(0.0328422, 0.3103626, 0.430911), - new Rotation3d(Math.PI, -0.261799, 0.0)), - CameraTrustZone.MIDDLE), - new CameraParams( - "Front-Center", // Back Left - 1280, - 960, - 50, - Rotation2d.fromDegrees(70), - new Transform3d( - new Translation3d(-0.3160014, 0.2327402, 0.3163316), - new Rotation3d(0.0, -0.408546671, Math.PI)), - CameraTrustZone.MIDDLE)); + public static final List cameras = List.of(); + + // new CameraParams( + // "Front-Left", // Front Right + // 1280, + // 960, + // 50, + // Rotation2d.fromDegrees(70), + // new Transform3d( + // new Translation3d(0.0328422, -0.3103626, 0.430911), + // new Rotation3d(0, -0.261799, 0.0)), + // CameraTrustZone.MIDDLE)); + + // new CameraParams( + // "Back-Right", // Back Right + // 1280, + // 960, + // 50, + // Rotation2d.fromDegrees(70), + // new Transform3d( + // new Translation3d(-0.3160014, -0.2327402, 0.3163316), + // new Rotation3d(0.0, -0.408546671, Math.PI)), + // CameraTrustZone.MIDDLE), + // new CameraParams( + // "Front-Right", // Front Left + // 1280, + // 960, + // 50, + // Rotation2d.fromDegrees(70), + // new Transform3d( + // new Translation3d(0.0328422, 0.3103626, 0.430911), + // new Rotation3d(Math.PI, -0.261799, 0.0)), + // CameraTrustZone.MIDDLE)); + + // new CameraParams( + // "Front-Center", // Back Left + // 1280, + // 960, + // 50, + // Rotation2d.fromDegrees(70), + // new Transform3d( + // new Translation3d(-0.3160014, 0.2327402, 0.3163316), + // new Rotation3d(0.0, -0.408546671, Math.PI)), + // CameraTrustZone.MIDDLE)); // new CameraParams( // "Front-Center", diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java index 7741265..bb7125e 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixDrive.java @@ -129,7 +129,7 @@ public PhoenixDrive( startSimThread(); } - thetaController.enableContinuousInput(0.0, 2 * Math.PI); + thetaController.enableContinuousInput(-Math.PI, Math.PI); thetaController.setTolerance(PhoenixDriveConstants.alignToleranceRadians); CommandScheduler.getInstance().registerSubsystem(this); @@ -143,6 +143,9 @@ public PhoenixDrive( startSimThread(); } + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(PhoenixDriveConstants.alignToleranceRadians); + CommandScheduler.getInstance().registerSubsystem(this); } @@ -169,9 +172,13 @@ public void configurePathPlanner() { () -> DriverStation.getAlliance().get() == Alliance.Red, this); - autoChooser.setDefaultOption("Default (nothing)", Commands.none()); + autoChooser.addOption("Nothing", Commands.none()); + autoChooser.setDefaultOption("Center Preload", new PathPlannerAuto("Center Preload")); + autoChooser.addOption("Amp Side Preload", new PathPlannerAuto("Amp Side Preload")); + autoChooser.addOption("Source Side Preload", new PathPlannerAuto("Source Side Preload")); autoChooser.addOption("Amp Side - 2 Note", new PathPlannerAuto("Amp Side - 2 Note")); autoChooser.addOption("Center - 4 Note", new PathPlannerAuto("Center - 4 Note")); + autoChooser.addOption("Source Side - 3 Note", new PathPlannerAuto("Source Side - 3 Note")); SmartDashboard.putData("Auto Chooser", autoChooser); } @@ -208,10 +215,12 @@ public void applyGoalSpeeds() { double omega = goalSpeeds.omegaRadiansPerSecond; if (aligning) { Rotation2d goalRotation = this.getAlignment().get(); + Logger.recordOutput("Drive/goalRotation", goalRotation); omega = thetaController.calculate( - this.getState().Pose.getRotation().getRadians() % (Math.PI * 2), - goalRotation.getRadians() % (Math.PI * 2)); + this.getState().Pose.getRotation().getRadians(), + goalRotation.getRadians()); + Logger.recordOutput("Drive/rotationError", thetaController.getPositionError()); } SwerveRequest request; @@ -390,6 +399,8 @@ public Optional getAlignment() { } public void logDrivetrainData() { + Logger.recordOutput("drive/pose", getState().Pose); + SwerveDriveState state = getState(); if (state.ModuleStates != null && state.ModuleTargets != null) { for (int i = 0; i < 4; i++) { diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java index 688b0f8..2119c19 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java @@ -61,7 +61,7 @@ public class AimerIORoboRio implements AimerIO { TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration(); public AimerIORoboRio() { - aimerMotor.setNeutralMode(NeutralModeValue.Brake); + aimerMotor.setNeutralMode(NeutralModeValue.Coast); setStatorCurrentLimit(ScoringConstants.aimerCurrentLimit); @@ -80,7 +80,7 @@ public AimerIORoboRio() { talonFXConfigs.Feedback.SensorToMechanismRatio = ScoringConstants.aimerEncoderToMechanismRatio; talonFXConfigs.Feedback.RotorToSensorRatio = ScoringConstants.aimerRotorToSensorRatio; - talonFXConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake; + talonFXConfigs.MotorOutput.NeutralMode = NeutralModeValue.Coast; talonFXConfigs.CurrentLimits.StatorCurrentLimitEnable = true; talonFXConfigs.CurrentLimits.StatorCurrentLimit = ScoringConstants.aimerCurrentLimit;