diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 7ee105f..5291dc7 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/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; + } + } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index c63b1c2..4416710 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/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() &&