From bf86433a9bd6b00396b543cb4cd021ba96fdd0f9 Mon Sep 17 00:00:00 2001 From: David Chen Date: Mon, 8 Apr 2024 20:46:06 -0400 Subject: [PATCH 1/7] megatag2 setup --- src/main/java/frc/robot/BuildConstants.java | 12 +- .../frc/robot/subsystems/drive/Drive.java | 34 ++++- .../java/frc/robot/util/LimelightHelpers.java | 143 +++++++++++++++++- 3 files changed, 179 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 11c1660..b0ec408 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2024RobotCode"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 230; - public static final String GIT_SHA = "07a5e92dce913b6541180162f8cf3cb04c5c398f"; - public static final String GIT_DATE = "2024-04-06 00:00:08 EDT"; - public static final String GIT_BRANCH = "msc"; - public static final String BUILD_DATE = "2024-04-06 11:05:30 EDT"; - public static final long BUILD_UNIX_TIME = 1712415930608L; + public static final int GIT_REVISION = 231; + public static final String GIT_SHA = "5b58b584f2d149355cf085b0220bf1286721ac91"; + public static final String GIT_DATE = "2024-04-07 20:06:05 EDT"; + public static final String GIT_BRANCH = "megatag2"; + public static final String BUILD_DATE = "2024-04-08 20:40:13 EDT"; + public static final long BUILD_UNIX_TIME = 1712623213819L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e550d2e..54e3f26 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -220,8 +220,17 @@ public void periodic() { // Apply odometry update poseEstimator.update(rawGyroRotation, modulePositions); + LimelightHelpers.SetRobotOrientation( + Constants.LL_ALIGN, + poseEstimator.getEstimatedPosition().getRotation().getDegrees(), + 0, + 0, + 0, + 0, + 0); if (DriverStation.getAlliance().isPresent() && LimelightHelpers.getTV(Constants.LL_ALIGN)) { - visionLogic(); + mt2TagFiltering(); + // visionLogic(); } Logger.recordOutput( @@ -270,6 +279,29 @@ private Pose2d posePicker(double time) { return robotPoseBuffer.getLast().pose; } + public void mt2TagFiltering() { + boolean doRejectUpdate = false; + + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.LL_ALIGN); + + if (Math.abs(gyroInputs.yawVelocityRadPerSec) > Math.toRadians(720)) { + doRejectUpdate = true; + } + + if (mt2.tagCount == 0) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + poseEstimator.setVisionMeasurementStdDevs( + VecBuilder.fill(0.7, 0.7, Units.degreesToRadians(9999999))); + poseEstimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds - (mt2.latency / 1000.)); + } + + Logger.recordOutput("Vision Measurement", mt2.pose); + } + public void visionLogic() { LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.LL_ALIGN); diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java index 97e9ae9..afa30a3 100644 --- a/src/main/java/frc/robot/util/LimelightHelpers.java +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -1,4 +1,4 @@ -// LimelightHelpers v1.3.1 (March 4, 2024) +// LimelightHelpers v1.5.0 (March 27, 2024) package frc.robot.util; @@ -371,6 +371,33 @@ public LimelightResults() { } } + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + public RawFiducial( + int id, + double txnc, + double tync, + double ta, + double distToCamera, + double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -379,6 +406,7 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; + public RawFiducial[] rawFiducials; public PoseEstimate( Pose2d pose, @@ -387,7 +415,9 @@ public PoseEstimate( int tagCount, double tagSpan, double avgTagDist, - double avgTagArea) { + double avgTagArea, + RawFiducial[] rawFiducials) { + this.pose = pose; this.timestampSeconds = timestampSeconds; this.latency = latency; @@ -395,6 +425,7 @@ public PoseEstimate( this.tagSpan = tagSpan; this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -451,7 +482,64 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr double tagArea = extractBotPoseEntry(poseArray, 10); // getlastchange() in microseconds, ll latency in milliseconds var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); - return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate( + pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } } public static NetworkTable getLimelightNTTable(String tableName) { @@ -663,6 +751,17 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the BLUE alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -686,6 +785,17 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -764,6 +874,33 @@ public static void setCropWindow( setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace( String limelightName, double forward, From ce0fafa8b5e21ab2d2cc957b488946802e6d3e0d Mon Sep 17 00:00:00 2001 From: David Chen Date: Tue, 9 Apr 2024 18:31:02 -0400 Subject: [PATCH 2/7] note alignment --- src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/commands/AlignToNoteTele.java | 9 +++ .../frc/robot/commands/PivotIntakeTele.java | 2 - .../frc/robot/subsystems/drive/Drive.java | 79 ++++++++++++++----- 5 files changed, 75 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index b0ec408..f837973 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2024RobotCode"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 231; - public static final String GIT_SHA = "5b58b584f2d149355cf085b0220bf1286721ac91"; - public static final String GIT_DATE = "2024-04-07 20:06:05 EDT"; + public static final int GIT_REVISION = 232; + public static final String GIT_SHA = "bf86433a9bd6b00396b543cb4cd021ba96fdd0f9"; + public static final String GIT_DATE = "2024-04-08 20:46:06 EDT"; public static final String GIT_BRANCH = "megatag2"; - public static final String BUILD_DATE = "2024-04-08 20:40:13 EDT"; - public static final long BUILD_UNIX_TIME = 1712623213819L; + public static final String BUILD_DATE = "2024-04-09 18:30:19 EDT"; + public static final long BUILD_UNIX_TIME = 1712701819167L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d68692f..acbbbe9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,7 +38,7 @@ public static Mode getMode() { }; } - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.SIM; public static final boolean tuningMode = true; public static final String CANBUS = "CAN Bus 2"; public static final double LOOP_PERIOD_SECS = 0.02; diff --git a/src/main/java/frc/robot/commands/AlignToNoteTele.java b/src/main/java/frc/robot/commands/AlignToNoteTele.java index fe4612f..8c6bb8d 100644 --- a/src/main/java/frc/robot/commands/AlignToNoteTele.java +++ b/src/main/java/frc/robot/commands/AlignToNoteTele.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.Constants.LED_STATE; @@ -23,6 +24,8 @@ public class AlignToNoteTele extends Command { Drive drive; Command pathCommand; + Translation2d noteTranslation2d; + public AlignToNoteTele(Intake intake, Pivot pivot, Shooter shooter, Drive drive, LED led) { this.intake = intake; this.pivot = pivot; @@ -36,6 +39,7 @@ public AlignToNoteTele(Intake intake, Pivot pivot, Shooter shooter, Drive drive, // Called when the command is initially scheduled. @Override public void initialize() { + noteTranslation2d = drive.getCachedNoteLocation(); this.pathCommand = drive.alignToNote(led); pathCommand.initialize(); intake.runRollers(12); @@ -47,6 +51,11 @@ public void initialize() { @Override public void execute() { pathCommand.execute(); + if (drive.getCachedNoteLocation().getDistance(noteTranslation2d) > 0.5) { + pathCommand.cancel(); + this.pathCommand = drive.alignToNote(led); + pathCommand.initialize(); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/PivotIntakeTele.java b/src/main/java/frc/robot/commands/PivotIntakeTele.java index dff3c81..9a739fe 100644 --- a/src/main/java/frc/robot/commands/PivotIntakeTele.java +++ b/src/main/java/frc/robot/commands/PivotIntakeTele.java @@ -30,8 +30,6 @@ public PivotIntakeTele(Pivot pivot, Intake intake, Shooter shooter, LED led, boo new IntakeNote(intake, shooter, led), new InstantCommand(() -> led.setState(LED_STATE.GREEN))); // new InstantCommand(() -> shooter.setFeedersRPM(150)), - // TODO:: adjust this delay - // new WaitCommand(0.7), // new InstantCommand(shooter::stopFeeders)); // new WaitCommand(2) // new InstantCommand(() -> led.setState(LED_STATE.BLUE)) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 54e3f26..a8f8261 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -591,21 +591,6 @@ public Optional turnToSpeakerAngle() { return Optional.empty(); } - private double calculateDistanceToSpeaker() { - double x = 0; - double y = 0; - - if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - x = FieldConstants.fieldLength - getPose().getX(); - y = FieldConstants.Speaker.speakerCenterY - getPose().getY(); - } else { - x = -getPose().getX(); - y = FieldConstants.Speaker.speakerCenterY - getPose().getY(); - } - - return Units.metersToFeet(Math.hypot(x, y)); - } - public PathPlannerPath generateTrajectory( Pose2d target, double maxVelMetersPerSec, @@ -661,7 +646,7 @@ public PathPlannerPath generatePathToNote() { new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); pointsToNote = PathPlannerPath.bezierFromPoses( - new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation()), + new Pose2d(getPose().getX(), getPose().getY(), targetRotation), new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); } else { Logger.recordOutput( @@ -672,7 +657,7 @@ public PathPlannerPath generatePathToNote() { new Pose2d( FieldConstants.fieldLength - getPose().getX(), getPose().getY(), - getPose().getRotation()), + targetRotation), new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); } PathPlannerPath path = @@ -707,6 +692,62 @@ public PathPlannerPath generatePathToNote() { new GoalEndState(0.5, getPose().getRotation(), true)); } } + + public Command noteAlignTest(LED led) { + + lastNoteLocT2d.translation = new Translation2d(5, 6); + lastNoteLocT2d.time = Timer.getFPGATimestamp(); + + Rotation2d targetRotation; + Logger.recordOutput("note timeess", getCachedNoteTime()); + if (getCachedNoteTime() != -1) { + led.setState(LED_STATE.FLASHING_RED); + Translation2d cachedNoteT2d = new Translation2d(5, 6); + Logger.recordOutput("better translate", cachedNoteT2d); + if (noteImageIsNew()) { + + targetRotation = + new Rotation2d( + cachedNoteT2d.getX() - getPose().getX(), cachedNoteT2d.getY() - getPose().getY()); + List pointsToNote; + if (DriverStation.getAlliance().get().equals(Alliance.Blue)) { + Logger.recordOutput( + "goal point blue", + new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); + pointsToNote = + PathPlannerPath.bezierFromPoses( + new Pose2d(getPose().getX(), getPose().getY(), targetRotation), + new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); + } else { + Logger.recordOutput( + "goal point red", + new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); + pointsToNote = + PathPlannerPath.bezierFromPoses( + new Pose2d( + FieldConstants.fieldLength - getPose().getX(), + getPose().getY(), + targetRotation), + new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); + } + PathPlannerPath path = + new PathPlannerPath( + pointsToNote, + new PathConstraints( + 2, 1.5, Units.degreesToRadians(100), Units.degreesToRadians(180)), + new GoalEndState(0.5, targetRotation, true)); + + path.preventFlipping = true; + Logger.recordOutput("follow path", true); + return AutoBuilder.followPath(path); + } else { + return new InstantCommand(() -> led.setState(LED_STATE.PAPAYA_ORANGE)); + } + } else { + return new InstantCommand(() -> led.setState(LED_STATE.WILLIAMS_BLUE)); + } + } + // } // } public Command alignToNote(LED led) { @@ -737,7 +778,7 @@ public Command alignToNote(LED led) { new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); pointsToNote = PathPlannerPath.bezierFromPoses( - new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation()), + new Pose2d(getPose().getX(), getPose().getY(), targetRotation), new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); } else { Logger.recordOutput( @@ -748,7 +789,7 @@ public Command alignToNote(LED led) { new Pose2d( FieldConstants.fieldLength - getPose().getX(), getPose().getY(), - getPose().getRotation()), + targetRotation), new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); } PathPlannerPath path = From 536ab7c456752d808429ee7c7856bbce222c43cd Mon Sep 17 00:00:00 2001 From: David Chen Date: Tue, 9 Apr 2024 19:42:50 -0400 Subject: [PATCH 3/7] intake safety nets (dont allow double tap) --- src/main/java/frc/robot/BuildConstants.java | 12 ++-- .../frc/robot/commands/AlignToNoteTele.java | 24 ++++++-- .../frc/robot/commands/DriveCommands.java | 6 ++ .../java/frc/robot/commands/IntakeNote.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 55 ------------------- 5 files changed, 35 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index f837973..9643a74 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2024RobotCode"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 232; - public static final String GIT_SHA = "bf86433a9bd6b00396b543cb4cd021ba96fdd0f9"; - public static final String GIT_DATE = "2024-04-08 20:46:06 EDT"; + public static final int GIT_REVISION = 233; + public static final String GIT_SHA = "ce0fafa8b5e21ab2d2cc957b488946802e6d3e0d"; + public static final String GIT_DATE = "2024-04-09 18:31:02 EDT"; public static final String GIT_BRANCH = "megatag2"; - public static final String BUILD_DATE = "2024-04-09 18:30:19 EDT"; - public static final long BUILD_UNIX_TIME = 1712701819167L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2024-04-09 18:38:18 EDT"; + public static final long BUILD_UNIX_TIME = 1712702298885L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/commands/AlignToNoteTele.java b/src/main/java/frc/robot/commands/AlignToNoteTele.java index 8c6bb8d..4d1dc29 100644 --- a/src/main/java/frc/robot/commands/AlignToNoteTele.java +++ b/src/main/java/frc/robot/commands/AlignToNoteTele.java @@ -4,7 +4,10 @@ package frc.robot.commands; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.Constants.LED_STATE; @@ -25,6 +28,7 @@ public class AlignToNoteTele extends Command { Command pathCommand; Translation2d noteTranslation2d; + boolean pathReplanned = false; public AlignToNoteTele(Intake intake, Pivot pivot, Shooter shooter, Drive drive, LED led) { this.intake = intake; @@ -50,12 +54,18 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if (shooter.seesNote()) { + end(true); + } pathCommand.execute(); - if (drive.getCachedNoteLocation().getDistance(noteTranslation2d) > 0.5) { - pathCommand.cancel(); - this.pathCommand = drive.alignToNote(led); - pathCommand.initialize(); + if (drive.getCachedNoteLocation().getDistance(noteTranslation2d) > Units.inchesToMeters(20)) { + pathReplanned = true; + replanPath(); + } else { + pathReplanned = false; } + + Logger.recordOutput("Path Replanned", pathReplanned); } // Called once the command ends or is interrupted. @@ -73,4 +83,10 @@ public boolean isFinished() { if (shooter.seesNote()) led.setState(LED_STATE.GREEN); return shooter.seesNote(); } + + public void replanPath() { + pathCommand.cancel(); + this.pathCommand = drive.alignToNote(led); + pathCommand.initialize(); + } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 2021474..2bb79a2 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -50,6 +50,12 @@ public static Command intakeCommand( DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { + if (shooter.seesNote()) { + return joystickDrive(drive, xSupplier, ySupplier, omegaSupplier); + } else { + + } + return new InstantCommand(() -> led.setState(LED_STATE.RED)) .andThen( new ParallelCommandGroup( diff --git a/src/main/java/frc/robot/commands/IntakeNote.java b/src/main/java/frc/robot/commands/IntakeNote.java index 347d0cc..98688c2 100644 --- a/src/main/java/frc/robot/commands/IntakeNote.java +++ b/src/main/java/frc/robot/commands/IntakeNote.java @@ -34,7 +34,9 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (shooter.seesNote()) end(true); + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a8f8261..1c5f82e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -693,61 +693,6 @@ public PathPlannerPath generatePathToNote() { } } - public Command noteAlignTest(LED led) { - - lastNoteLocT2d.translation = new Translation2d(5, 6); - lastNoteLocT2d.time = Timer.getFPGATimestamp(); - - Rotation2d targetRotation; - Logger.recordOutput("note timeess", getCachedNoteTime()); - if (getCachedNoteTime() != -1) { - led.setState(LED_STATE.FLASHING_RED); - Translation2d cachedNoteT2d = new Translation2d(5, 6); - Logger.recordOutput("better translate", cachedNoteT2d); - if (noteImageIsNew()) { - - targetRotation = - new Rotation2d( - cachedNoteT2d.getX() - getPose().getX(), cachedNoteT2d.getY() - getPose().getY()); - List pointsToNote; - if (DriverStation.getAlliance().get().equals(Alliance.Blue)) { - Logger.recordOutput( - "goal point blue", - new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); - pointsToNote = - PathPlannerPath.bezierFromPoses( - new Pose2d(getPose().getX(), getPose().getY(), targetRotation), - new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); - } else { - Logger.recordOutput( - "goal point red", - new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); - pointsToNote = - PathPlannerPath.bezierFromPoses( - new Pose2d( - FieldConstants.fieldLength - getPose().getX(), - getPose().getY(), - targetRotation), - new Pose2d(cachedNoteT2d.getX(), cachedNoteT2d.getY(), targetRotation)); - } - PathPlannerPath path = - new PathPlannerPath( - pointsToNote, - new PathConstraints( - 2, 1.5, Units.degreesToRadians(100), Units.degreesToRadians(180)), - new GoalEndState(0.5, targetRotation, true)); - - path.preventFlipping = true; - Logger.recordOutput("follow path", true); - return AutoBuilder.followPath(path); - } else { - return new InstantCommand(() -> led.setState(LED_STATE.PAPAYA_ORANGE)); - } - } else { - return new InstantCommand(() -> led.setState(LED_STATE.WILLIAMS_BLUE)); - } - } - // } // } public Command alignToNote(LED led) { From 989b504dd646560e9bfd5ed42ad6ca89c3bb813e Mon Sep 17 00:00:00 2001 From: David Chen Date: Tue, 9 Apr 2024 20:31:09 -0400 Subject: [PATCH 4/7] intake fix --- src/main/java/frc/robot/BuildConstants.java | 12 +++--- src/main/java/frc/robot/RobotContainer.java | 43 +++++++++++-------- .../frc/robot/commands/AlignToNoteTele.java | 3 +- .../frc/robot/commands/DriveCommands.java | 4 +- .../frc/robot/subsystems/Shooter/Shooter.java | 8 ++++ 5 files changed, 42 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 9643a74..cda1fb7 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2024RobotCode"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 233; - public static final String GIT_SHA = "ce0fafa8b5e21ab2d2cc957b488946802e6d3e0d"; - public static final String GIT_DATE = "2024-04-09 18:31:02 EDT"; + public static final int GIT_REVISION = 234; + public static final String GIT_SHA = "536ab7c456752d808429ee7c7856bbce222c43cd"; + public static final String GIT_DATE = "2024-04-09 19:42:50 EDT"; public static final String GIT_BRANCH = "megatag2"; - public static final String BUILD_DATE = "2024-04-09 18:38:18 EDT"; - public static final long BUILD_UNIX_TIME = 1712702298885L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2024-04-09 20:30:02 EDT"; + public static final long BUILD_UNIX_TIME = 1712709002159L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7af162..f191ee1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -125,6 +125,8 @@ private boolean isAimbot() { private Command goBackClimbCommands; + private final Command intakeReleaseCommands; + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { switch (Constants.getMode()) { @@ -212,6 +214,27 @@ public RobotContainer() { // Map.entry(true, new InstantCommand(() -> led.setState(LED_STATE.BLUE), led))), // this::isAutoAlign); + intakeReleaseCommands = + new SelectCommand<>( + Map.ofEntries( + Map.entry( + false, + new InstantCommand(() -> led.setState(LED_STATE.BLUE)) + .andThen(new InstantCommand(shooter::stopFeeders)) + .andThen( + new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))), + Map.entry( + true, + new InstantCommand(() -> led.setState(LED_STATE.BLUE)) + .andThen( + new InstantCommand(() -> shooter.setFeedersRPM(500)) + .andThen(new WaitCommand(0.15)) + .andThen(new InstantCommand(shooter::stopFeeders))) + .andThen( + new SetPivotTarget( + Constants.PivotConstants.STOW_SETPOINT_DEG, pivot)))), + shooter::inTimeThreshold); + shootCommands = new SelectCommand<>( Map.ofEntries( @@ -562,15 +585,7 @@ private void driverControls() { () -> -driveController.getLeftY(), () -> -driveController.getLeftX(), () -> -driveController.getRightX()))); - driveController - .leftBumper() - .onFalse( - new InstantCommand(() -> led.setState(LED_STATE.BLUE)) - .andThen( - new InstantCommand(() -> shooter.setFeedersRPM(500)) - .andThen(new WaitCommand(0.15)) - .andThen(new InstantCommand(shooter::stopFeeders))) - .andThen(new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))); + driveController.leftBumper().onFalse(intakeReleaseCommands); driveController.leftTrigger().whileTrue(new PivotIntakeTele(pivot, intake, shooter, led, true)); driveController @@ -599,15 +614,7 @@ private void driverControls() { new SequentialCommandGroup( new SetElevatorTarget(0, 1.5, elevator), new AlignToNoteTele(intake, pivot, shooter, drive, led))); - driveController - .rightBumper() - .onFalse( - new InstantCommand(() -> led.setState(LED_STATE.BLUE)) - .andThen( - new InstantCommand(() -> shooter.setFeedersRPM(500)) - .andThen(new WaitCommand(0.15)) - .andThen(new InstantCommand(shooter::stopFeeders))) - .andThen(new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))); + driveController.rightBumper().onFalse(intakeReleaseCommands); } private void manipControls() { diff --git a/src/main/java/frc/robot/commands/AlignToNoteTele.java b/src/main/java/frc/robot/commands/AlignToNoteTele.java index 4d1dc29..49faffb 100644 --- a/src/main/java/frc/robot/commands/AlignToNoteTele.java +++ b/src/main/java/frc/robot/commands/AlignToNoteTele.java @@ -4,8 +4,6 @@ package frc.robot.commands; -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -16,6 +14,7 @@ import frc.robot.subsystems.led.LED; import frc.robot.subsystems.pivot.Pivot; import frc.robot.subsystems.shooter.Shooter; +import org.littletonrobotics.junction.Logger; public class AlignToNoteTele extends Command { /** Creates a new AlignToNote. */ diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 2bb79a2..b55c6c6 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -51,9 +51,9 @@ public static Command intakeCommand( DoubleSupplier omegaSupplier) { if (shooter.seesNote()) { - return joystickDrive(drive, xSupplier, ySupplier, omegaSupplier); + return joystickDrive(drive, xSupplier, ySupplier, omegaSupplier); } else { - + } return new InstantCommand(() -> led.setState(LED_STATE.RED)) diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index 0cb6e1f..0656bb2 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.LoggedTunableNumber; @@ -19,6 +20,8 @@ public class Shooter extends SubsystemBase { private final FeederIO feeder; private DistanceSensorIO dist; + private double lastSeenNoteTime = 0; + private final FlywheelIOInputsAutoLogged flyInputs = new FlywheelIOInputsAutoLogged(); private final FeederIOInputsAutoLogged feedInputs = new FeederIOInputsAutoLogged(); private final DistanceSensorIOInputsAutoLogged sInputs = new DistanceSensorIOInputsAutoLogged(); @@ -148,11 +151,16 @@ public boolean atFeederSetpoint() { return Math.abs(getFeederError()) <= Constants.ShooterConstants.FEEDER_THRESHOLD; } + public boolean inTimeThreshold() { + return Math.abs(Timer.getFPGATimestamp() - lastSeenNoteTime) > 1; + } + public boolean seesNote() { Logger.recordOutput("i callewd sees note!", 63); if ((sInputs.distance > Constants.ShooterConstants.FEEDER_DIST && sInputs.distance < 2150) || feedInputs.currentAmps > 12.9) { Logger.recordOutput("i callewd sees note!", 37); + lastSeenNoteTime = Timer.getFPGATimestamp(); return true; } else { From 1d6037ce4b306a305f03b2433f27a6292f317767 Mon Sep 17 00:00:00 2001 From: David Chen Date: Wed, 10 Apr 2024 20:02:26 -0400 Subject: [PATCH 5/7] custom path following and conditional checkign for notes taken or not --- .../pathplanner/autos/New New Auto.auto | 31 ++++++++ .../pathplanner/autos/conditional auto.auto | 37 ++++++++++ .../deploy/pathplanner/paths/c5 check.path | 52 +++++++++++++ src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/RobotContainer.java | 25 ++++++- src/main/java/frc/robot/RobotMap.java | 8 +- .../frc/robot/subsystems/drive/Drive.java | 73 +++++++++++++++++++ 7 files changed, 223 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New New Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/conditional auto.auto create mode 100644 src/main/deploy/pathplanner/paths/c5 check.path diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto new file mode 100644 index 0000000..326e9d4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New New Auto.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 3.9355271535655185, + "y": 6.819606105579925 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "test path" + } + }, + { + "type": "named", + "data": { + "name": "test" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/conditional auto.auto b/src/main/deploy/pathplanner/autos/conditional auto.auto new file mode 100644 index 0000000..e6f8ab0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/conditional auto.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.548236363234494, + "y": 4.075371117931965 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "$s!p-c5" + } + }, + { + "type": "named", + "data": { + "name": "test" + } + }, + { + "type": "path", + "data": { + "pathName": "$s!score c5" + } + } + ] + } + }, + "folder": "limelight autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/c5 check.path b/src/main/deploy/pathplanner/paths/c5 check.path new file mode 100644 index 0000000..1605b81 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/c5 check.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.8, + "y": 0.91 + }, + "prevControl": null, + "nextControl": { + "x": 7.099172885934929, + "y": 1.5224087509515962 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.33338961685056, + "y": 1.885444683870822 + }, + "prevControl": { + "x": 7.169437905209619, + "y": 1.5341195874973776 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.5, + "rotation": 32.61924307119286, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index cda1fb7..b5a48c8 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2024RobotCode"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 234; - public static final String GIT_SHA = "536ab7c456752d808429ee7c7856bbce222c43cd"; - public static final String GIT_DATE = "2024-04-09 19:42:50 EDT"; + public static final int GIT_REVISION = 235; + public static final String GIT_SHA = "989b504dd646560e9bfd5ed42ad6ca89c3bb813e"; + public static final String GIT_DATE = "2024-04-09 20:31:09 EDT"; public static final String GIT_BRANCH = "megatag2"; - public static final String BUILD_DATE = "2024-04-09 20:30:02 EDT"; - public static final long BUILD_UNIX_TIME = 1712709002159L; + public static final String BUILD_DATE = "2024-04-10 19:55:24 EDT"; + public static final long BUILD_UNIX_TIME = 1712793324688L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f191ee1..aa5dd2c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,11 +17,13 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -401,6 +403,15 @@ CLIMB_STATES.SHOOT_NOTE, new InstantCommand(() -> shooter.setFeedersRPM(500))), NamedCommands.registerCommand( "DisableOverride", new InstantCommand(() -> drive.disableOverride())); + NamedCommands.registerCommand( + "test", + new ConditionalCommand( + new AlignToNoteAuto(led, drive, shooter, intake, pivot), + drive + .followPathCommand("c5 check", false) + .andThen(new AlignToNoteAuto(led, drive, shooter, intake, pivot)), + () -> drive.isNoteAt(new Translation2d(8.2, 0.77)))); + // Set up auto routines autos = new SendableChooser<>(); @@ -421,6 +432,10 @@ CLIMB_STATES.SHOOT_NOTE, new InstantCommand(() -> shooter.setFeedersRPM(500))), autos.addOption("New Auto", AutoBuilder.buildAuto("New Auto")); + autos.addOption("New New Auto", AutoBuilder.buildAuto("New New Auto")); + + autos.addOption("conditional auto", AutoBuilder.buildAuto("conditional auto")); + autoChooser = new LoggedDashboardChooser<>("Auto Choices", autos); configureButtonBindings(); @@ -433,10 +448,10 @@ CLIMB_STATES.SHOOT_NOTE, new InstantCommand(() -> shooter.setFeedersRPM(500))), * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - driverControls(); - manipControls(); + // driverControls(); + // manipControls(); - // testControls(); + testControls(); } private void testControls() { @@ -544,7 +559,9 @@ private void testControls() { driveController .rightTrigger() - .onTrue(new InstantCommand(() -> shooter.setFlywheelRPMs(4000, 4000))); + .onTrue( + new InstantCommand( + () -> shooter.setFlywheelRPMs(flywheelSpeed.get(), flywheelSpeed.get()))); driveController.rightTrigger().onFalse(new InstantCommand(() -> shooter.stopFlywheels())); driveController.leftTrigger().onTrue(new InstantCommand(() -> shooter.setFeedersRPM(1000))); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 1166116..4c1d186 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -20,10 +20,10 @@ public static class ElevatorIDs { public static class PivotIDs { public static final int GYRO = 10; - public static final int LEFT = 14; - public static final int RIGHT = 15; - // public static final int LEFT = 10000; - // public static final int RIGHT = 99999; + // public static final int LEFT = 14; + // public static final int RIGHT = 15; + public static final int LEFT = 10000; + public static final int RIGHT = 99999; } public static class LEDIDs { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 1c5f82e..f458774 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -16,6 +16,7 @@ import static edu.wpi.first.units.Units.*; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.FollowPathHolonomic; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathConstraints; @@ -764,4 +765,76 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) }; } + + public boolean isNoteAt(Translation2d coord) { + // return getCachedNoteLocation().getDistance(coord) < 1.5; + return false; + } + + public Command followPathCommand(String pathName, boolean lowerPID) { + PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); + if (lowerPID) { + return new FollowPathHolonomic( + path, + this::getPose, // Robot pose supplier + () -> + kinematics.toChassisSpeeds( + getModuleStates()), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live + // in your Constants class + new PIDConstants(5.0), // Translation PID constants + new PIDConstants(0.5), // Rotation PID constants + Constants.SwerveConstants.MAX_LINEAR_SPEED, // Max module speed, in m/s + DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options + // here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } else { + return new FollowPathHolonomic( + path, + this::getPose, // Robot pose supplier + () -> + kinematics.toChassisSpeeds( + getModuleStates()), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live + // in your Constants class + new PIDConstants(5.0), // Translation PID constants + new PIDConstants(1.5), // Rotation PID constants + Constants.SwerveConstants.MAX_LINEAR_SPEED, // Max module speed, in m/s + DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options + // here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } + } } From 7cdddaf230242f7db2f888b2a60cad26f510a1c2 Mon Sep 17 00:00:00 2001 From: David Chen Date: Wed, 10 Apr 2024 20:05:43 -0400 Subject: [PATCH 6/7] checks note using fieldconstants --- src/main/java/frc/robot/RobotContainer.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aa5dd2c..96c8010 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,6 +84,8 @@ import frc.robot.subsystems.shooter.FlywheelIOSim; import frc.robot.subsystems.shooter.FlywheelIOTalonFX; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.util.FieldConstants; + import java.util.Map; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -410,7 +412,7 @@ CLIMB_STATES.SHOOT_NOTE, new InstantCommand(() -> shooter.setFeedersRPM(500))), drive .followPathCommand("c5 check", false) .andThen(new AlignToNoteAuto(led, drive, shooter, intake, pivot)), - () -> drive.isNoteAt(new Translation2d(8.2, 0.77)))); + () -> drive.isNoteAt(FieldConstants.StagingLocations.centerlineTranslations[4]))); // Set up auto routines autos = new SendableChooser<>(); From f2854b770ca4fc01eb56456e306757c90012878a Mon Sep 17 00:00:00 2001 From: David Chen Date: Wed, 10 Apr 2024 20:52:43 -0400 Subject: [PATCH 7/7] speed up! --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index acbbbe9..c9cbfcd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -121,6 +121,8 @@ public static class PivotConstants { public static final double CLIMB_SETPOINT_ONE_DEG = 50; public static final double CLIMB_SETPOINT_TWO_DEG = 95; + public static final double TRAP_SETPOINT_DEG = 75; + public static final double THRESHOLD = 1.5; public static final double[] PID = {0, 0, 0}; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f458774..ba5b866 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -60,7 +60,7 @@ import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - private static final double MAX_LINEAR_SPEED = Constants.SwerveConstants.MAX_LINEAR_SPEED * 0.8; + private static final double MAX_LINEAR_SPEED = Constants.SwerveConstants.MAX_LINEAR_SPEED * 0.86; private static final double TRACK_WIDTH_X = Constants.SwerveConstants.TRACK_WIDTH_X; private static final double TRACK_WIDTH_Y = Constants.SwerveConstants.TRACK_WIDTH_Y; private static final double DRIVE_BASE_RADIUS = Constants.SwerveConstants.DRIVE_BASE_RADIUS;