Skip to content

Commit

Permalink
Wrist tuning (#46)
Browse files Browse the repository at this point in the history 
* working battery voltage status bar

* more led stuff

* more led stuff

* fix feeder and LL

* finish leds

* spotless

* elevator real

* resolved merge conflicts

* theta stuff

* more merge conflicts

* new stuff

* tree map tuning

* tuned shooter

* tuned wrist

* add high interpolating map

* auto fixes and high note tree map

* load in changes

* add aiming leds

* add passing shot and day 1 changes

* y

* fix amp auto and add second passsing shot

* hopefully fix intake handoff issues

* EOL for drive

* add autonomous check for extra grount intake transitions

* tuned wrist

* wedsday robot work

* revert testing changes

* force idle when auto aim release

* pre worlds auto changes

* fix build

---------

Co-authored-by: Matthew Choulas <[email protected]>
Co-authored-by: AlphaPranav9102 <[email protected]>
  • Loading branch information
@Shom770 @MatthewChoulas @AlphaPranav9102
3 people authored Apr 17, 2024
1 parent ae5b851 commit d1421ed
Show file tree
Hide file tree
Showing 46 changed files with 1,621 additions and 614 deletions.
228 changes: 220 additions & 8 deletions src/main/java/com/team4099/utils/LimelightHelpers.java
View file
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;
}
}

}
3 changes: 1 addition & 2 deletions src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package com.team4099.lib.trajectory

import edu.wpi.first.math.controller.PIDController
import edu.wpi.first.math.controller.ProfiledPIDController
import edu.wpi.first.math.geometry.Pose2d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.kinematics.ChassisSpeeds
Expand All @@ -22,7 +21,7 @@ import org.team4099.lib.units.derived.radians
class CustomHolonomicDriveController(
private val m_xController: PIDController,
private val m_yController: PIDController,
private val m_thetaController: ProfiledPIDController
private val m_thetaController: PIDController
) {
private var m_poseError = Pose2d()
private var m_rotationError = Rotation2d()
Expand Down
15 changes: 8 additions & 7 deletions src/main/kotlin/com/team4099/robot2023/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
import edu.wpi.first.wpilibj.simulation.DriverStationSim
import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.CommandScheduler
import edu.wpi.first.wpilibj2.command.WaitCommand
import edu.wpi.first.wpilibj2.command.Commands.runOnce
import org.ejml.EjmlVersion.BUILD_DATE
import org.ejml.EjmlVersion.DIRTY
import org.ejml.EjmlVersion.GIT_BRANCH
Expand Down Expand Up @@ -128,15 +128,15 @@ object Robot : LoggedRobot() {

// Set the scheduler to log events for command initialize, interrupt, finish
CommandScheduler.getInstance().onCommandInitialize { command: Command ->
DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", true)
Logger.recordOutput("/ActiveCommands/${command.name}", true)
}

CommandScheduler.getInstance().onCommandFinish { command: Command ->
DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false)
Logger.recordOutput("/ActiveCommands/${command.name}", false)
}

CommandScheduler.getInstance().onCommandInterrupt { command: Command ->
DebugLogger.recordDebugOutput("/ActiveCommands/${command.name}", false)
Logger.recordOutput("/ActiveCommands/${command.name}", false)
}

val autoTab = Shuffleboard.getTab("Pre-match")
Expand All @@ -146,12 +146,13 @@ object Robot : LoggedRobot() {
.withPosition(0, 1)
.withWidget(BuiltInWidgets.kTextView)
.entry

RobotContainer.zeroSensors(isInAutonomous = true)
}

override fun autonomousInit() {
val autonCommandWithWait = WaitCommand(0.001).andThen(autonomousCommand)
RobotContainer.setSteeringCoastMode()

val autonCommandWithWait =
runOnce({ RobotContainer.zeroSensors(isInAutonomous = true) }).andThen(autonomousCommand)
autonCommandWithWait?.schedule()
}

Expand Down
Loading

0 comments on commit d1421ed

Please sign in to comment.