Dual Limelight odometry with FOM

build.gradle

@@ -69,6 +69,9 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher:1.10.1'
+
+     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
 }
 
 test {
@@ -99,3 +102,24 @@ wpi.java.configureTestTasks(test)
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
+
+repositories {
+    maven {
+        url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
+        credentials {
+            username = "Mechanical-Advantage-Bot"
+            password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
+        }
+    }
+    mavenLocal()
+}
+
+configurations.all {
+    exclude group: "edu.wpi.first.wpilibj"
+}
+
+task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.CheckInstall"
+    classpath = sourceSets.main.runtimeClasspath
+}
+compileJava.finalizedBy checkAkitInstall

src/main/java/frc/robot/Robot.java

@@ -4,7 +4,15 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -14,7 +22,7 @@
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
@@ -27,6 +35,23 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
+
+    Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
+
+if (isReal()) {
+    Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
+    Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+    new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+  } else {
+    setUseTiming(false); // Run as fast as possible
+    String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
+    Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+    Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+    Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+}
+
+// Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the "Understanding Data Flow" page
+Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
     m_robotContainer = new RobotContainer();
   }
 

src/main/java/frc/robot/helpers/MythicalMath.java

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.helpers;
+
+import java.security.PublicKey;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+
+/** Add your docs here. */
+public class MythicalMath {
+/**
+ * finds the absolute distance from the origin of any point on a 3d plane
+ * @param XfromOrigin x-coordinate of point
+ * @param YfromOrigin y-coordinate of point
+ * @param ZfromOrigin z-coordinate of point
+ * @return distance from origin of point
+ */
+    public static double DistanceFromOrigin3d(double XfromOrigin, double YfromOrigin, double ZfromOrigin) {
+        double Distance2d = Math.sqrt(Math.pow(YfromOrigin, 2)+Math.pow(XfromOrigin, 2));
+        return Math.sqrt(Math.pow(Distance2d, 2)+Math.pow(ZfromOrigin, 2));
+    }
+
+    public static Pose2d multiplyOnlyPos(Pose2d pose, Double scalar)
+    {
+        return new Pose2d(pose.getX()*scalar,pose.getY()*scalar,pose.getRotation());
+    }
+
+    public static Pose2d divideOnlyPos(Pose2d pose, Double scalar)
+    {
+        return new Pose2d(pose.getX()/scalar, pose.getY()/scalar,pose.getRotation());
+    }
+
+    /**
+     * 
+     * @param pose1
+     * @param pose2
+     * @return  poses added together, with pose1's rotation
+     */
+    public static Pose2d addOnlyPosTogether(Pose2d pose1, Pose2d pose2)
+    {
+     return new Pose2d(pose1.getX()+pose2.getX(), pose1.getY()+pose2.getY() ,pose1.getRotation());
+    }
+}

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

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import static frc.robot.settings.Constants.ShooterConstants.*;
@@ -38,8 +39,10 @@ public class AngleShooterSubsystem extends SubsystemBase {
 	public double desiredZeroOffset;
 	int runsValid;
 	double speakerDistGlobal;
-
+	Field2d offsetSpeakerLocationPose = new Field2d();
+
 	public AngleShooterSubsystem() {
+		SmartDashboard.putData("offsetSpeakerPose", offsetSpeakerLocationPose);
 
 		SmartDashboard.putNumber("CALLIBRATION/redShooterX", Field.CALCULATED_SHOOTER_RED_SPEAKER_X);
 		SmartDashboard.putNumber("CALLIBRATION/blueShooterX", Field.CALCULATED_SHOOTER_BLUE_SPEAKER_X);
@@ -177,8 +180,9 @@ public double calculateSpeakerAngle() {
 		double offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaX, 2) + Math.pow(offsetDeltaY, 2));
 		offsetSpeakerdist = offsetSpeakerdist+0.127; //to compensate for the pivot point of the shooter bieng offset from the center of the robot
 		SmartDashboard.putString("offset amount", targetOffset.toString());
-		SmartDashboard.putString("offset speaker location",
-		new Translation2d(offsetSpeakerX, offsetSpeakerY).toString());
+		Translation2d offsetSpeakerLocation = new Translation2d(offsetSpeakerX, offsetSpeakerY);
+		SmartDashboard.putString("offset speaker location", offsetSpeakerLocation.toString());
+		offsetSpeakerLocationPose.setRobotPose(new Pose2d(offsetSpeakerLocation, new Rotation2d(0)));
 		// getting desired robot angle
 		double totalOffsetDistToSpeaker = Math
 				.sqrt(Math.pow(offsetSpeakerdist, 2) + Math.pow(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT, 2));

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

@@ -19,27 +19,33 @@
 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;
 import static frc.robot.settings.Constants.ShooterConstants.OFFSET_MULTIPLIER;
 import static frc.robot.settings.Constants.Vision.APRILTAG_LIMELIGHT2_NAME;
 import static frc.robot.settings.Constants.Vision.APRILTAG_LIMELIGHT3_NAME;
+import static frc.robot.settings.Constants.Vision.OBJ_DETECTION_LIMELIGHT_NAME;
 
 import java.util.Arrays;
 import java.util.Collections;
 import java.util.Optional;
 import java.util.function.DoubleSupplier;
+//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;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -60,12 +66,17 @@
 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;
 import frc.robot.settings.Constants.Field;
 import frc.robot.settings.Constants.ShooterConstants;
 
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+
 public class DrivetrainSubsystem extends SubsystemBase {
 	public static final CTREConfigs ctreConfig = new CTREConfigs();
 	public SwerveDriveKinematics kinematics = DriveConstants.kinematics;
@@ -302,7 +313,7 @@ public void updateOdometry() {
 	 * larger pose shifts will take multiple calls to complete.
 	 */
 	public void updateOdometryWithVision() {
-		PoseEstimate estimate = limelight.getTrustedPose(getPose());
+		PoseEstimate estimate = limelight.getTrustedPose(getPose(), getLLFOM(APRILTAG_LIMELIGHT2_NAME), getLLFOM(APRILTAG_LIMELIGHT3_NAME));
 		if (estimate != null) {
 			boolean doRejectUpdate = false;
 			if(Math.abs(pigeon.getRate()) > 720) {
@@ -312,12 +323,15 @@ public void updateOdometryWithVision() {
 				doRejectUpdate = true;
 			}
 			if(!doRejectUpdate) {
-				odometer.addVisionMeasurement(estimate.pose, estimate.timestampSeconds);
+				Logger.recordOutput("Vision/MergesPose", estimate.pose);
+				odometer.addVisionMeasurement(new Pose2d(estimate.pose.getX(), estimate.pose.getY(), getGyroscopeRotation()), estimate.timestampSeconds);
 			}
 			RobotState.getInstance().LimelightsUpdated = true;
 		} else {
 			RobotState.getInstance().LimelightsUpdated = false;
 		}
+
+		limelight.updateLoggingWithPoses();
 	}
 	/**
 	 * Set the odometry using the current apriltag estimate, disregarding the pose trustworthyness.
@@ -580,5 +594,65 @@ public void periodic() {
 		for (int i = 0; i < 4; i++) {
 			motorLoggers[i].log(modules[i].getDriveMotor());
 		}
+
+		Logger.recordOutput("MyStates", getModuleStates());
+		Logger.recordOutput("Position", odometer.getEstimatedPosition());
+		Logger.recordOutput("Gyro", getGyroscopeRotation());
+
+		//Logger.recordOutput("Vision/targetposes/NotePoses/FieldSpace", robotToFieldCoordinates(LimelightHelpers.getTargetPose3d_RobotSpace(OBJ_DETECTION_LIMELIGHT_NAME)));
+		//liamsEstimates();
+
 	}
+
+	/**
+	 * 
+	 * @param robotCoordinates - put in the coordinates that have an origin of the robot
+	 * @return
+	 */
+	private Pose3d robotToFieldCoordinates(Pose3d robotCoordinates)
+	{
+		double robotCoordinatesX = robotCoordinates.getX();
+		double robotCoordinatesY = robotCoordinates.getY();
+		double robotCoordinatesZ = robotCoordinates.getZ();
+		Rotation3d robotCoordinatesAngle = robotCoordinates.getRotation();
+
+		double odometrPoseX = odometer.getEstimatedPosition().getX();
+		double odometrPosey = odometer.getEstimatedPosition().getY();
+
+
+		return new Pose3d(robotCoordinatesX+odometrPoseX,robotCoordinatesY+odometrPosey,robotCoordinatesZ, robotCoordinatesAngle);
+	}
+
+	public double getLLFOM(String limelightName) //larger fom is BAD, and is less trustworthy. 
+    {
+		//the value we place on each variable in the FOM. Higher value means it will get weighted more in the final FOM
+		/*These values should be tuned based on how heavily you want a contributer to be favored. Right now, we want the # of tags to be the most important 
+		 * with the distance from the tags also being immportant. and the tx and ty should only factor in a little bit, so they have the smallest number. Test this by making sure the two 
+		 * limelights give very different robot positions, and see where it decides to put the real robot pose.
+		*/
+		double distValue = 6;
+		double tagCountValue = 7;
+		double xyValue = 1;
+
+		//numTagsContributer is better when smaller, and is based off of how many april tags the Limelight identifies
+		double numTagsContributer;
+		if(limelight.getLLTagCount(limelightName) <= 0){
+			numTagsContributer = 0;
+		}else{
+			numTagsContributer = 1/limelight.getLLTagCount(limelightName);
+		}
+		//tx and ty contributers are based off where on the limelights screen the april tag is. Closer to the center means the contributer will bea smaller number, which is better.
+		double centeredTxContributer = Math.abs((limelight.getAprilValues(limelightName).tx))/29.8; //tx gets up to 29.8, the closer to 0 tx is, the closer to the center it is.
+		double centeredTyContributer = Math.abs((limelight.getAprilValues(limelightName).ty))/20.5; //ty gets up to 20.5 for LL2's and down. LL3's go to 24.85. The closer to 0 ty is, the closer to the center it is.
+		//the distance contributer gets smaller when the distance is closer, and is based off of how far away the closest tag is
+		double distanceContributer = (limelight.getClosestTagDist(limelightName)/5);
+
+		// calculates the final FOM by taking the contributors and multiplying them by their values, adding them all together and then dividing by the sum of the values.
+		double LLFOM = (
+			(distValue*distanceContributer)+(tagCountValue*numTagsContributer)+(centeredTxContributer*xyValue)+(centeredTyContributer)
+			)/distValue+tagCountValue+xyValue+xyValue;
+		Logger.recordOutput("Vision/LLFOM" + limelightName, LLFOM);
+		return LLFOM;
+    }
+
 }