Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Addressable leds #45

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
Open

Addressable leds #45

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
c1ecbcf
working battery voltage status bar
Shom770 Apr 1, 2024
2047e84
more led stuff
Shom770 Apr 1, 2024
d9f698d
more led stuff
Shom770 Apr 1, 2024
8b4b31f
fix feeder and LL
MatthewChoulas Apr 1, 2024
448b6fc
finish leds
MatthewChoulas Apr 2, 2024
80b4059
spotless
MatthewChoulas Apr 2, 2024
8f3d28d
elevator real
MatthewChoulas Apr 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
228 changes: 220 additions & 8 deletions src/main/java/com/team4099/utils/LimelightHelpers.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
package com.team4099.utils;//com.team4099.utils.LimelightHelpers v1.2.1 (March 1, 2023)
package com.team4099.utils;//com.team4099.utils.LimelightHelpers
//LimelightHelpers v1.5.0 (March 27, 2024)

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
Expand Down Expand Up @@ -301,6 +302,18 @@ public static class Results {
@JsonProperty("botpose_wpiblue")
public double[] botpose_wpiblue;

@JsonProperty("botpose_tagcount")
public double botpose_tagcount;

@JsonProperty("botpose_span")
public double botpose_span;

@JsonProperty("botpose_avgdist")
public double botpose_avgdist;

@JsonProperty("botpose_avgarea")
public double botpose_avgarea;

@JsonProperty("t6c_rs")
public double[] camerapose_robotspace;

Expand Down Expand Up @@ -361,8 +374,59 @@ public static class LimelightResults {
@JsonProperty("Results")
public Results targetingResults;

public String error;

public LimelightResults() {
targetingResults = new Results();
error = "";
}


}

public static class RawFiducial {
public int id;
public double txnc;
public double tync;
public double ta;
public double distToCamera;
public double distToRobot;
public double ambiguity;


public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
this.id = id;
this.txnc = txnc;
this.tync = tync;
this.ta = ta;
this.distToCamera = distToCamera;
this.distToRobot = distToRobot;
this.ambiguity = ambiguity;
}
}

public static class PoseEstimate {
public Pose2d pose;
public double timestampSeconds;
public double latency;
public int tagCount;
public double tagSpan;
public double avgTagDist;
public double avgTagArea;
public RawFiducial[] rawFiducials;

public PoseEstimate(Pose2d pose, double timestampSeconds, double latency,
int tagCount, double tagSpan, double avgTagDist,
double avgTagArea, RawFiducial[] rawFiducials) {

this.pose = pose;
this.timestampSeconds = timestampSeconds;
this.latency = latency;
this.tagCount = tagCount;
this.tagSpan = tagSpan;
this.avgTagDist = avgTagDist;
this.avgTagArea = avgTagArea;
this.rawFiducials = rawFiducials;
}
}

Expand All @@ -383,7 +447,7 @@ static final String sanitizeName(String name) {
private static Pose3d toPose3D(double[] inData){
if(inData.length < 6)
{
System.err.println("Bad LL 3D Pose Data!");
//System.err.println("Bad LL 3D Pose Data!");
return new Pose3d();
}
return new Pose3d(
Expand All @@ -395,14 +459,94 @@ private static Pose3d toPose3D(double[] inData){
private static Pose2d toPose2D(double[] inData){
if(inData.length < 6)
{
System.err.println("Bad LL 2D Pose Data!");
//System.err.println("Bad LL 2D Pose Data!");
return new Pose2d();
}
Translation2d tran2d = new Translation2d(inData[0], inData[1]);
Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5]));
return new Pose2d(tran2d, r2d);
}

private static double extractBotPoseEntry(double[] inData, int position){
if(inData.length < position+1)
{
return 0;
}
return inData[position];
}

private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) {
var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName);
var poseArray = poseEntry.getDoubleArray(new double[0]);
var pose = toPose2D(poseArray);
double latency = extractBotPoseEntry(poseArray,6);
int tagCount = (int)extractBotPoseEntry(poseArray,7);
double tagSpan = extractBotPoseEntry(poseArray,8);
double tagDist = extractBotPoseEntry(poseArray,9);
double tagArea = extractBotPoseEntry(poseArray,10);
//getlastchange() in microseconds, ll latency in milliseconds
var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0);


RawFiducial[] rawFiducials = new RawFiducial[tagCount];
int valsPerFiducial = 7;
int expectedTotalVals = 11 + valsPerFiducial*tagCount;

if (poseArray.length != expectedTotalVals) {
// Don't populate fiducials
}
else{
for(int i = 0; i < tagCount; i++) {
int baseIndex = 11 + (i * valsPerFiducial);
int id = (int)poseArray[baseIndex];
double txnc = poseArray[baseIndex + 1];
double tync = poseArray[baseIndex + 2];
double ta = poseArray[baseIndex + 3];
double distToCamera = poseArray[baseIndex + 4];
double distToRobot = poseArray[baseIndex + 5];
double ambiguity = poseArray[baseIndex + 6];
rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
}
}

return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials);
}

private static void printPoseEstimate(PoseEstimate pose) {
if (pose == null) {
System.out.println("No PoseEstimate available.");
return;
}

System.out.printf("Pose Estimate Information:%n");
System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
System.out.printf("Latency: %.3f ms%n", pose.latency);
System.out.printf("Tag Count: %d%n", pose.tagCount);
System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
System.out.println();

if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
System.out.println("No RawFiducials data available.");
return;
}

System.out.println("Raw Fiducials Details:");
for (int i = 0; i < pose.rawFiducials.length; i++) {
RawFiducial fiducial = pose.rawFiducials[i];
System.out.printf(" Fiducial #%d:%n", i + 1);
System.out.printf(" ID: %d%n", fiducial.id);
System.out.printf(" TXNC: %.2f%n", fiducial.txnc);
System.out.printf(" TYNC: %.2f%n", fiducial.tync);
System.out.printf(" TA: %.2f%n", fiducial.ta);
System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera);
System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot);
System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity);
System.out.println();
}
}

public static NetworkTable getLimelightNTTable(String tableName) {
return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
}
Expand Down Expand Up @@ -542,8 +686,8 @@ public static double getFiducialID(String limelightName) {
return getLimelightNTDouble(limelightName, "tid");
}

public static double getNeuralClassID(String limelightName) {
return getLimelightNTDouble(limelightName, "tclass");
public static String getNeuralClassID(String limelightName) {
return getLimelightNTString(limelightName, "tclass");
}

/////
Expand Down Expand Up @@ -602,6 +746,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
return toPose2D(result);
}

/**
* Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
* alliance
*
* @param limelightName
* @return
*/
public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
return getBotPoseEstimate(limelightName, "botpose_wpiblue");
}

/**
* Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
* alliance
*
* @param limelightName
* @return
*/
public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue");
}

/**
* Gets the Pose2d for easy use with Odometry vision pose estimator
* (addVisionMeasurement)
Expand All @@ -616,6 +782,26 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) {

}

/**
* Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
* alliance
* @param limelightName
* @return
*/
public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
return getBotPoseEstimate(limelightName, "botpose_wpired");
}

/**
* Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
* alliance
* @param limelightName
* @return
*/
public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
return getBotPoseEstimate(limelightName, "botpose_orb_wpired");
}

/**
* Gets the Pose2d for easy use with Odometry vision pose estimator
* (addVisionMeasurement)
Expand All @@ -641,6 +827,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) {
setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
}


public static void setPriorityTagID(String limelightName, int ID) {
setLimelightNTDouble(limelightName, "priorityid", ID);
}

/**
* The LEDs will be controlled by Limelight pipeline settings, and not by robot
* code.
Expand Down Expand Up @@ -694,6 +885,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c
setLimelightNTDoubleArray(limelightName, "crop", entries);
}

public static void SetRobotOrientation(String limelightName, double yaw, double yawRate,
double pitch, double pitchRate,
double roll, double rollRate) {

double[] entries = new double[6];
entries[0] = yaw;
entries[1] = yawRate;
entries[2] = pitch;
entries[3] = pitchRate;
entries[4] = roll;
entries[5] = rollRate;
setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
}

public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
double[] validIDsDouble = new double[validIDs.length];
for (int i = 0; i < validIDs.length; i++) {
validIDsDouble[i] = validIDs[i];
}
setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
}

public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
double[] entries = new double[6];
entries[0] = forward;
Expand Down Expand Up @@ -763,7 +976,7 @@ public static LimelightResults getLatestResults(String limelightName) {
try {
results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class);
} catch (JsonProcessingException e) {
System.err.println("lljson error: " + e.getMessage());
results.error = "lljson error: " + e.getMessage();
}

long end = System.nanoTime();
Expand All @@ -775,5 +988,4 @@ public static LimelightResults getLatestResults(String limelightName) {

return results;
}
}

}
10 changes: 7 additions & 3 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
import com.team4099.robot2023.subsystems.elevator.Elevator
import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
import com.team4099.robot2023.subsystems.elevator.ElevatorIO
import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
import com.team4099.robot2023.subsystems.feeder.Feeder
import com.team4099.robot2023.subsystems.feeder.FeederIONeo
Expand All @@ -28,6 +28,7 @@ import com.team4099.robot2023.subsystems.intake.IntakeIOFalconNEO
import com.team4099.robot2023.subsystems.intake.IntakeIOSim
import com.team4099.robot2023.subsystems.limelight.LimelightVision
import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal
import com.team4099.robot2023.subsystems.superstructure.Request
import com.team4099.robot2023.subsystems.superstructure.Superstructure
import com.team4099.robot2023.subsystems.vision.Vision
Expand All @@ -46,6 +47,7 @@ import org.team4099.lib.units.derived.Angle
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inDegrees
import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO

object RobotContainer {
private val drivetrain: Drivetrain
Expand Down Expand Up @@ -77,7 +79,7 @@ object RobotContainer {

drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2"))
limelight = LimelightVision(object : LimelightVisionIO {})
limelight = LimelightVision(LimelightVisionIOReal)
intake = Intake(IntakeIOFalconNEO)
feeder = Feeder(FeederIONeo)
elevator = Elevator(ElevatorIONEO)
Expand All @@ -95,7 +97,8 @@ object RobotContainer {
wrist = Wrist(WristIOSim)
}

superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision)
superstructure =
Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision, limelight)
vision.setDataInterfaces(
{ drivetrain.fieldTRobot },
{ drivetrain.addVisionData(it) },
Expand Down Expand Up @@ -161,6 +164,7 @@ object RobotContainer {
}

fun mapTeleopControls() {
limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE

ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
ControlBoard.intake.whileTrue(
Expand Down
Loading
Loading