From aa435550bee806a69f578d96c52c73f81e4b1f96 Mon Sep 17 00:00:00 2001 From: roboblazers7617 <62038235+roboblazers7617@users.noreply.github.com> Date: Sat, 6 Apr 2024 22:09:39 -0400 Subject: [PATCH] end of day 1 --- .../pathplanner/autos/Amp start Woofer.auto | 49 ------- .../pathplanner/autos/Default Path.auto | 31 ---- .../deploy/pathplanner/autos/Dont Move.auto | 25 ---- ...w Auto.auto => Source Side disrupter.auto} | 0 src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/subsystems/Drivetrain.java | 21 ++- .../java/frc/robot/util/LimelightHelpers.java | 136 +++++++++++++++++- 8 files changed, 153 insertions(+), 115 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Amp start Woofer.auto delete mode 100644 src/main/deploy/pathplanner/autos/Default Path.auto delete mode 100644 src/main/deploy/pathplanner/autos/Dont Move.auto rename src/main/deploy/pathplanner/autos/{New Auto.auto => Source Side disrupter.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/Amp start Woofer.auto b/src/main/deploy/pathplanner/autos/Amp start Woofer.auto deleted file mode 100644 index 837ae57..0000000 --- a/src/main/deploy/pathplanner/autos/Amp start Woofer.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.66, - "y": 6.72 - }, - "rotation": -120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShootSpeaker" - } - }, - { - "type": "path", - "data": { - "pathName": "Woofer leave" - } - }, - { - "type": "named", - "data": { - "name": "turnTo0" - } - }, - { - "type": "named", - "data": { - "name": "Stow" - } - }, - { - "type": "named", - "data": { - "name": "StopShooter" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Default Path.auto b/src/main/deploy/pathplanner/autos/Default Path.auto deleted file mode 100644 index a70ccab..0000000 --- a/src/main/deploy/pathplanner/autos/Default Path.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.45, - "y": 7.0 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "turnToSpeaker" - } - }, - { - "type": "named", - "data": { - "name": "ShootSpeaker" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Dont Move.auto b/src/main/deploy/pathplanner/autos/Dont Move.auto deleted file mode 100644 index 88aafbd..0000000 --- a/src/main/deploy/pathplanner/autos/Dont Move.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.5053379907329615, - "y": 4.128036518181299 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "turnTo0" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Source Side disrupter.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Source Side disrupter.auto diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b108a7b..118feb4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -191,7 +191,6 @@ public static class SwerveConstants { public static final double TURN_TO_TAG_RANGE_FOR_END = 1.0; public static final double TURN_TO_ANGLE_RANGE_FOR_END = 2.0; public static final double FAST_TURN_TIME = 2.0; - public static final double MAX_DETECTION_RANGE = 3.2; } public static class IntakeConstants { @@ -244,6 +243,7 @@ public static class ClimberConstants { public static final int NUMBER_OF_MOTORS = 10; public static class VisionConstants { + public static final double MAX_DETECTION_RANGE = 5.5;//TODO 3.2 public static final Transform3d INTAKE_CAMERA_POSITION = new Transform3d(Units.inchesToMeters(10.0 + 5.0 / 8.0), -Units.inchesToMeters(-(11.0 + 3.0/6.0)), Units.inchesToMeters(9.0 + 1.0 / 4.0), new Rotation3d(0, Units.degreesToRadians(-45), 0)); public static final Transform3d SHOOTER_CAMERA_POSITION = new Transform3d(-Units.inchesToMeters(-(10.0 + 5.0 / 8.0)), Units.inchesToMeters(11.0 + 3.0/6.0), Units.inchesToMeters(9.0 + 1.0 / 4.0), new Rotation3d(0, Units.degreesToRadians(45), Units.degreesToRadians(180))); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f11e549..78ff612 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -108,7 +108,7 @@ public RobotContainer() { NamedCommands.registerCommand("TurnAndShoot", Commands.sequence(turnToSpeaker(),MechanismCommands.AutonomousShoot(arm, head, drivetrain))); NamedCommands.registerCommand("variableShoot", MechanismCommands.PrepareShoot(arm, head, ()->drivetrain.getDistanceToSpeaker()).andThen(MechanismCommands.Shoot(arm, head))); - autoChooser = AutoBuilder.buildAutoChooser("Default Path"); + autoChooser = AutoBuilder.buildAutoChooser("mid start 2 piece"); // Configure the trigger bindings configureDefaultCommands(); @@ -203,7 +203,7 @@ private void configureOperatorBindings(){ operatorControllerCommands.leftBumper().onTrue(MechanismCommands.Shoot(driverController, operatorController, arm, head)); operatorControllerCommands.rightTrigger().onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, drivetrain::getDistanceToSpeaker)) - .onFalse(MechanismCommands.PrepareShoot(operatorController, arm, head, drivetrain::getDistanceToSpeaker).andThen(MechanismCommands.Shoot(arm, head))); + .onFalse(MechanismCommands.PrepareShoot(operatorController, arm, head, drivetrain::getDistanceToSpeaker).andThen(MechanismCommands.Shoot(arm, head)).andThen(arm.Stow())); operatorControllerCommands.rightBumper().onTrue(MechanismCommands.PrepareShoot(operatorController, arm, head, ShootingPosition.SUBWOOFER)).onFalse(MechanismCommands.Shoot(driverController, operatorController, arm, head)); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 1ca5f73..b365769 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -36,6 +36,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.SwerveConstants; +import frc.robot.Constants.VisionConstants; import frc.robot.shuffleboard.MotorTab; import java.io.File; @@ -278,13 +279,29 @@ public void periodic() { public void simulationPeriodic() {} private void processVision() { - poseData = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); + //if(checkAllianceColors(Alliance.Blue)){ + LimelightHelpers.SetRobotOrientation("", getPose().getRotation().getDegrees(), 0, 0, 0, 0, 0); + //} + /*else{ + LimelightHelpers.SetRobotOrientation("", getPose().getRotation().plus(Rotation2d.fromDegrees(180)).getDegrees(), 0, 0, 0, 0, 0); + }*/ + + poseData = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(""); if (poseData.tagCount > 0 ) { - if( fieldLayout.getTagPose((int)LimelightHelpers.getFiducialID("")).orElseThrow().toPose2d().getTranslation().getDistance(getPose().getTranslation()) < SwerveConstants.MAX_DETECTION_RANGE){ + if( fieldLayout.getTagPose((int)LimelightHelpers.getFiducialID("")).orElseThrow().toPose2d().getTranslation(). + getDistance(getPose().getTranslation()) < VisionConstants.MAX_DETECTION_RANGE){ swerveDrive.addVisionMeasurement(poseData.pose,poseData.timestampSeconds, kalmanStdDevs); + } } } + + private boolean checkAllianceColors(Alliance checkAgainst) { + if (DriverStation.getAlliance().isPresent()) { + return DriverStation.getAlliance().get() == checkAgainst; + } + return false; + } /** * Get the swerve drive kinematics object. diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java index c2793f1..01c16c1 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.0 (Feb 24, 2024) +//LimelightHelpers v1.5.0 (March 27, 2024) package frc.robot.util; @@ -385,6 +385,27 @@ 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; @@ -393,9 +414,12 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; + public RawFiducial[] rawFiducials; + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea) { this.pose = pose; this.timestampSeconds = timestampSeconds; this.latency = latency; @@ -403,6 +427,7 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int ta this.tagSpan = tagSpan; this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -462,7 +487,65 @@ 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) { @@ -604,8 +687,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -675,6 +758,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) @@ -699,6 +793,16 @@ 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) @@ -782,6 +886,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c 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, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward;