Skip to content

Commit 72ff876

Browse files
GreenTomato5GreenTomato5
committed
random ahh ci
1 parent cd89b72 commit 72ff876

File tree

4 files changed

+373
-19
lines changed

4 files changed

+373
-19
lines changed

‎.github/workflows/ball-detection.yml‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
name: Ball
2+
3+
on:
4+
push:
5+
pull_request:
6+
7+
jobs:
8+
BallDetection:
9+
runs-on: ubuntu-latest
10+
11+
steps:
12+
- name: Checkout code
13+
uses: actions/checkout@v2
14+
15+
- name: Set up JDK
16+
uses: actions/setup-java@v1
17+
with:
18+
java-version: '17'
19+
20+
- name: Grant execute permission
21+
run: chmod +x gradlew
22+
23+
- name: Build robot code
24+
run: ./gradlew build
25+
26+
- name: Run Ball Detection Tests
27+
env:
28+
CI_NAME: "BallDetection"
29+
run: ./gradlew simulateJava | tee ball-detection.log
30+
31+
- name: Check Test Results
32+
run: |
33+
# Check for test failures
34+
if grep -qE "TEST.*FAILED|Exception|Error" ball-detection.log; then
35+
echo "Ball detection tests failed"
36+
cat ball-detection.log
37+
exit 1
38+
fi
39+
40+
# Check that all tests ran
41+
if grep -q "TEST A PASSED" ball-detection.log && \
42+
grep -q "TEST B PASSED" ball-detection.log && \
43+
grep -q "TEST C PASSED" ball-detection.log && \
44+
grep -q "TEST D PASSED" ball-detection.log; then
45+
echo "All ball detection tests passed"
46+
else
47+
echo "Not all tests completed successfully"
48+
cat ball-detection.log
49+
exit 1
50+
fi
51+
52+
- name: Upload test logs
53+
if: always()
54+
uses: actions/upload-artifact@v3
55+
with:
56+
name: ball-detection-logs
57+
path: ball-detection.log

‎src/main/java/frc/robot/Lib/BallDetectionTest.java‎

