Skip to content

Commit

Permalink
Vision overhaul 2 electric boogaloo (#34)
Browse files Browse the repository at this point in the history 
* something like this

* apply spotless

* yay

* push name change

* Update src/main/java/frc/robot/extras/LimelightHelpers.java

jacks w change

Co-authored-by: Jack <85963782+JacksonElia@users.noreply.github.com>

* make stuff work with jacks w change

* apply spotless

* Update VisionSubsystem.java

* push

* push other fix

* idk what this is

* w changes

* yay it works im so happy

* Update VisionSubsystem.java

add some docstrings

* format code

---------

Co-authored-by: Jack <85963782+JacksonElia@users.noreply.github.com>
Co-authored-by: Ishan1522 <36376931+NNLUliskey@users.noreply.github.com>
  • Loading branch information
@Ishan1522 @JacksonElia @lukeful
3 people authored Oct 4, 2024
1 parent c01227b commit 1ce2f18
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 57 deletions.
7 changes: 0 additions & 7 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,3 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/extras/LimelightHelpers.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -551,6 +551,10 @@ private static void printPoseEstimate(PoseEstimate pose) {
}
}

public static Boolean isValidPoseEstimate(PoseEstimate pose) {
return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
}

public static NetworkTable getLimelightNTTable(String tableName) {
return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
}
Expand Down
103 changes: 53 additions & 50 deletions src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.FieldConstants;
Expand Down Expand Up @@ -65,29 +66,34 @@ && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS.
*
* @param limelightNumber the number of the limelight
*/
public void updateLimelightPoseEstimate(int limelightNumber) {
// this can probably be removed after testing
if (!canSeeAprilTags(limelightNumber)) {
limelightEstimates[limelightNumber] = new PoseEstimate();
}
// Soon to be implemented code:
// if (canSeeAprilTags(limelightNumber)) {
// if (isValidPoseEstimate(limelightNumber)) {

// if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber)
// && getLimelightAprilTagDistance(limelightNumber) <
// VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) {
// limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
// } else
if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) {
LimelightHelpers.SetRobotOrientation(
getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0);
limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber);
} else {
limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
public PoseEstimate enabledPoseUpdate(int limelightNumber) {
if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) {
if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber)
&& getLimelightAprilTagDistance(limelightNumber)
< VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) {
return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
} else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) {
LimelightHelpers.SetRobotOrientation(
getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0);
return limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber);
} else {
return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
}
}
// }
// }
return limelightEstimates[limelightNumber] = new PoseEstimate();
}

/**
* If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link
* #enabledPoseUpdate(int)}
*
* @param limelightNumber the number of the limelight
*/
public void updatePoseEstimate(int limelightNumber) {
limelightEstimates[limelightNumber] =
DriverStation.isEnabled()
? enabledPoseUpdate(limelightNumber)
: getMegaTag1PoseEstimate(limelightNumber);
}

/**
Expand Down Expand Up @@ -148,43 +154,36 @@ public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) {
}

/**
* Checks if the MegaTag1 and MegaTag2 pose estimates are within the field parameters
* Checks if the MT1 and MT2 pose estimate exists and whether it is within the field
*
* @param limelightNumber the number of the limelight
* @return true if the poses are within the field, false if not.
* @return true if the pose estimate exists within the field and the pose estimate is not null
*/
public boolean isValidPoseEstimate(int limelightNumber) {
return isMegaTag1Good(limelightNumber) && isMegaTag2Good(limelightNumber);
}

/**
* Checks if the MegaTag1 pose estimate is within the field parameters
*
* @param limelightNumber the number of the limelight
* @return true if the pose is within the field, false if not.
*/
public boolean isMegaTag1Good(int limelightNumber) {
PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber);
PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber);

return ((megaTag1Estimate.pose.getX() > 0
&& megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
&& (megaTag1Estimate.pose.getY() > 0
&& megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS));
return LimelightHelpers.isValidPoseEstimate(megaTag1Estimate)
&& LimelightHelpers.isValidPoseEstimate(megaTag2Estimate)
&& isWithinFieldBounds(megaTag1Estimate, megaTag2Estimate);
}

/**
* Checks if the MegaTag2 pose estimate is within the field parameters
* Checks whether the pose estimate for MT1 and MT2 is within the field
*
* @param limelightNumber the number of the limelight
* @return true if the pose is within the field, false if not.
* @param megaTag1Estimate the MT1 pose estimate to check
* @param megaTag2Estimate the MT2 pose estimate to check
*/
public boolean isMegaTag2Good(int limelightNumber) {
PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber);

return ((megaTag2Estimate.pose.getX() > 0
private boolean isWithinFieldBounds(
PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) {
return (megaTag1Estimate.pose.getX() > 0
&& megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
&& (megaTag1Estimate.pose.getY() > 0
&& megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)
&& (megaTag2Estimate.pose.getX() > 0
&& megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
&& (megaTag2Estimate.pose.getY() > 0
&& megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS));
&& megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS);
}

/**
Expand Down Expand Up @@ -294,7 +293,7 @@ public void checkAndUpdatePose(int limelightNumber) {
// because to get the timestamp of the reading, you need to parse the JSON dump which can be
// very demanding whereas this only has to get the Network Table entries for TX and TY.
if (current_TX != last_TX || current_TY != last_TY) {
updateLimelightPoseEstimate(limelightNumber);
updatePoseEstimate(limelightNumber);
limelightThreads.computeIfPresent(
limelightNumber, (key, value) -> new AtomicBoolean(true));
// This is to keep track of the last valid pose calculated by the limelights
Expand Down Expand Up @@ -344,9 +343,9 @@ public void visionThread(int limelightNumber) {
executorService.submit(
() -> {
try {
while (limelightThreads.get(limelightNumber).get()) {
checkAndUpdatePose(limelightNumber);
}
// while (limelightThreads.get(limelightNumber).get()) {
checkAndUpdatePose(limelightNumber);
// }
} catch (Exception e) {
System.err.println(
"Error executing task for the: "
Expand Down Expand Up @@ -405,5 +404,9 @@ public void periodic() {
visionThread(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER);
visionThread(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER);
SmartDashboard.putNumber("april tag dist", getLimelightAprilTagDistance(0));
SmartDashboard.putString("shooter ll odom", getPoseFromAprilTags(0).toString());
SmartDashboard.putString("left ll odom", getPoseFromAprilTags(1).toString());

SmartDashboard.putString("right ll odom", getPoseFromAprilTags(2).toString());
}
}

0 comments on commit 1ce2f18

Please sign in to comment.