Merge pull request #136 from 2491-NoMythic/liamsmergingFOMbranch

added LL1 FOM

src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java

@@ -19,6 +19,7 @@
 import static frc.robot.settings.Constants.DriveConstants.FR_DRIVE_MOTOR_ID;
 import static frc.robot.settings.Constants.DriveConstants.FR_STEER_ENCODER_ID;
 import static frc.robot.settings.Constants.DriveConstants.FR_STEER_MOTOR_ID;
+import static frc.robot.settings.Constants.DriveConstants.MAX_VELOCITY_METERS_PER_SECOND;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kD;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kI;
 import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kP;
@@ -34,6 +35,7 @@
 //import java.util.logging.Logger;
 
 import com.ctre.phoenix6.hardware.Pigeon2;
+import com.google.flatbuffers.DoubleVector;
 import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.math.Matrix;
@@ -64,6 +66,7 @@
 import frc.robot.commands.AngleShooter;
 import frc.robot.commands.RotateRobot;
 import frc.robot.helpers.MotorLogger;
+import frc.robot.helpers.MythicalMath;
 import frc.robot.settings.Constants;
 import frc.robot.settings.Constants.CTREConfigs;
 import frc.robot.settings.Constants.DriveConstants;
@@ -620,4 +623,17 @@ private Pose3d robotToFieldCoordinates(Pose3d robotCoordinates)
 		return new Pose3d(robotCoordinatesX+odometrPoseX,robotCoordinatesY+odometrPosey,robotCoordinatesZ, robotCoordinatesAngle);
 	}
 
+	public double getLL1FOM() //larger fom is BAD, and is less trustworthy. 
+    {
+		int maximumNumbOfTags = 2;
+		double maxTagDist = 10.0;
+
+		double VelocityContributer = (MythicalMath.DistanceFromOrigin3d(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond,0.0)/MAX_VELOCITY_METERS_PER_SECOND);
+		double centeredContributer = (limelight.getAprilValues(APRILTAG_LIMELIGHT2_NAME).tx); //I think this only gets up to 1???
+		double numTagsContributer = maximumNumbOfTags/limelight.getLLTagCount(APRILTAG_LIMELIGHT2_NAME);
+		double distanceContributer = (LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT2_NAME).avgTagDist/maxTagDist);
+
+		return VelocityContributer*centeredContributer*numTagsContributer*distanceContributer;
+    }
+
 }

src/main/java/frc/robot/subsystems/Limelight.java

@@ -126,6 +126,7 @@ public PoseEstimate mergedPose(PoseEstimate pose1, PoseEstimate pose2) {
 		double limelight2y = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getY();
 		double limelight2x = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getX();
 		double limelight2z = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getZ();
+
         // use those coordinates to find the distance to the closest apriltag for each limelight
         double distance1 = MythicalMath.DistanceFromOrigin3d(limelight1x, limelight1y, limelight1z);
         double distance2 = MythicalMath.DistanceFromOrigin3d(limelight2x, limelight2y, limelight2z);
@@ -136,6 +137,7 @@ public PoseEstimate mergedPose(PoseEstimate pose1, PoseEstimate pose2) {
         Pose2d scaledPose2 = MythicalMath.multiplyOnlyPos(pose2.pose, confidenceSource2);
     Pose2d newPose = MythicalMath.divideOnlyPos((MythicalMath.addOnlyPosTogether(scaledPose1, scaledPose2)), (confidenceSource1+confidenceSource2));
     //Pose2d newPose = scaledPose1.plus(new Transform2d(scaledPose2.getTranslation(), new Rotation2d())).div(confidenceSource1+confidenceSource2);
+
     pose1.pose = newPose;
     return pose1;
     }
@@ -210,6 +212,22 @@ public LimelightDetectorData getNeuralDetectorValues() {
                 LimelightHelpers.getTV(OBJ_DETECTION_LIMELIGHT_NAME));
     }
 
+      public LimelightDetectorData getAprilValues(String cameraname) {
+        return new LimelightDetectorData(
+                LimelightHelpers.getTX(cameraname),
+                LimelightHelpers.getTY(cameraname),
+                LimelightHelpers.getTA(cameraname),
+                LimelightHelpers.getNeuralClassID(cameraname),
+                LimelightHelpers.getTV(cameraname));
+    }
+
+    public int getLLTagCount(String cameraname)
+    {
+        return LimelightHelpers.getLatestResults(cameraname).targetingResults.targets_Fiducials.length;
+    }
+
+
+
     private boolean isValid(String limelightName, PoseEstimate estimate) {
         Boolean valid = (
                 estimate.pose.getX() < FIELD_CORNER.getX() &&