Skip to content

Commit

Permalink
Create vision subsystem (#38)
Browse files Browse the repository at this point in the history
# 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
Show file tree
Hide file tree
Showing 8 changed files with 526 additions and 0 deletions.
142 changes: 142 additions & 0 deletions src/main/java/frc/robot/subsystems/limelight/Limelight.java
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());
}
}
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 src/main/java/frc/robot/subsystems/limelight/LimelightIO.java
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) {}
}
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 src/main/java/frc/robot/subsystems/limelight/LimelightPoseData.java
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)));
}
}
Loading

0 comments on commit 6a93d2e

Please sign in to comment.