Lines changed: 277 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,277 @@
1+
// org/team7525/CI/BallDetectionTest.java
2+
3+
package frc.robot.Lib;
4+
5+
import edu.wpi.first.hal.HAL;
6+
import edu.wpi.first.math.geometry.*;
7+
import edu.wpi.first.wpilibj.IterativeRobotBase;
8+
import frc.robot.Subsystems.GamePieceFinder.GamePieceFinder;
9+
import org.photonvision.targeting.PhotonTrackedTarget;
10+
11+
public class BallDetectionTest extends IterativeRobotBase {
12+
13+
private GamePieceFinder finder;
14+
private int testsPassed = 0;
15+
private int testsFailed = 0;
16+
private long startTime;
17+
18+
// TODO: No magic numbers
19+
20+
public BallDetectionTest() {
21+
super(0.02);
22+
HAL.initialize(500, 0);
23+
}
24+
25+
@Override
26+
public void startCompetition() {
27+
System.out.println("********** Parallax Triangulation Tests **********\n");
28+
29+
finder = GamePieceFinder.getInstance();
30+
startTime = System.nanoTime();
31+
32+
// Test the actual ray intersection math
33+
testParallaxGeometry();
34+
35+
printResults();
36+
System.exit(testsFailed > 0 ? 1 : 0);
37+
}
38+
39+
private void testParallaxGeometry() {
40+
System.out.println("=== Testing Parallax Ray Intersection Math ===\n");
41+
42+
// Test Case 1: Ball straight ahead with robot moving sideways
43+
testCase1_StraightAhead();
44+
45+
// Test Case 2: Ball at an angle
46+
testCase2_AtAngle();
47+
48+
// Test Case 3: Large baseline (robot moved far between samples)
49+
testCase3_LargeBaseline();
50+
51+
// Test Case 4: Small angle difference (barely above threshold)
52+
testCase4_SmallAngle();
53+
}
54+
55+
private void testCase1_StraightAhead() {
56+
System.out.println("Test 1: Ball straight ahead, robot moves sideways");
57+
finder.clearPoseEstimates();
58+
59+
// Ball at (5, 0)
60+
Translation2d ballPos = new Translation2d(5.0, 0.0);
61+
62+
// Robot at (0, -1) looking at ball
63+
Pose2d pose1 = new Pose2d(0.0, -1.0, new Rotation2d());
64+
double yaw1 = calculateYaw(pose1, ballPos);
65+
66+
// Robot at (0, 1) looking at ball
67+
Pose2d pose2 = new Pose2d(0.0, 1.0, new Rotation2d());
68+
double yaw2 = calculateYaw(pose2, ballPos);
69+
70+
System.out.printf(" Sample 1: Robot at (%.1f, %.1f), Yaw = %.1f°\n",
71+
pose1.getX(), pose1.getY(), yaw1);
72+
System.out.printf(" Sample 2: Robot at (%.1f, %.1f), Yaw = %.1f°\n",
73+
pose2.getX(), pose2.getY(), yaw2);
74+
75+
// Area random bc uh i like dont have a formula for area that works and wouldnt sim that so yeah
76+
finder.setTestRobotPose(pose1);
77+
finder.addVisionSample(createTarget(yaw1, 5.0));
78+
79+
try { Thread.sleep(50); } catch (InterruptedException e) {}
80+
81+
finder.setTestRobotPose(pose2);
82+
finder.addVisionSample(createTarget(yaw2, 5.0));
83+
84+
finder.periodic();
85+
86+
evaluateResult(ballPos, "Test 1");
87+
}
88+
89+
private void testCase2_AtAngle() {
90+
System.out.println("\nTest 2: Ball at an angle");
91+
finder.clearPoseEstimates();
92+
93+
// Ball at (4, 2)
94+
Translation2d ballPos = new Translation2d(4.0, 2.0);
95+
96+
// Robot at (0, 0) rotated 30°
97+
Pose2d pose1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(30));
98+
double yaw1 = calculateYaw(pose1, ballPos);
99+
100+
// Robot at (1, -1) rotated 60°
101+
Pose2d pose2 = new Pose2d(1.0, -1.0, Rotation2d.fromDegrees(60));
102+
double yaw2 = calculateYaw(pose2, ballPos);
103+
104+
System.out.printf(" Sample 1: Robot at (%.1f, %.1f) @ %.0f°, Yaw = %.1f°\n",
105+
pose1.getX(), pose1.getY(), pose1.getRotation().getDegrees(), yaw1);
106+
System.out.printf(" Sample 2: Robot at (%.1f, %.1f) @ %.0f°, Yaw = %.1f°\n",
107+
pose2.getX(), pose2.getY(), pose2.getRotation().getDegrees(), yaw2);
108+
109+
finder.setTestRobotPose(pose1);
110+
finder.addVisionSample(createTarget(yaw1, 5.0));
111+
112+
try { Thread.sleep(50); } catch (InterruptedException e) {}
113+
114+
finder.setTestRobotPose(pose2);
115+
finder.addVisionSample(createTarget(yaw2, 5.0));
116+
117+
finder.periodic();
118+
119+
evaluateResult(ballPos, "Test 2");
120+
}
121+
122+
private void testCase3_LargeBaseline() {
123+
System.out.println("\nTest 3: Large baseline (good triangulation)");
124+
finder.clearPoseEstimates();
125+
126+
// Ball at (3, 3)
127+
Translation2d ballPos = new Translation2d(3.0, 3.0);
128+
129+
// Robot far left
130+
Pose2d pose1 = new Pose2d(-2.0, 0.0, Rotation2d.fromDegrees(45));
131+
double yaw1 = calculateYaw(pose1, ballPos);
132+
133+
// Robot far right
134+
Pose2d pose2 = new Pose2d(2.0, 0.0, Rotation2d.fromDegrees(135));
135+
double yaw2 = calculateYaw(pose2, ballPos);
136+
137+
System.out.printf(" Sample 1: Robot at (%.1f, %.1f) @ %.0f°, Yaw = %.1f°\n",
138+
pose1.getX(), pose1.getY(), pose1.getRotation().getDegrees(), yaw1);
139+
System.out.printf(" Sample 2: Robot at (%.1f, %.1f) @ %.0f°, Yaw = %.1f°\n",
140+
pose2.getX(), pose2.getY(), pose2.getRotation().getDegrees(), yaw2);
141+
142+
double baseline = pose1.getTranslation().getDistance(pose2.getTranslation());
143+
System.out.printf(" Baseline distance: %.2fm\n", baseline);
144+
145+
finder.setTestRobotPose(pose1);
146+
finder.addVisionSample(createTarget(yaw1, 5.0));
147+
148+
try { Thread.sleep(50); } catch (InterruptedException e) {}
149+
150+
finder.setTestRobotPose(pose2);
151+
finder.addVisionSample(createTarget(yaw2, 5.0));
152+
153+
finder.periodic();
154+
155+
evaluateResult(ballPos, "Test 3");
156+
}
157+
158+
private void testCase4_SmallAngle() {
159+
System.out.println("\nTest 4: Small angle difference (edge case)");
160+
finder.clearPoseEstimates();
161+
162+
// Ball at (6, 1)
163+
Translation2d ballPos = new Translation2d(6.0, 1.0);
164+
165+
// Robot moves slightly - should be right at the MIN_YAW_DIFFERENCE threshold
166+
Pose2d pose1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0));
167+
double yaw1 = calculateYaw(pose1, ballPos);
168+
169+
Pose2d pose2 = new Pose2d(0.0, 1.0, Rotation2d.fromDegrees(0));
170+
double yaw2 = calculateYaw(pose2, ballPos);
171+
172+
System.out.printf(" Sample 1: Yaw = %.1f°\n", yaw1);
173+
System.out.printf(" Sample 2: Yaw = %.1f°\n", yaw2);
174+
System.out.printf(" Yaw difference: %.1f° (threshold is 10°)\n", Math.abs(yaw2 - yaw1));
175+
176+
finder.setTestRobotPose(pose1);
177+
finder.addVisionSample(createTarget(yaw1, 5.0));
178+
179+
try { Thread.sleep(50); } catch (InterruptedException e) {}
180+
181+
finder.setTestRobotPose(pose2);
182+
finder.addVisionSample(createTarget(yaw2, 5.0));
183+
184+
finder.periodic();
185+
186+
evaluateResult(ballPos, "Test 4");
187+
}
188+
189+
// Calculate yaw angle from robot to ball (relative to robot frame)
190+
private double calculateYaw(Pose2d robotPose, Translation2d ballPos) {
191+
// Vector from robot to ball in global frame
192+
Translation2d robotToBall = ballPos.minus(robotPose.getTranslation());
193+
194+
// Global angle to ball
195+
double globalAngle = Math.atan2(robotToBall.getY(), robotToBall.getX());
196+
197+
// Convert to robot-relative angle
198+
double robotAngle = robotPose.getRotation().getRadians();
199+
double yawRad = globalAngle - robotAngle;
200+
201+
// Convert to degrees and normalize to [-180, 180]
202+
double yawDeg = Math.toDegrees(yawRad);
203+
while (yawDeg > 180) yawDeg -= 360;
204+
while (yawDeg < -180) yawDeg += 360;
205+
206+
return yawDeg;
207+
}
208+
209+
private PhotonTrackedTarget createTarget(double yaw, double area) {
210+
return new PhotonTrackedTarget(
211+
// just a ton of random stuff, yaw and area only useful
212+
yaw, // yaw
213+
0, // pitch (not used)
214+
area, // area (dummy value)
215+
0, // skew
216+
-1, // fiducialId
217+
-1, // i dont even know what this is
218+
1,
219+
null, // bestCameraToTarget
220+
null, // altCameraToTarget
221+
0, // poseAmbiguity
222+
null, // minAreaRectCorners
223+
null // detectedCorners
224+
);
225+
}
226+
227+
private void evaluateResult(Translation2d expectedBall, String testName) {
228+
Pose2d estimate = finder.getLatestGamepieceEstimate();
229+
230+
if (estimate == null) {
231+
System.out.printf(" %s FAILED: No estimate generated\n", testName);
232+
System.out.println("(Samples may not have passed MIN_YAW_DIFFERENCE threshold)\n");
233+
testsFailed++;
234+
return;
235+
}
236+
237+
Translation2d estimatedPos = estimate.getTranslation();
238+
double error = estimatedPos.getDistance(expectedBall);
239+
240+
System.out.printf(" Expected ball at: (%.2f, %.2f)\n",
241+
expectedBall.getX(), expectedBall.getY());
242+
System.out.printf(" Estimated ball at: (%.2f, %.2f)\n",
243+
estimatedPos.getX(), estimatedPos.getY());
244+
System.out.printf(" Error: %.3fm\n", error);
245+
246+
// Very tight tolerance since we're testing pure geometry
247+
boolean pass = error < 0.1; // 10cm tolerance for pure math
248+
249+
if (pass) {
250+
System.out.printf("%s PASSED\n\n", testName);
251+
testsPassed++;
252+
} else {
253+
System.out.printf("%s FAILED: Error too large (>0.1m)\n\n", testName);
254+
testsFailed++;
255+
}
256+
}
257+
258+
private void printResults() {
259+
double elapsed = (System.nanoTime() - startTime) / 1e9;
260+
System.out.println("========================================");
261+
System.out.println("Parallax Triangulation Test Results");
262+
System.out.println("========================================");
263+
System.out.printf("Tests Passed: %d\n", testsPassed);
264+
System.out.printf("Tests Failed: %d\n", testsFailed);
265+
System.out.printf("Time Elapsed: %.2fs\n", elapsed);
266+
System.out.println("========================================");
267+
268+
if (testsFailed == 0) {
269+
System.out.println("ALL TESTS PASSED - Parallax math is correct!");
270+
} else {
271+
System.out.println("SOME TESTS FAILED - Check ray intersection logic");
272+
}
273+
}
274+
275+
@Override
276+
public void endCompetition() {}
277+
}

‎src/main/java/frc/robot/Main.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,13 +5,15 @@
55
package frc.robot;
66

77
import edu.wpi.first.wpilibj.RobotBase;
8+
import frc.robot.Lib.BallDetectionTest;
9+
810
import org.team7525.CI.CrashCheck;
911

1012
public final class Main {
1113

1214
private Main() {}
1315

1416
public static void main(String... args) {
15-
RobotBase.startRobot("Crash".equals(System.getenv("CI_NAME")) ? () -> new CrashCheck(new Robot()) : Robot::new);
17+
RobotBase.startRobot("Ball".equals(System.getenv("CI_NAME")) ? () -> new BallDetectionTest() : Robot::new);
1618
}
1719
}

0 commit comments

Comments
 (0)