end of day 1

roboblazers7617 · Apr 7, 2024 · aa43555 · aa43555
1 parent aa53363
commit aa43555
Show file tree

Hide file tree

Showing 8 changed files with 153 additions and 115 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Amp start Woofer.auto b/src/main/deploy/pathplanner/autos/Amp start Woofer.auto
diff --git a/src/main/deploy/pathplanner/autos/Default Path.auto b/src/main/deploy/pathplanner/autos/Default Path.auto
diff --git a/src/main/deploy/pathplanner/autos/Dont Move.auto b/src/main/deploy/pathplanner/autos/Dont Move.auto
diff --git a/...in/deploy/pathplanner/autos/New Auto.auto → ...hplanner/autos/Source Side disrupter.auto b/...in/deploy/pathplanner/autos/New Auto.auto → ...hplanner/autos/Source Side disrupter.auto
diff --git 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
@@ -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
@@ -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
@@ -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,16 +414,20 @@ 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;
             this.tagCount = tagCount;
             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;