Skip to content

Commit d629b56

Browse files
ArjunSharma-cklArjunSharma-ckl
committed
UPDATED
1 parent d82399d commit d629b56

File tree

1 file changed

+56
-126
lines changed

1 file changed

+56
-126
lines changed

‎src/main/java/frc/robot/subsystems/localization/localizationSubsystem.java‎

Lines changed: 56 additions & 126 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99

1010
public class localizationSubsystem extends SubsystemBase {
1111

12-
private static final String LIMELIGHT_NAME = "limelight-left";
12+
private static final String LIMELIGHT_NAME = "limelight-left"; // must match Limelight name in UI
1313
private final NetworkTable llTable = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME);
1414
private final NetworkTable logTable = NetworkTableInstance.getDefault().getTable("LocalizationSubsystem");
1515

@@ -20,54 +20,24 @@ public class localizationSubsystem extends SubsystemBase {
2020
private double ta = 0.0;
2121
private double[] tcornxy = new double[8]; // [x0,y0,x1,y1,x2,y2,x3,y3]
2222

23-
// ---- Vision → Motion planning parameters ----
24-
private static final double DEG_PER_PIXEL = 0.16875;
25-
private static final double PIXEL_TOLERANCE = 10.0;
26-
private static final boolean USE_PIXEL_TOL_FOR_TX = false;
27-
private static final double TX_TOLERANCE_DEG = USE_PIXEL_TOL_FOR_TX ? (PIXEL_TOLERANCE * DEG_PER_PIXEL) : 1.0;
28-
private static final double DESIRED_RANGE_M = 0.0254; // 1 inch
29-
30-
private static final double MIN_TA = 1e-6;
31-
private static final double MAX_REASONABLE_METERS = 15.0;
32-
33-
// ---- TA calibration ranges ----
34-
private double[] TA_1FT = {29000, 31000};
35-
private double[] TA_3FT = {12200, 12400};
36-
private double[] TA_5FT = {6200, 6400};
37-
private double[] TA_7FT = {3600, 3800};
38-
private double[] TA_10FT = {1900, 2100};
39-
40-
// ---- Plan output ----
41-
public static final class Plan {
42-
public double forwardErrorMeters;
43-
public double strafeErrorMeters;
44-
public double headingErrorDeg;
45-
public boolean atRange;
46-
public boolean centered;
47-
public boolean facing;
48-
public String recommendation;
49-
}
50-
51-
private final Plan currentPlan = new Plan();
52-
5323
@Override
5424
public void periodic() {
5525
double now = Timer.getFPGATimestamp();
56-
if (now - lastUpdate < 0.011) return; // ~90 FPS
26+
// Run ~90 FPS (~11 ms per update)
27+
if (now - lastUpdate < 0.011)
28+
return;
5729
lastUpdate = now;
5830

5931
updateFromLimelight();
6032

33+
// only compute geometry if valid corners
6134
if (isCornersValid()) {
6235
calculate3DPosition();
6336
calculateTagArea();
6437
calculateDiagonalsAndMidpoint();
6538
} else {
6639
Logger.recordOutput("Localization/Status", "No valid corners");
6740
}
68-
69-
computePlanFromVision();
70-
logPlan(currentPlan);
7141
}
7242

7343
private void updateFromLimelight() {
@@ -77,6 +47,25 @@ private void updateFromLimelight() {
7747
ta = llTable.getEntry("ta").getDouble(0);
7848
tcornxy = llTable.getEntry("tcornxy").getDoubleArray(new double[8]);
7949

50+
// Always log
51+
// Logger.recordOutput("Localization/tv", tv);
52+
// Logger.recordOutput("Localization/tx", tx);
53+
// Logger.recordOutput("Localization/ty", ty);
54+
// Logger.recordOutput("Localization/ta", ta);
55+
56+
// Individual corner points
57+
if (tcornxy.length >= 8) {
58+
// Logger.recordOutput("Localization/x0", tcornxy[0]);
59+
// Logger.recordOutput("Localization/y0", tcornxy[1]);
60+
// Logger.recordOutput("Localization/x1", tcornxy[2]);
61+
// Logger.recordOutput("Localization/y1", tcornxy[3]);
62+
// Logger.recordOutput("Localization/x2", tcornxy[4]);
63+
// Logger.recordOutput("Localization/y2", tcornxy[5]);
64+
// Logger.recordOutput("Localization/x3", tcornxy[6]);
65+
// Logger.recordOutput("Localization/y3", tcornxy[7]);
66+
}
67+
68+
// Publish to NetworkTables only if valid target
8069
if (tv == 1) {
8170
logTable.getEntry("tx").setDouble(tx);
8271
logTable.getEntry("ty").setDouble(ty);
@@ -85,28 +74,42 @@ private void updateFromLimelight() {
8574
logTable.getEntry("timestamp").setDouble(Timer.getFPGATimestamp());
8675
}
8776

88-
//System.out.println("tv=" + tv + " tx=" + tx + " ty=" + ty + " ta=" + ta);
77+
// Console debug
78+
System.out.println("tv=" + tv + " tx=" + tx + " ty=" + ty + " ta=" + ta);
79+
System.out.println("tcornxy: " + Arrays.toString(tcornxy));
8980
}
9081

82+
/** Checks whether corner data is valid (not all zeros and length = 8) */
9183
private boolean isCornersValid() {
92-
if (tcornxy == null || tcornxy.length < 8) return false;
93-
for (double v : tcornxy) if (Math.abs(v) > 1e-3) return true;
84+
if (tcornxy == null || tcornxy.length < 8)
85+
return false;
86+
for (double v : tcornxy) {
87+
if (Math.abs(v) > 1e-3)
88+
return true; // some non-zero value
89+
}
9490
return false;
9591
}
9692

93+
/** Vertical & horizontal side lengths */
9794
private void calculate3DPosition() {
9895
double y0 = tcornxy[1], y1 = tcornxy[3], y2 = tcornxy[5], y3 = tcornxy[7];
9996
double x0 = tcornxy[0], x1 = tcornxy[2], x2 = tcornxy[4], x3 = tcornxy[6];
97+
10098
double hLeft = Math.abs(y3 - y0);
10199
double hRight = Math.abs(y2 - y1);
102100
double wTop = Math.abs(x2 - x3);
103101
double wBottom = Math.abs(x1 - x0);
102+
104103
Logger.recordOutput("Localization/hLeft", hLeft);
105104
Logger.recordOutput("Localization/hRight", hRight);
106105
Logger.recordOutput("Localization/wTop", wTop);
107106
Logger.recordOutput("Localization/wBottom", wBottom);
107+
108+
// System.out.printf("Heights: L=%.2f R=%.2f Widths: T=%.2f B=%.2f%n", hLeft,
109+
// hRight, wTop, wBottom);
108110
}
109111

112+
/** Approximates tag area in pixel² */
110113
private void calculateTagArea() {
111114
double x0 = tcornxy[0], y0 = tcornxy[1];
112115
double x1 = tcornxy[2], y1 = tcornxy[3];
@@ -125,8 +128,12 @@ private void calculateTagArea() {
125128
Logger.recordOutput("Localization/avgHeight", avgHeight);
126129
Logger.recordOutput("Localization/avgWidth", avgWidth);
127130
Logger.recordOutput("Localization/TagArea", area);
131+
132+
// System.out.printf("Tag pixel area: %.2f (avgW=%.2f avgH=%.2f)%n", area,
133+
// avgWidth, avgHeight);
128134
}
129135

136+
/** Calculates diagonals and midpoint */
130137
private void calculateDiagonalsAndMidpoint() {
131138
double x0 = tcornxy[0], y0 = tcornxy[1];
132139
double x1 = tcornxy[2], y1 = tcornxy[3];
@@ -142,102 +149,25 @@ private void calculateDiagonalsAndMidpoint() {
142149
Logger.recordOutput("Localization/Diagonal2", diag2);
143150
Logger.recordOutput("Localization/MidpointX", midX);
144151
Logger.recordOutput("Localization/MidpointY", midY);
145-
}
146-
147-
private double robotHeadingDeg = 0.0;
148-
public void setRobotHeadingDeg(double headingDeg) { this.robotHeadingDeg = headingDeg; }
149-
150-
private static double feetToMeters(double ft) { return ft * 0.3048; }
151-
152-
/** Compute distance from TA using average of min/max calibration ranges */
153-
private double estimateDistanceMetersFromTA(double taNow) {
154-
double[] ds_ft = {1, 3, 5, 7, 10};
155-
double[] tas = {
156-
avg(TA_1FT), avg(TA_3FT), avg(TA_5FT), avg(TA_7FT), avg(TA_10FT)
157-
};
158-
double[] ks = new double[5];
159-
int n = 0;
160152

161-
for (int i = 0; i < 5; i++) {
162-
double ta_i = Math.max(tas[i], MIN_TA);
163-
if (ta_i > MIN_TA)
164-
ks[n++] = feetToMeters(ds_ft[i]) * Math.sqrt(ta_i);
165-
}
166-
if (n == 0) return MAX_REASONABLE_METERS;
167-
168-
Arrays.sort(ks, 0, n);
169-
double kMed = (n % 2 == 1) ? ks[n / 2] : 0.5 * (ks[n / 2 - 1] + ks[n / 2]);
170-
double taSafe = Math.max(taNow, MIN_TA);
171-
double d = kMed / Math.sqrt(taSafe);
172-
if (Double.isNaN(d) || Double.isInfinite(d)) d = MAX_REASONABLE_METERS;
173-
return Math.min(d, MAX_REASONABLE_METERS);
153+
// System.out.printf("Diagonals: %.2f %.2f Midpoint: %.2f %.2f%n", diag1,
154+
// diag2, midX, midY);
174155
}
175156

176-
private static double avg(double[] range) { return (range[0] + range[1]) / 2.0; }
177-
178-
private static double normalizeDeg(double deg) {
179-
double a = deg % 360.0;
180-
if (a > 180.0) a -= 360.0;
181-
if (a < -180.0) a += 360.0;
182-
return a;
157+
// Accessors
158+
public double getTx() {
159+
return tx;
183160
}
184161

185-
private void computePlanFromVision() {
186-
double rangeM = estimateDistanceMetersFromTA(ta);
187-
double txRad = Math.toRadians(tx);
188-
double strafeM = rangeM * Math.tan(txRad);
189-
double headingErrDeg = normalizeDeg(0.0 - robotHeadingDeg);
190-
double forwardErrM = rangeM - DESIRED_RANGE_M;
191-
192-
boolean centered = Math.abs(tx) <= TX_TOLERANCE_DEG;
193-
boolean atRange = Math.abs(forwardErrM) <= 0.01;
194-
boolean facing = Math.abs(headingErrDeg) <= 2.0;
195-
196-
String rec;
197-
if (!atRange) rec = "MOVE";
198-
else if (!facing) rec = "ROTATE";
199-
else if (!centered) rec = "STRAFE";
200-
else rec = "ALIGNED";
201-
202-
currentPlan.forwardErrorMeters = forwardErrM;
203-
currentPlan.strafeErrorMeters = strafeM;
204-
currentPlan.headingErrorDeg = headingErrDeg;
205-
currentPlan.atRange = atRange;
206-
currentPlan.centered = centered;
207-
currentPlan.facing = facing;
208-
currentPlan.recommendation = rec;
162+
public double getTy() {
163+
return ty;
209164
}
210165

211-
private void logPlan(Plan p) {
212-
Logger.recordOutput("Align/forwardError_m", p.forwardErrorMeters);
213-
Logger.recordOutput("Align/strafeError_m", p.strafeErrorMeters);
214-
Logger.recordOutput("Align/headingError_deg", p.headingErrorDeg);
215-
Logger.recordOutput("Align/atRange", p.atRange);
216-
Logger.recordOutput("Align/centered", p.centered);
217-
Logger.recordOutput("Align/facing", p.facing);
218-
Logger.recordOutput("Align/recommendation", p.recommendation);
166+
public double getTa() {
167+
return ta;
219168
}
220169

221-
// ---- Manual range setters ----
222-
public void setCalibrationRanges(
223-
double[] ft1, double[] ft3, double[] ft5, double[] ft7, double[] ft10) {
224-
TA_1FT = ft1.clone();
225-
TA_3FT = ft3.clone();
226-
TA_5FT = ft5.clone();
227-
TA_7FT = ft7.clone();
228-
TA_10FT = ft10.clone();
229-
230-
Logger.recordOutput("Calibration/1ft_range", TA_1FT);
231-
Logger.recordOutput("Calibration/3ft_range", TA_3FT);
232-
Logger.recordOutput("Calibration/5ft_range", TA_5FT);
233-
Logger.recordOutput("Calibration/7ft_range", TA_7FT);
234-
Logger.recordOutput("Calibration/10ft_range", TA_10FT);
170+
public double[] getTcornxy() {
171+
return tcornxy;
235172
}
236-
237-
public Plan getPlan() { return currentPlan; }
238-
239-
public double getTx() { return tx; }
240-
public double getTy() { return ty; }
241-
public double getTa() { return ta; }
242-
public double[] getTcornxy() { return tcornxy; }
243173
}

0 commit comments

Comments
 (0)