Skip to content

Commit

Permalink
fix build and apply spotless
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ishan1522
Ishan1522 committed Jul 28, 2024
1 parent c51500e commit 82eb435
Showing 1 changed file with 120 additions and 82 deletions.
202 changes: 120 additions & 82 deletions src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package frc.robot.subsystems.vision;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
Expand All @@ -11,10 +10,8 @@
import frc.robot.Constants.VisionConstants;
import frc.robot.extras.LimelightHelpers;
import frc.robot.extras.LimelightHelpers.PoseEstimate;

import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;
Expand All @@ -26,7 +23,8 @@ public class VisionSubsystem extends SubsystemBase {
private double headingDegrees = 0;
private double headingRateDegreesPerSecond = 0;
private final Map<Integer, AtomicBoolean> limelightThreads = new ConcurrentHashMap<>();
private final ScheduledExecutorService scheduledExecutorService = Executors.newScheduledThreadPool(3); // lol no clue if this'll work
private final ScheduledExecutorService scheduledExecutorService =
Executors.newScheduledThreadPool(3); // lol no clue if this'll work

/**
* The pose estimates from the limelights in the following order {shooterLimelight,
Expand All @@ -42,7 +40,6 @@ public VisionSubsystem() {
}
}


/**
* Checks if the specified limelight can fully see one or more April Tag.
*
Expand Down Expand Up @@ -72,22 +69,22 @@ && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS.
* @param limelightNumber the number of the limelight
*/
public void updateLimelightPoseEstimate(int limelightNumber) {
//Soon to be implemented code:
// Soon to be implemented code:
// if (canSeeAprilTags(limelightNumber)) {
// if (isValidPoseEstimate(limelightNumber)) {
// double distanceToAprilTags = getLimelightAprilTagDistance(limelightNumber);

// if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber)
// && distanceToAprilTags < 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);
}
// double distanceToAprilTags = getLimelightAprilTagDistance(limelightNumber);

// if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber)
// && distanceToAprilTags < 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);
}
// }
// }
}
Expand All @@ -110,16 +107,19 @@ public boolean isLargeDiscrepancyBetweenMegaTag1And2(int limelightNumber) {
double megaTag2RotationDegrees = megaTag2Estimate.pose.getRotation().getDegrees();

// Calculate the discrepancy between the two MegaTag translations in meters
double megaTagTranslationDiscrepancyMeters = megaTag1TranslationMeters.getDistance(megaTag2TranslationMeters);
double megaTagRotationDiscrepancyDegrees = Math.abs(megaTag1RotationDegrees - megaTag2RotationDegrees);
double megaTagTranslationDiscrepancyMeters =
megaTag1TranslationMeters.getDistance(megaTag2TranslationMeters);
double megaTagRotationDiscrepancyDegrees =
Math.abs(megaTag1RotationDegrees - megaTag2RotationDegrees);

// Define a threshold (meters) for what constitutes a "large" discrepancy
// This value should be determined based on your testing
double thresholdMeters = 0.5;
double thresholdDegrees = 45;

// Check if the discrepancy is larger than the threshold (meters)
return megaTagTranslationDiscrepancyMeters > thresholdMeters || megaTagRotationDiscrepancyDegrees > thresholdDegrees;
return megaTagTranslationDiscrepancyMeters > thresholdMeters
|| megaTagRotationDiscrepancyDegrees > thresholdDegrees;
}

