Skip to content

Commit 597a694

Browse files
CoolSpy3CoolSpy3
authored
Merge pull request #34 from DeepBlueRobotics/apriltags
Lib199 Drivetrain Changes
2 parents cec4387 + 2f92390 commit 597a694

9 files changed

Lines changed: 344 additions & 116 deletions

File tree

‎src/main/java/org/carlmontrobotics/lib199/Limelight.java‎

Lines changed: 53 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,21 @@
77

88
package org.carlmontrobotics.lib199;
99

10-
import edu.wpi.first.networktables.NetworkTableInstance;
10+
import java.util.function.Consumer;
11+
12+
import com.fasterxml.jackson.databind.ObjectMapper;
13+
1114
import edu.wpi.first.math.controller.PIDController;
15+
import edu.wpi.first.math.geometry.Pose3d;
16+
import edu.wpi.first.math.geometry.Rotation3d;
17+
import edu.wpi.first.networktables.NetworkTableEntry;
18+
import edu.wpi.first.networktables.NetworkTableInstance;
1219
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1320

1421
public class Limelight {
1522

23+
public static final ObjectMapper JSON_MAPPER = new ObjectMapper();
24+
1625
public enum Mode {
1726
DIST, STEER, TARGET
1827
}
@@ -70,7 +79,7 @@ public Limelight(String ntName, double period) {
7079
*/
7180
public double determineMountingAngle(double distance, double cameraHeight, double objectHeight) {
7281
// NOTE: ty may be negative.
73-
tyDeg = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("ty").getDouble(0.0);
82+
tyDeg = getNTEntry("ty").getDouble(0.0);
7483
mountingAngleDeg = Math.atan((cameraHeight - objectHeight) / distance) * 180 / Math.PI - tyDeg;
7584
return mountingAngleDeg;
7685
}
@@ -84,9 +93,9 @@ public double determineMountingAngle(double distance, double cameraHeight, doubl
8493
*/
8594
public double[] determineObjectDist(double cameraHeight, double objectHeight, double cameraAngle) {
8695
// angle b/t where limelight is looking, and target in x direction
87-
txDeg = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("tx").getDouble(0.0);
96+
txDeg = getNTEntry("tx").getDouble(0.0);
8897
// angle b/t where limelight is looking, and target in y direction
89-
tyDeg = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("ty").getDouble(0.0);
98+
tyDeg = getNTEntry("ty").getDouble(0.0);
9099
// angle b/t limelight's looking line and flat
91100
mountingAngleDeg = cameraAngle;
92101
// diff b/t height of camera and height of target
@@ -108,8 +117,8 @@ public double[] determineObjectDist(double cameraHeight, double objectHeight, do
108117
// Adjusts the distance between a vision target and the robot. Uses basic PID
109118
// with the ty value from the network table.
110119
public double distanceAssist() {
111-
tv = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("tv").getDouble(0.0);
112-
ta = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("ta").getDouble(0.0);
120+
tv = getNTEntry("tv").getDouble(0.0);
121+
ta = getNTEntry("ta").getDouble(0.0);
113122
putValue("tyDeg", tyDeg);
114123
double adjustment = 0.0;
115124
double area_threshold = config.areaThresholdPercentage;
@@ -125,9 +134,9 @@ public double distanceAssist() {
125134
// Adjusts the angle facing a vision target. Uses basic PID with the tx value
126135
// from the network table.
127136
public double steeringAssist() {
128-
tv = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("tv").getDouble(0.0);
129-
txDeg = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("tx").getDouble(0.0);
130-
ta = NetworkTableInstance.getDefault().getTable(config.ntName).getEntry("ta").getDouble(0.0);
137+
tv = getNTEntry("tv").getDouble(0.0);
138+
txDeg = getNTEntry("tx").getDouble(0.0);
139+
ta = getNTEntry("ta").getDouble(0.0);
131140
putValue("txDeg", txDeg);
132141
putValue("tv", tv);
133142

@@ -182,7 +191,37 @@ public TurnDirection getIdleTurnDirection() {
182191
public void setIdleTurnDirection(TurnDirection direction) {
183192
idleTurnDirection = direction;
184193
}
185-
194+
195+
/**
196+
* Get the JSON dump from the limelight. This method returns via a callback because of the high latency observed in JSON parsing.
197+
* Keep in mind that the callback will be called asynchronously.
198+
*
199+
* @param onSuccess The callback to run if the JSON dump is successful
200+
* @param onFailure The callback to run if an error occurs
201+
*/
202+
public void getJsonDump(Consumer<LimelightJsonDump> onSuccess, Consumer<Exception> onFailure) {
203+
new Thread(() -> {
204+
try {
205+
onSuccess.accept(JSON_MAPPER.readValue(JSON_MAPPER.readTree(getNTEntry("json").getString(null)).elements().next().toString(), LimelightJsonDump.class));
206+
} catch (Exception e) {
207+
onFailure.accept(e);
208+
}
209+
}).start();
210+
}
211+
212+
public Pose3d getTransform(Transform transform) {
213+
double[] rawData = getNTEntry(transform.name().toLowerCase()).getDoubleArray(new double[6]);
214+
return new Pose3d(rawData[0], rawData[1], rawData[2], new Rotation3d(Math.toRadians(rawData[3]), Math.toRadians(rawData[4]), Math.toRadians(rawData[5])));
215+
}
216+
217+
public NetworkTableEntry getNTEntry(String key) {
218+
return NetworkTableInstance.getDefault().getTable(config.ntName).getEntry(key);
219+
}
220+
221+
public boolean hasTarget() {
222+
return getNTEntry("tv").getDouble(0.0) == 1.0;
223+
}
224+
186225
public static class Config {
187226
/**
188227
* Limelight name (if using more than one Limelight)
@@ -227,4 +266,8 @@ private TurnDirection(int sign) {
227266
this.sign = sign;
228267
}
229268
}
269+
270+
public static enum Transform {
271+
BOTPOSE, BOTPOSE_WPIBLUE, BOTPOSE_WPIRED, CAMERAPOSE_TARGETSPACE, TARGETPOSE_CAMERASPACE, TARGETPOSE_ROBOTSPACE, BOTPOSE_TARGETSPACE;
272+
}
230273
}

‎src/main/java/org/carlmontrobotics/lib199/LimelightJsonDump.java‎

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
package org.carlmontrobotics.lib199;
2+
3+
import com.fasterxml.jackson.annotation.JsonFormat;
4+
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
5+
import com.fasterxml.jackson.annotation.JsonProperty;
6+
import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
7+
import com.fasterxml.jackson.databind.util.StdConverter;
8+
9+
import edu.wpi.first.math.geometry.Pose3d;
10+
import edu.wpi.first.math.geometry.Rotation3d;
11+
12+
/**
13+
* A class that represents the JSON dump from the Limelight.
14+
*
15+
* Note: This class was only designed to read JSON dumps from the Limelight. It should not be used for generating a JSON dump.
16+
*/
17+
public class LimelightJsonDump {
18+
19+
@JsonProperty("pID")
20+
public int pipelineIndex;
21+
@JsonProperty("tl")
22+
public long latencyMs;
23+
@JsonProperty("ts")
24+
public long timestampMs;
25+
@JsonProperty("v")
26+
@JsonFormat(shape = JsonFormat.Shape.NUMBER)
27+
public boolean hasValidTargets;
28+
@JsonProperty("botpose")
29+
@JsonDeserialize(converter = Pose3dConverter.class)
30+
public Pose3d robotPose;
31+
@JsonProperty("Retro")
32+
public RetroResults[] retroResults;
33+
@JsonProperty("Fiducial")
34+
public FiducialResults[] fiducialResults;
35+
@JsonProperty("Detector")
36+
public DetectorResults[] detectorResults;
37+
@JsonProperty("Classifier")
38+
public ClassifierResults[] classifierResults;
39+
40+
public static class RetroResults {
41+
@JsonProperty("pts")
42+
public double[][] points;
43+
@JsonProperty("t6c_ts")
44+
@JsonDeserialize(converter = Pose3dConverter.class)
45+
public Pose3d cameraPoseInTargetSpace;
46+
@JsonProperty("t6r_fs")
47+
@JsonDeserialize(converter = Pose3dConverter.class)
48+
public Pose3d robotPoseInFieldSpace;
49+
@JsonProperty("t6r_ts")
50+
@JsonDeserialize(converter = Pose3dConverter.class)
51+
public Pose3d robotPoseInTargetSpace;
52+
@JsonProperty("t6t_cs")
53+
@JsonDeserialize(converter = Pose3dConverter.class)
54+
public Pose3d targetPoseInCameraSpace;
55+
@JsonProperty("t6t_rs")
56+
@JsonDeserialize(converter = Pose3dConverter.class)
57+
public Pose3d targetPoseInRobotSpace;
58+
@JsonProperty("ta")
59+
public double targetArea;
60+
@JsonProperty("tx")
61+
public double centerTxDeg;
62+
@JsonProperty("txp")
63+
public double centerTxPx;
64+
@JsonProperty("ty")
65+
public double centerTyDeg;
66+
@JsonProperty("typ")
67+
public double centerTyPx;
68+
}
69+
70+
@JsonIgnoreProperties("skew")
71+
public static class FiducialResults {
72+
@JsonProperty("fID")
73+
public int tagId;
74+
@JsonProperty("fam")
75+
public String tagFamily;
76+
@JsonProperty("pts")
77+
public double[][] points;
78+
@JsonProperty("t6c_ts")
79+
@JsonDeserialize(converter = Pose3dConverter.class)
80+
public Pose3d cameraPoseInTargetSpace;
81+
@JsonProperty("t6r_fs")
82+
@JsonDeserialize(converter = Pose3dConverter.class)
83+
public Pose3d robotPoseInFieldSpace;
84+
@JsonProperty("t6r_ts")
85+
@JsonDeserialize(converter = Pose3dConverter.class)
86+
public Pose3d robotPoseInTargetSpace;
87+
@JsonProperty("t6t_cs")
88+
@JsonDeserialize(converter = Pose3dConverter.class)
89+
public Pose3d targetPoseInCameraSpace;
90+
@JsonProperty("t6t_rs")
91+
@JsonDeserialize(converter = Pose3dConverter.class)
92+
public Pose3d targetPoseInRobotSpace;
93+
@JsonProperty("ta")
94+
public double targetArea;
95+
@JsonProperty("tx")
96+
public double centerTxDeg;
97+
@JsonProperty("txp")
98+
public double centerTxPx;
99+
@JsonProperty("ty")
100+
public double centerTyDeg;
101+
@JsonProperty("typ")
102+
public double centerTyPx;
103+
}
104+
105+
public static class DetectorResults {
106+
@JsonProperty("class")
107+
public String className;
108+
@JsonProperty("classID")
109+
public int classId;
110+
@JsonProperty("conf")
111+
public double confidence;
112+
@JsonProperty("pts")
113+
public double[][] points;
114+
@JsonProperty("ta")
115+
public double targetArea;
116+
@JsonProperty("tx")
117+
public double centerTxDeg;
118+
@JsonProperty("txp")
119+
public double centerTxPx;
120+
@JsonProperty("ty")
121+
public double centerTyDeg;
122+
@JsonProperty("typ")
123+
public double centerTyPx;
124+
}
125+
126+
public static class ClassifierResults {
127+
@JsonProperty("class")
128+
public String className;
129+
@JsonProperty("classID")
130+
public int classId;
131+
@JsonProperty("conf")
132+
public double confidence;
133+
}
134+
135+
public static class Pose3dConverter extends StdConverter<double[], Pose3d> {
136+
@Override
137+
public Pose3d convert(double[] value) {
138+
return value.length == 0 ? null : new Pose3d(value[0], value[1], value[2],
139+
new Rotation3d(Math.toRadians(value[3]), Math.toRadians(value[4]), Math.toRadians(value[5])));
140+
}
141+
}
142+
}

‎src/main/java/org/carlmontrobotics/lib199/path/DifferentialDriveInterface.java‎

Lines changed: 1 addition & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,8 @@
55

66
import edu.wpi.first.math.controller.RamseteController;
77
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
8-
import edu.wpi.first.math.geometry.Pose2d;
98
import edu.wpi.first.math.geometry.Rotation2d;
109
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
11-
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
1210
import edu.wpi.first.math.trajectory.Trajectory;
1311
import edu.wpi.first.math.trajectory.TrajectoryConfig;
1412
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
@@ -46,20 +44,6 @@ public interface DifferentialDriveInterface extends DrivetrainInterface {
4644
*/
4745
public double[][] getCharacterizationValues();
4846

49-
/**
50-
* Gets odometry
51-
*
52-
* @return Odometry
53-
*/
54-
public DifferentialDriveOdometry getOdometry();
55-
56-
/**
57-
* Sets odometry
58-
*
59-
* @param odometry Odometry that will be set
60-
*/
61-
public void setOdometry(DifferentialDriveOdometry odometry);
62-
6347
/**
6448
* @return The left encoder position in meters
6549
*/
@@ -108,20 +92,7 @@ public default void configureAutoPath(RobotPath path) {
10892
*/
10993
@Override
11094
public default Command createAutoCommand(Trajectory trajectory, Supplier<Rotation2d> desiredHeading) {
111-
return new RamseteCommand(trajectory,
112-
// Call getOdometry in the supplier because the odometry object may be reset
113-
// when the command is run
114-
() -> getOdometry().getPoseMeters(), createRamsete(), getKinematics(), this::drive, this);
115-
}
116-
117-
/**
118-
* Sets odometry to the specified pose
119-
*
120-
* @param initialPose The starting position of the robot on the field.
121-
*/
122-
@Override
123-
public default void setOdometry(Pose2d initialPose) {
124-
setOdometry(new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeadingDeg()), getLeftEncoderPosition(), getRightEncoderPosition(), initialPose));
95+
return new RamseteCommand(trajectory, this::getPose, createRamsete(), getKinematics(), this::drive, this);
12596
}
12697

12798
}

‎src/main/java/org/carlmontrobotics/lib199/path/DrivetrainInterface.java‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,12 +49,17 @@ public interface DrivetrainInterface extends Subsystem {
4949
*/
5050
public double getHeadingDeg();
5151

52+
/**
53+
* @return The current position of the robot
54+
*/
55+
public Pose2d getPose();
56+
5257
/**
5358
* Sets odometry to the specified pose
5459
*
5560
* @param initialPose The starting position of the robot on the field.
5661
*/
57-
public void setOdometry(Pose2d initialPose);
62+
public void setPose(Pose2d initialPose);
5863

5964
/**
6065
* Stops the drivetrain

0 commit comments

Comments
 (0)