diff --git a/build.gradle b/build.gradle index 33d77dbc..44693ed8 100644 --- a/build.gradle +++ b/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 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1b3dcc27..f7c8c537 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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(); } diff --git a/src/main/java/frc/robot/helpers/MythicalMath.java b/src/main/java/frc/robot/helpers/MythicalMath.java new file mode 100644 index 00000000..bef6a66d --- /dev/null +++ b/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()); + } +} diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index 43396cf1..274e4e7d 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/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)); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index b28bdf9a..f576cb16 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -19,19 +19,23 @@ 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; @@ -39,7 +43,9 @@ 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; + } + } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index b2fa2adb..2faf8765 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -2,13 +2,30 @@ import frc.robot.LimelightHelpers; import frc.robot.LimelightHelpers.PoseEstimate; +import frc.robot.helpers.MythicalMath; import frc.robot.settings.LimelightDetectorData; +import static frc.robot.settings.Constants.DriveConstants.MAX_VELOCITY_METERS_PER_SECOND; import static frc.robot.settings.Constants.Vision.*; +import java.io.IOException; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + public class Limelight { @@ -19,6 +36,17 @@ public class Limelight { public static Boolean detectorEnabled = false; + public static final AprilTagFieldLayout FIELD_LAYOUT; + + static { + try { + FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + FIELD_LAYOUT.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + private Limelight() { SmartDashboard.putBoolean("Vision/Left/valid", false); SmartDashboard.putBoolean("Vision/Left/trusted", false); @@ -26,6 +54,18 @@ private Limelight() { SmartDashboard.putBoolean("Vision/Right/trusted", false); SmartDashboard.putData("Vision/Left/pose", field1); SmartDashboard.putData("Vision/Right/pose", field2); + + + //logs active selected path + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); } public static Limelight getInstance() { @@ -55,15 +95,19 @@ public static void useDetectorLimelight(boolean enabled) { * @return A valid and trustworthy pose. Null if no valid pose. Poses are * prioritized by lowest tagDistance. */ - public PoseEstimate getTrustedPose(Pose2d odometryPose) { + public PoseEstimate getTrustedPose(Pose2d odometryPose, double LL1FOM, double LL2FOM) { PoseEstimate pose1 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT2_NAME); PoseEstimate pose2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT3_NAME); Boolean pose1Trust = isTrustworthy(APRILTAG_LIMELIGHT2_NAME, pose1, odometryPose); Boolean pose2Trust = isTrustworthy(APRILTAG_LIMELIGHT3_NAME, pose2, odometryPose); + //if the limelight positions will be merged, let SmartDashboard know! + boolean mergingPoses = false; + if(pose1Trust&&pose2Trust) { mergingPoses = true;} + SmartDashboard.putBoolean("LL poses merged", mergingPoses); if (pose1Trust && pose2Trust) { - return ((pose1.avgTagDist < pose2.avgTagDist) ? pose1 : pose2); //select the limelight that has closer tags. + return (mergedPose(pose1, pose2, LL1FOM, LL2FOM)); //merge the two positions proportionally based on the closest tag distance } else if (pose1Trust) { return pose1; } else if (pose2Trust) { @@ -71,6 +115,71 @@ public PoseEstimate getTrustedPose(Pose2d odometryPose) { } else return null; } +/** + * returns a new limelight pose that has the gyroscope rotation of pose1, with the FOMs used to calculate a new pose that proportionally averages the two given positions + * @param pose1 + * @param pose2 + * @param LL1FOM + * @param LL2FOM + * @return + */ + public PoseEstimate mergedPose(PoseEstimate pose1, PoseEstimate pose2, double LL1FOM, double LL2FOM) { + double confidenceSource1 = 1/Math.pow(LL1FOM,2); + double confidenceSource2 = 1/Math.pow(LL2FOM,2); + Pose2d scaledPose1 = MythicalMath.multiplyOnlyPos(pose1.pose, confidenceSource1); + Pose2d scaledPose2 = MythicalMath.multiplyOnlyPos(pose2.pose, confidenceSource2); + Pose2d newPose = MythicalMath.divideOnlyPos((MythicalMath.addOnlyPosTogether(scaledPose1, scaledPose2)), (confidenceSource1+confidenceSource2)); + pose1.pose = newPose; + return pose1; + } +/** + * calculates the distance to the closest tag that is seen by the limelight + * @param limelightName + * @return + */ + public double getClosestTagDist(String limelightName) { + double limelight1y = LimelightHelpers.getTargetPose3d_CameraSpace(limelightName).getY(); + double limelight1x = LimelightHelpers.getTargetPose3d_CameraSpace(limelightName).getX(); + double limelight1z = LimelightHelpers.getTargetPose3d_CameraSpace(limelightName).getZ(); + // use those coordinates to find the distance to the closest apriltag for each limelight + double distance1 = MythicalMath.DistanceFromOrigin3d(limelight1x, limelight1y, limelight1z); + return distance1; + } + + + public void updateLoggingWithPoses() { + Pose2d pose1 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT2_NAME).pose; + Pose2d pose2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT3_NAME).pose; + + field1.setRobotPose(pose1); + field2.setRobotPose(pose2); + + Logger.recordOutput("LeftPose", pose1); + Logger.recordOutput("RightPose", pose2); + + Pose3d[] AprilTagPoses; + + for (int i = 0; i < 6; i++) + { + if(FIELD_LAYOUT.getTagPose(i) == null) + { + return; + } + + // AprilTagPoses[i] = FIELD_LAYOUT.getTagPose((int) LimelightHelpers.getLimelightNTTableEntry("botpose", APRILTAG_LIMELIGHT2_NAME).getInteger(i)); + } + + // Logger.recordOutput("AprilTagVision", ); + Logger.recordOutput("AprilTagVision", LimelightHelpers.getLimelightNTTableEntry("botpose", APRILTAG_LIMELIGHT2_NAME).getDoubleArray(new double[6])); + Logger.recordOutput("Vision/targetposes/LeftPose/CameraSpace", LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT2_NAME)); + Logger.recordOutput("Vision/targetposes/RightPose/CameraSpace", LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME)); + Logger.recordOutput("Vision/targetposes/NotePoses/CameraSpace", LimelightHelpers.getTargetPose3d_CameraSpace(OBJ_DETECTION_LIMELIGHT_NAME)); + + Logger.recordOutput("Vision/targetposes/LeftPose/RobotSpace", LimelightHelpers.getTargetPose3d_RobotSpace(APRILTAG_LIMELIGHT2_NAME)); + Logger.recordOutput("Vision/targetposes/LeftPose/RobotSpace", LimelightHelpers.getTargetPose3d_RobotSpace(APRILTAG_LIMELIGHT3_NAME)); + Logger.recordOutput("Vision/targetposes/NotePoses/RobotSpace", LimelightHelpers.getTargetPose3d_RobotSpace(OBJ_DETECTION_LIMELIGHT_NAME)); + + } /** * Gets the most recent limelight pose estimate, given that a valid estimate is @@ -108,6 +217,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.getBotPoseEstimate_wpiBlue(cameraname).tagCount; + } + + + private boolean isValid(String limelightName, PoseEstimate estimate) { Boolean valid = ( estimate.pose.getX() < FIELD_CORNER.getX() && @@ -138,7 +263,7 @@ private boolean isValid(String limelightName, PoseEstimate estimate) { * @param limelightName the name of the requested limelight, as seen on NetworkTables * @param estimate the poseEstimate from that limelight * @param odometryPose the robot's pose from the DriveTrain, unused right now - * @return + * @return true if the pose is within the field bounds and the tag distance is less than 7 */ private boolean isTrustworthy(String limelightName, PoseEstimate estimate, Pose2d odometryPose) { Boolean trusted = ( diff --git a/vendordeps - Copy/PathplannerLib.json b/vendordeps - Copy/PathplannerLib.json deleted file mode 100644 index 9e913435..00000000 --- a/vendordeps - Copy/PathplannerLib.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2023.4.4", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2023.4.4" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2023.4.4", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena" - ] - } - ] -} diff --git a/vendordeps - Copy/PhoenixProAnd5.json b/vendordeps - Copy/PhoenixProAnd5.json deleted file mode 100644 index 8f37c20a..00000000 --- a/vendordeps - Copy/PhoenixProAnd5.json +++ /dev/null @@ -1,458 +0,0 @@ -{ - "fileName": "PhoenixProAnd5.json", - "name": "CTRE-Phoenix (Pro And v5)", - "version": "23.0.11", - "frcYear": 2023, - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/PhoenixProAnd5-frc2023-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.4" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.30.4" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "wpiapi-java", - "version": "23.0.11" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "tools-sim", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonSRX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonFX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simVictorSPX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simPigeonIMU", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simCANCoder", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProTalonFX", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProCANcoder", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProPigeon2", - "version": "23.0.11", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "version": "23.0.11", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "tools-sim", - "version": "23.0.11", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonSRX", - "version": "23.0.11", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simTalonFX", - "version": "23.0.11", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simVictorSPX", - "version": "23.0.11", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simPigeonIMU", - "version": "23.0.11", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simCANCoder", - "version": "23.0.11", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProTalonFX", - "version": "23.0.11", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProCANcoder", - "version": "23.0.11", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "simProPigeon2", - "version": "23.0.11", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", - "artifactId": "wpiapi-cpp", - "version": "23.0.11", - "libName": "CTRE_PhoenixPro_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "23.0.11", - "libName": "CTRE_PhoenixPro_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps - Copy/REV2mDistanceSensor.json b/vendordeps - Copy/REV2mDistanceSensor.json deleted file mode 100644 index adcfa42a..00000000 --- a/vendordeps - Copy/REV2mDistanceSensor.json +++ /dev/null @@ -1,56 +0,0 @@ -{ - "fileName": "REV2mDistanceSensor.json", - "name": "REV2mDistanceSensor", - "version": "0.4.0", - "frcYear": "2024", - "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae", - "mavenUrls": [ - "https://www.revrobotics.com/content/sw/max/sdk/maven/" - ], - "jsonUrl": "https://www.revrobotics.com/content/sw/max/sdk/Rev2mDistanceSensor.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "DistanceSensor-java", - "version": "0.4.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "DistanceSensor-driver", - "version": "0.4.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "linuxathena" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "DistanceSensor-cpp", - "version": "0.4.0", - "libName": "libDistanceSensor", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "DistanceSensor-driver", - "version": "0.4.0", - "libName": "libDistanceSensorDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps - Copy/REVLib.json b/vendordeps - Copy/REVLib.json deleted file mode 100644 index f2d0b7df..00000000 --- a/vendordeps - Copy/REVLib.json +++ /dev/null @@ -1,73 +0,0 @@ -{ - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2023.1.3", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2023.1.3" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2023.1.3", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2023.1.3", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2023.1.3", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps - Copy/WPILibNewCommands.json b/vendordeps - Copy/WPILibNewCommands.json deleted file mode 100644 index 65dcc03c..00000000 --- a/vendordeps - Copy/WPILibNewCommands.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "fileName": "WPILibNewCommands.json", - "name": "WPILib-New-Commands", - "version": "1.0.0", - "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } - ] -} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 00000000..63dacbb5 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,42 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.2.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.2.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f4..f112e62d 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib2024.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib2024.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,