/**
Expand Down Expand Up @@ -148,6 +148,7 @@ public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) {

/**
* Checks is megatag 1 and megatag 2 estimates are good
*
* @param limelightNumber
* @return
*/
Expand All @@ -160,31 +161,37 @@ public boolean isValidPoseEstimate(int limelightNumber) {

/**
* Checks if the MegaTag1 Estimate is within the field parameters
*
* @param limelightNumber
* @return
* @return
*/
public boolean isMegaTag1Good(int limelightNumber) {
PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber);

if ((megaTag1Estimate.pose.getX() > 0 && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) &&
(megaTag1Estimate.pose.getY() > 0 && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)) {
if ((megaTag1Estimate.pose.getX() > 0
&& megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
&& (megaTag1Estimate.pose.getY() > 0
&& megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)) {
return true;
}
}
return false;
}

/**
* Checks if the MegaTag2 Estimate is within the field parameters
*
* @param limelightNumber
* @return
*/
public boolean isMegaTag2Good(int limelightNumber) {
PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber);

if ((megaTag2Estimate.pose.getX() > 0 && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) &&
(megaTag2Estimate.pose.getY() > 0 && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)) {
return true;
}
if ((megaTag2Estimate.pose.getX() > 0
&& megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
&& (megaTag2Estimate.pose.getY() > 0
&& megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)) {
return true;
}
return false;
}

Expand Down Expand Up @@ -221,6 +228,10 @@ public double getLatencySeconds(int limelightNumber) {
return (limelightEstimates[limelightNumber].latency) / 1000.0;
}

public Pose2d getLastSeenPose() {
return lastSeenPose;
}

/**
* Gets the average distance between the specified limelight and the April Tags it sees
*
Expand Down Expand Up @@ -256,14 +267,15 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) {
*/
public String getLimelightName(int limelightNumber) {
return switch (limelightNumber) {
case 0 -> VisionConstants.SHOOTER_LIMELIGHT_NAME;
case 1 -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME;
case 2 -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME;
default -> throw new IllegalArgumentException("You entered a number for a non-existent limelight");
case 0 -> VisionConstants.SHOOTER_LIMELIGHT_NAME;
case 1 -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME;
case 2 -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME;
default ->
throw new IllegalArgumentException("You entered a number for a non-existent limelight");
};
}
/**

/**
* This checks is there is new pose detected by a limelight, and if so, updates the pose estimate
*
* @param limelightNumber the limelight number
Expand All @@ -272,11 +284,14 @@ public void checkAndUpdatePose(int limelightNumber) {
double last_TX = 0;
double last_TY = 0;

// Syncronization block to ensure thread safety during the critical section where pose information is read and compared.
// This helps prevent race conditions, where one limelight may be updating an object that another limelight is reading.
// A race condition could cause unpredictable things to happen. Such as causing a limelight to be unable to reference an
// Syncronization block to ensure thread safety during the critical section where pose
// information is read and compared.
// This helps prevent race conditions, where one limelight may be updating an object that
// another limelight is reading.
// A race condition could cause unpredictable things to happen. Such as causing a limelight to
// be unable to reference an
// object, as its reference was modified earlier.
synchronized (this) {
synchronized (this) {
try {
double current_TX = LimelightHelpers.getTX(getLimelightName(limelightNumber));
double current_TY = LimelightHelpers.getTY(getLimelightName(limelightNumber));
Expand All @@ -287,56 +302,73 @@ public void checkAndUpdatePose(int limelightNumber) {
// 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);
limelightThreads.computeIfPresent(limelightNumber, (key, value) -> new AtomicBoolean(true));
limelightThreads.computeIfPresent(
limelightNumber, (key, value) -> new AtomicBoolean(true));
// This is to keep track of the last valid pose calculated by the limelights
// it is used when the driver resets the robot odometry to the limelight calculated position
// it is used when the driver resets the robot odometry to the limelight calculated
// position
if (canSeeAprilTags(limelightNumber)) {
lastSeenPose = getMegaTag1PoseEstimate(limelightNumber).pose;
}
} else {
// Retrieve the AtomicBoolean for the given limelight number
AtomicBoolean threadIsRunning = limelightThreads.getOrDefault(limelightNumber, new AtomicBoolean());
// Only stop the thread if it's currently running
if (threadIsRunning.get()) {
// stop the thread for the specified limelight
stopThread(limelightNumber);
}
// Retrieve the AtomicBoolean for the given limelight number
AtomicBoolean threadIsRunning =
limelightThreads.getOrDefault(limelightNumber, new AtomicBoolean());
// Only stop the thread if it's currently running
if (threadIsRunning.get()) {
// stop the thread for the specified limelight
stopThread(limelightNumber);
}
}
last_TX = current_TX;
last_TY = current_TY;
} catch (Exception e) {
System.err.println("Error communicating with the: " + getLimelightName(limelightNumber) + ": " + e.getMessage());
handleLimelightDisconnect(limelightNumber);
} catch (Exception e) {
System.err.println(
"Error communicating with the: "
+ getLimelightName(limelightNumber)
+ ": "
+ e.getMessage());
handleLimelightDisconnect(limelightNumber);
}
}
}

/**
* theoretically should handle if a limelight disconnects or not.
*
* @param limelightNumber
*/
public void handleLimelightDisconnect(int limelightNumber) {
System.err.println(getLimelightName(limelightNumber) + " disconnected.");
// Schedule a task to attempt reconnection after a short delay
scheduledExecutorService.schedule(() -> {
try {
// Attempt to access NetworkTable associated with Limelight to check connection health
NetworkTableInstance limelightTableInstance = NetworkTableInstance.getDefault();
NetworkTable limelightTable = limelightTableInstance.getTable(getLimelightName(limelightNumber));

// Perform a simple read operation as a health check
boolean isConnected = limelightTable.containsKey("v");

if (isConnected) {
System.out.println("Successfully reconnected to " + getLimelightName(limelightNumber));
} else {
throw new RuntimeException("Failed to reconnect to " + getLimelightName(limelightNumber));
scheduledExecutorService.schedule(
() -> {
try {
// Attempt to access NetworkTable associated with Limelight to check connection health
NetworkTableInstance limelightTableInstance = NetworkTableInstance.getDefault();
NetworkTable limelightTable =
limelightTableInstance.getTable(getLimelightName(limelightNumber));

// Perform a simple read operation as a health check
boolean isConnected = limelightTable.containsKey("v");

if (isConnected) {
System.out.println(
"Successfully reconnected to " + getLimelightName(limelightNumber));
} else {
throw new RuntimeException(
"Failed to reconnect to " + getLimelightName(limelightNumber));
}
} catch (Exception e) {
System.err.println(
"Reconnection attempt failed for "
+ getLimelightName(limelightNumber)
+ ": "
+ e.getMessage());
}
} catch (Exception e) {
System.err.println("Reconnection attempt failed for " + getLimelightName(limelightNumber) + ": " + e.getMessage());

}
}, 5, TimeUnit.SECONDS); // Retry after 5 seconds
},
5,
TimeUnit.SECONDS); // Retry after 5 seconds
}

/**
Expand All @@ -354,20 +386,21 @@ public void handleLimelightDisconnect(int limelightNumber) {
* @param limelightNumber the limelight number
*/
public void visionThread(int limelightNumber) {
scheduledExecutorService.submit(
() -> {
try {

scheduledExecutorService.submit(
() -> {
try {
while (limelightThreads.get(limelightNumber).get()) {
checkAndUpdatePose(limelightNumber);
}
} catch (Exception e) {
System.err.println(
"Error executing task for the: "
+ getLimelightName(limelightNumber)
+ ": "
+ e.getMessage());
}
catch (Exception e) {
System.err.println("Error executing task for the: " + getLimelightName(limelightNumber) + ": " + e.getMessage());
}
});


});
}

/**
Expand All @@ -378,11 +411,16 @@ public void visionThread(int limelightNumber) {
*/
public void stopThread(int limelightNumber) {
try {
// Since we can't see an April Tag, set the estimate for the specified limelight to an empty PoseEstimate()
// Since we can't see an April Tag, set the estimate for the specified limelight to an empty
// PoseEstimate()
limelightEstimates[limelightNumber] = new PoseEstimate();
limelightThreads.get(limelightNumber).set(false);
} catch (Exception e) {
System.err.println("Error stopping thread for the: " + getLimelightName(limelightNumber) + ": " + e.getMessage());
System.err.println(
"Error stopping thread for the: "
+ getLimelightName(limelightNumber)
+ ": "
+ e.getMessage());
}
}

Expand Down

0 comments on commit 82eb435

Please sign in to comment.