-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
# Description Created a vision subsystem that can be used for the drive odometry, tracking our position on the field, or for tracking game objects. Fixes #36 ## Type of change Please delete options that are not relevant. - [x] New feature (non-breaking change which adds functionality) # Checklist: - [x] My code follows the style guidelines of this project - [x] I have performed a self-review of my code - [x] I have commented my code, particularly in hard-to-understand areas - [x] I have made corresponding changes to the documentation, if any - [x] My changes generate no new warnings - [x] I have performed tests that prove my fix is effective or that my feature works, if necessary - [x] New and existing unit tests pass locally with my changes --------- Co-authored-by: github-actions <>
- Loading branch information
Ian Tapply
authored
Dec 7, 2023
1 parent
5896726
commit 6a93d2e
Showing
8 changed files
with
526 additions
and
0 deletions.
There are no files selected for viewing
142 changes: 142 additions & 0 deletions
142
src/main/java/frc/robot/subsystems/limelight/Limelight.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,142 @@ | ||
package frc.robot.subsystems.limelight; | ||
|
||
import edu.wpi.first.math.geometry.Pose2d; | ||
import edu.wpi.first.math.geometry.Pose3d; | ||
import edu.wpi.first.networktables.NetworkTable; | ||
import edu.wpi.first.networktables.NetworkTableEntry; | ||
import edu.wpi.first.networktables.NetworkTableInstance; | ||
import edu.wpi.first.wpilibj.DriverStation; | ||
import edu.wpi.first.wpilibj.Timer; | ||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
import frc.robot.subsystems.limelight.enums.LimelightLEDMode; | ||
import frc.robot.subsystems.limelight.enums.LimelightStream; | ||
|
||
public class Limelight implements LimelightIO { | ||
private String limelightName; | ||
private NetworkTable networkTable; | ||
private LimelightPoseData limelightPoseData; | ||
|
||
/** | ||
* Creates a new limelight | ||
* | ||
* @param limelightName The name of the limelight | ||
*/ | ||
public Limelight(String limelightName) { | ||
this.limelightName = limelightName; | ||
|
||
// Initializes and sets camera and pipeline of the limelight | ||
this.networkTable = NetworkTableInstance.getDefault().getTable(this.limelightName); | ||
this.networkTable.getEntry(LimelightConstants.TableConstants.CAMERA).setNumber(0); | ||
this.networkTable.getEntry(LimelightConstants.TableConstants.PIPELINE).setNumber(0); | ||
} | ||
|
||
@Override | ||
public void updateData(LimelightIOData limelightIOData) { | ||
NetworkTableEntry robotPoseEntry; | ||
|
||
// Creates a new limelight pose data instance if it doesn't exist | ||
if (this.limelightPoseData == null) { | ||
this.limelightPoseData = new LimelightPoseData(new double[7]); | ||
} | ||
|
||
// Sets the pose of the limelight based on what alliance we are on | ||
if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { | ||
robotPoseEntry = this.networkTable.getEntry(LimelightConstants.TableConstants.BOT_POSE_BLUE); | ||
} else if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { | ||
robotPoseEntry = this.networkTable.getEntry(LimelightConstants.TableConstants.BOT_POSE_RED); | ||
} else { | ||
robotPoseEntry = this.networkTable.getEntry(LimelightConstants.TableConstants.BOT_POSE); | ||
} | ||
|
||
// Update the limelight pose data | ||
this.limelightPoseData.update(robotPoseEntry.getDoubleArray(new double[7])); | ||
|
||
// Create a 3d pose from data from the limelight | ||
Pose3d limelightPose = limelightPoseData.toPose3d(); | ||
|
||
// Set if the limelight is locked onto a target | ||
if (this.networkTable.getEntry(LimelightConstants.TableConstants.VALID_TARGET).getDouble(0) | ||
== 1) { | ||
limelightIOData.lockedOnTarget = true; | ||
} else { | ||
limelightIOData.lockedOnTarget = false; | ||
} | ||
|
||
if (limelightIOData.lockedOnTarget) { | ||
// Set the time that the limelight was updated | ||
limelightIOData.limelightLastUpdated = | ||
Timer.getFPGATimestamp() - limelightPoseData.getTotalLatency(); | ||
|
||
limelightIOData.isNewPose = true; | ||
|
||
// Set the target x and y | ||
limelightIOData.targetX = | ||
this.networkTable.getEntry(LimelightConstants.TableConstants.TARGET_X).getDouble(0); | ||
limelightIOData.targetY = | ||
this.networkTable.getEntry(LimelightConstants.TableConstants.TARGET_Y).getDouble(0); | ||
|
||
// Set the pose of the limelight (x, y, rotation) | ||
Pose2d pose2d = limelightPose.toPose2d(); | ||
limelightIOData.limelightX = pose2d.getX(); | ||
limelightIOData.limelightY = pose2d.getY(); | ||
limelightIOData.limelightRotation = pose2d.getRotation().getRadians(); | ||
limelightIOData.limelightPose = pose2d; | ||
|
||
} else { | ||
limelightIOData.isNewPose = false; | ||
|
||
// Set the target x and y to 0 because we don't have a target | ||
limelightIOData.targetX = 0; | ||
limelightIOData.targetY = 0; | ||
} | ||
} | ||
|
||
/** Gets the name of a limelight that's registered into the network table */ | ||
@Override | ||
public String getName() { | ||
return this.limelightName; | ||
} | ||
|
||
/** | ||
* Sets the led mode of the limelight | ||
* | ||
* @param limelightLEDMode The led mode to set the limelight to (see LimelightLEDMode.java for | ||
* options) | ||
*/ | ||
public void setLEDMode(LimelightLEDMode limelightLEDMode) { | ||
this.networkTable | ||
.getEntry(LimelightConstants.TableConstants.LED_MODE) | ||
.setNumber(limelightLEDMode.getNetworkTableValue()); | ||
} | ||
|
||
/** | ||
* Sets the streaming mode of the limelight | ||
* | ||
* @param stream The streaming mode to set the limelight to as a limelight stream type | ||
*/ | ||
public void setStream(LimelightStream limelightStream) { | ||
this.networkTable | ||
.getEntry(LimelightConstants.TableConstants.STREAM) | ||
.setNumber(limelightStream.getNetworkTableValue()); | ||
} | ||
|
||
/** | ||
* Sets the pipeline of the limelight based on our presets for it | ||
* | ||
* @param pipeline The pipeline to set the limelight to as an integer | ||
*/ | ||
public void setPipeline(int pipeline) { | ||
this.networkTable.getEntry(LimelightConstants.TableConstants.PIPELINE).setNumber(pipeline); | ||
} | ||
|
||
/** | ||
* Displays the pose values of the limelight on the smart dashboard | ||
* | ||
* @param pose The pose to retrieve and display the values from | ||
*/ | ||
public void displayLimelightPoseValues(Pose2d pose) { | ||
SmartDashboard.putNumber(this.limelightName + " x", pose.getX()); | ||
SmartDashboard.putNumber(this.limelightName + " y", pose.getY()); | ||
SmartDashboard.putNumber(this.limelightName + " angleDegrees", pose.getRotation().getDegrees()); | ||
} | ||
} |
29 changes: 29 additions & 0 deletions
29
src/main/java/frc/robot/subsystems/limelight/LimelightConstants.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,29 @@ | ||
package frc.robot.subsystems.limelight; | ||
|
||
import edu.wpi.first.math.util.Units; | ||
|
||
public class LimelightConstants { | ||
public static final double LOWEST_DISTANCE = Units.feetToMeters(10.0); | ||
|
||
static class TableConstants { | ||
/** | ||
* All tables for limelights can be found here | ||
* https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api | ||
*/ | ||
public static final String CAMERA = "camera"; | ||
|
||
public static final String PIPELINE = "pipeline"; | ||
public static final String LED_MODE = "ledMode"; | ||
public static final String STREAM = "stream"; | ||
|
||
// Driverstation/botposes | ||
public static final String BOT_POSE_BLUE = "botpose_wpiblue"; | ||
public static final String BOT_POSE_RED = "botpose_wpired"; | ||
public static final String BOT_POSE = "botpose"; | ||
|
||
// Targeting related | ||
public static final String VALID_TARGET = "tv"; | ||
public static final String TARGET_X = "tx"; | ||
public static final String TARGET_Y = "ty"; | ||
} | ||
} |
52 changes: 52 additions & 0 deletions
52
src/main/java/frc/robot/subsystems/limelight/LimelightIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,52 @@ | ||
package frc.robot.subsystems.limelight; | ||
|
||
import edu.wpi.first.math.geometry.Pose2d; | ||
|
||
public interface LimelightIO { | ||
|
||
/** Data that the limelight will collect and track */ | ||
class LimelightIOData { | ||
public double limelightX; | ||
public double limelightY; | ||
public double limelightRotation; | ||
|
||
public double targetX; | ||
public double targetY; | ||
|
||
public boolean isNewPose; | ||
public Pose2d limelightPose; | ||
public double limelightLastUpdated; | ||
|
||
public double maxDistance; | ||
public double minDistance; | ||
|
||
public boolean lockedOnTarget = false; | ||
public int singleIDUsed; | ||
|
||
public double translationToTargetX; | ||
public double translationToTargetY; | ||
} | ||
|
||
/** | ||
* Updates the data of the limelight | ||
* | ||
* @param data | ||
*/ | ||
default void updateData(LimelightIOData data) {} | ||
|
||
/** | ||
* Gets, or sets the name of the limelight | ||
* | ||
* @return | ||
*/ | ||
default String getName() { | ||
return ""; | ||
} | ||
|
||
/** | ||
* Sets the refrence pose of the limelight | ||
* | ||
* @param pose | ||
*/ | ||
default void setRefrencePose(Pose2d pose) {} | ||
} |
21 changes: 21 additions & 0 deletions
21
src/main/java/frc/robot/subsystems/limelight/LimelightPoseAndTimestamp.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,21 @@ | ||
package frc.robot.subsystems.limelight; | ||
|
||
import edu.wpi.first.math.geometry.Pose2d; | ||
|
||
public class LimelightPoseAndTimestamp { | ||
Pose2d limelightPose; | ||
double lastUpdatedTimestamp; | ||
|
||
public LimelightPoseAndTimestamp(Pose2d limelightPose, double lastUpdatedTimestamp) { | ||
this.limelightPose = limelightPose; | ||
this.lastUpdatedTimestamp = lastUpdatedTimestamp; | ||
} | ||
|
||
public Pose2d getLimelightPose() { | ||
return this.limelightPose; | ||
} | ||
|
||
public double getLastUpdatedTimestamp() { | ||
return this.lastUpdatedTimestamp; | ||
} | ||
} |
116 changes: 116 additions & 0 deletions
116
src/main/java/frc/robot/subsystems/limelight/LimelightPoseData.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,116 @@ | ||
package frc.robot.subsystems.limelight; | ||
|
||
import edu.wpi.first.math.geometry.Pose3d; | ||
import edu.wpi.first.math.geometry.Rotation3d; | ||
|
||
public class LimelightPoseData { | ||
private double x; | ||
private double y; | ||
private double z; | ||
private double roll; | ||
private double pitch; | ||
private double yaw; | ||
private double totalLatency; | ||
|
||
public LimelightPoseData(double[] data) { | ||
if (data.length != 7) { | ||
throw new IllegalArgumentException("Data array must have 7 elements"); | ||
} | ||
|
||
this.x = data[0]; | ||
this.y = data[1]; | ||
this.z = data[2]; | ||
this.roll = data[3]; | ||
this.pitch = data[4]; | ||
this.yaw = data[5]; | ||
this.totalLatency = data[6]; | ||
} | ||
|
||
/** | ||
* Updates the pose data without creating a new instance of limelight pose data | ||
* | ||
* @param data The data to update the pose data with | ||
*/ | ||
public void update(double[] data) { | ||
if (data.length != 7) { | ||
throw new IllegalArgumentException("Data array must have 7 elements"); | ||
} | ||
|
||
this.x = data[0]; | ||
this.y = data[1]; | ||
this.z = data[2]; | ||
this.roll = data[3]; | ||
this.pitch = data[4]; | ||
this.yaw = data[5]; | ||
this.totalLatency = data[6]; | ||
} | ||
|
||
/** | ||
* Gets the x position of the limelight | ||
* | ||
* @return The x position of the limelight | ||
*/ | ||
public double getX() { | ||
return this.x; | ||
} | ||
|
||
/** | ||
* Gets the y position of the limelight | ||
* | ||
* @return The y position of the limelight | ||
*/ | ||
public double getY() { | ||
return this.y; | ||
} | ||
|
||
/** | ||
* Gets the z position of the limelight | ||
* | ||
* @return The z position of the limelight | ||
*/ | ||
public double getZ() { | ||
return this.z; | ||
} | ||
|
||
/** | ||
* Gets a 3d rotation position of the limelight | ||
* | ||
* @return The 3d rotation position of the limelight | ||
*/ | ||
public Rotation3d getRotation() { | ||
return new Rotation3d( | ||
Math.toRadians(this.roll), Math.toRadians(this.pitch), Math.toRadians(this.yaw)); | ||
} | ||
|
||
/** | ||
* Gets the raw latency of the limelight straight from the network table | ||
* | ||
* @return The raw latency of the limelight straight from the network table | ||
*/ | ||
public double getRawTotalLatency() { | ||
return this.totalLatency; | ||
} | ||
|
||
/** | ||
* Gets the total latency of the limelight in seconds | ||
* | ||
* @return The total latency of the limelight in seconds | ||
*/ | ||
public double getTotalLatency() { | ||
return this.totalLatency / 1000; | ||
} | ||
|
||
/** | ||
* Converts the limelight pose data to a 3d pose | ||
* | ||
* @return A 3d pose of the limelight pose data | ||
*/ | ||
public Pose3d toPose3d() { | ||
return new Pose3d( | ||
this.x, | ||
this.y, | ||
this.z, | ||
new Rotation3d( | ||
Math.toRadians(this.roll), Math.toRadians(this.pitch), Math.toRadians(this.yaw))); | ||
} | ||
} |
Oops, something went wrong.