Skip to content

Commit 48efaeb

Browse files
HutoubenHutouben
authored
Create AprilTagPatternAuto.java
1 parent 61eadc5 commit 48efaeb

File tree

1 file changed

+386
-0
lines changed

1 file changed

+386
-0
lines changed

‎AprilTagPatternAuto.java‎

Lines changed: 386 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,386 @@
1+
2+
/*
3+
Hello :D my names mikey and i'm the head of software on team 21721. I was looking at the april tag sample code on the PP (pedro pathing) website and it kinda confused me or just wasn't
4+
what I needed to do, so I decided to make my own! Before you worry about the code itself u need to know a bit about April tags. April tags are basically just QR codes; in the sense
5+
that when you scan them they give u a numerical value. the april tag values for this season are as the following-
6+
7+
Blue Goal: 20
8+
Motif GPP: 21
9+
Motif PGP: 22
10+
Motif PPG: 23
11+
Red Goal: 24
12+
13+
So basically, you lineup your robot in front of the motif april tag. It scans said April Tag and then gives you a value back. You then have three if/then statements where you pretty much
14+
say "if the numeric value is 21, then run the GPP pathbuilder" and so on. Right now, though, the code just has movement. So whenever you get your shooting and intake mechanisms figured out, just add that code in the
15+
designated function and call the function in whichever part of the pathbuilder it is needed. I hope this helps!
16+
*/
17+
18+
19+
package org.firstinspires.ftc.teamcode.examples;
20+
21+
// FTC SDK
22+
23+
import com.bylazar.configurables.annotations.Configurable;
24+
import com.bylazar.telemetry.PanelsTelemetry;
25+
import com.bylazar.telemetry.TelemetryManager;
26+
import com.pedropathing.follower.Follower;
27+
import com.pedropathing.geometry.BezierLine;
28+
import com.pedropathing.geometry.Pose;
29+
import com.pedropathing.paths.PathChain;
30+
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
31+
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
32+
import com.qualcomm.robotcore.util.ElapsedTime;
33+
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
34+
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
35+
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
36+
import org.firstinspires.ftc.vision.VisionPortal;
37+
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
38+
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
39+
40+
import java.util.List;
41+
42+
@Autonomous(name = "April Tag with PP test", group = "Opmode")
43+
@Configurable // Panels
44+
@SuppressWarnings("FieldCanBeLocal") // Stop Android Studio from bugging about variables being predefined
45+
public class AprilTagPatternAuto extends LinearOpMode {
46+
// Initialize elapsed timer
47+
private final ElapsedTime runtime = new ElapsedTime();
48+
49+
// Initialize poses
50+
private final Pose startPose = new Pose(72, 120, Math.toRadians(90)); // Start Pose of our robot.
51+
private final Pose scorePose = new Pose(72, 20, Math.toRadians(115)); // Scoring Pose of our robot. It is facing the goal at a 115 degree angle.
52+
private final Pose PPGPose = new Pose(100, 83.5, Math.toRadians(0)); // Highest (First Set) of Artifacts from the Spike Mark.
53+
private final Pose PGPPose = new Pose(100, 59.5, Math.toRadians(0)); // Middle (Second Set) of Artifacts from the Spike Mark.
54+
private final Pose GPPPose = new Pose(100, 35.5, Math.toRadians(0)); // Lowest (Third Set) of Artifacts from the Spike Mark.
55+
56+
// Initialize variables for paths
57+
58+
private PathChain grabPPG;
59+
private PathChain scorePPG;
60+
private PathChain grabPGP;
61+
private PathChain scorePGP;
62+
private PathChain grabGPP;
63+
private PathChain scoreGPP;
64+
65+
66+
//set April Tag values to specific patterns
67+
private static final int PPG_TAG_ID = 23;
68+
private static final int PGP_TAG_ID = 22;
69+
private static final int GPP_TAG_ID = 21;
70+
private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
71+
private VisionPortal visionPortal; // Used to manage the video source.
72+
private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
73+
private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
74+
75+
76+
// Other variables
77+
private Pose currentPose; // Current pose of the robot
78+
private Follower follower; // Pedro Pathing follower
79+
private TelemetryManager panelsTelemetry; // Panels telemetry
80+
private int pathStatePPG; // Current state machine value
81+
private int pathStatePGP; // Current state machine value
82+
private int pathStateGPP; // Current state machine value
83+
84+
private int foundID; // Current state machine value, dictates which one to run
85+
86+
87+
88+
// Custom logging function to support telemetry and Panels
89+
private void log(String caption, Object... text) {
90+
if (text.length == 1) {
91+
telemetry.addData(caption, text[0]);
92+
panelsTelemetry.debug(caption + ": " + text[0]);
93+
} else if (text.length >= 2) {
94+
StringBuilder message = new StringBuilder();
95+
for (int i = 0; i < text.length; i++) {
96+
message.append(text[i]);
97+
if (i < text.length - 1) message.append(" ");
98+
}
99+
telemetry.addData(caption, message.toString());
100+
panelsTelemetry.debug(caption + ": " + message);
101+
}
102+
}
103+
104+
// a place to put your intake and shooting functions
105+
public void intakeArtifacts() {
106+
// Put your intake logic/functions here
107+
}
108+
109+
public void shootArtifacts() {
110+
// Put your shooting logic/functions here
111+
}
112+
113+
114+
@Override
115+
public void runOpMode() {
116+
// Initialize Panels telemetry
117+
panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry();
118+
119+
// Initialize Pedro Pathing follower
120+
follower = Constants.createFollower(hardwareMap);
121+
follower.setStartingPose(startPose);
122+
123+
boolean targetFound = false; // Set to true when an AprilTag target is detected
124+
initAprilTag();
125+
126+
if (USE_WEBCAM) {
127+
setManualExposure(6, 250); // Use low exposure time to reduce motion blur
128+
}
129+
130+
// Log completed initialization to Panels and driver station (custom log function)
131+
log("Status", "Initialized");
132+
telemetry.update(); // Update driver station after logging
133+
134+
// Wait for the game to start (driver presses START)
135+
waitForStart();
136+
runtime.reset();
137+
138+
setpathStatePPG(0);
139+
setpathStatePGP(0);
140+
setpathStateGPP(0);
141+
runtime.reset();
142+
143+
while (opModeIsActive()) {
144+
// Update Pedro Pathing and Panels every iteration
145+
follower.update();
146+
panelsTelemetry.update();
147+
currentPose = follower.getPose(); // Update the current pose
148+
targetFound = false;
149+
desiredTag = null;
150+
151+
// Step through the list of detected tags and look for a matching tag
152+
List<AprilTagDetection> currentDetections = aprilTag.getDetections();
153+
for (AprilTagDetection detection : currentDetections) {
154+
// Look to see if we have size info on this tag.
155+
if (detection.metadata != null) {
156+
// Check to see if we want to track towards this tag.
157+
if (detection.id == PPG_TAG_ID) {
158+
// call lines for the PGP pattern
159+
buildPathsPPG();
160+
targetFound = true;
161+
desiredTag = detection;
162+
foundID = 21; // This should likely be PPG_TAG_ID or the corresponding state machine ID
163+
break; // don't look any further.
164+
} else if (detection.id == PGP_TAG_ID) {
165+
// call lines for the PGP pattern
166+
buildPathsPGP();
167+
targetFound = true;
168+
desiredTag = detection;
169+
foundID = 22; // This should likely be PGP_TAG_ID or the corresponding state machine ID
170+
break; // don't look any further.
171+
172+
} else if (detection.id == GPP_TAG_ID) {
173+
// call lines for the GPP pattern
174+
buildPathsGPP();
175+
targetFound = true;
176+
desiredTag = detection;
177+
foundID = 23; // This should likely be GPP_TAG_ID or the corresponding state machine ID
178+
break; // don't look any further.
179+
}
180+
} else {
181+
// This tag is NOT in the library, so we don't have enough information to track to it.
182+
telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
183+
}
184+
}
185+
186+
187+
// Update the state machine
188+
if (foundID == 21) { // Consider using the TAG_ID constants or a dedicated variable for which path was found
189+
updateStateMachinePPG();
190+
} else if (foundID == 22) {
191+
updateStateMachinePGP();
192+
} else if (foundID == 23) {
193+
updateStateMachineGPP();
194+
}
195+
196+
197+
// Log to Panels and driver station (custom log function)
198+
log("Elapsed", runtime.toString());
199+
log("X", currentPose.getX());
200+
log("Y", currentPose.getY());
201+
log("Heading", currentPose.getHeading());
202+
telemetry.update(); // Update the driver station after logging
203+
}
204+
}
205+
206+
207+
public void buildPathsPPG() {
208+
// basically just plotting the points for the lines that score the PPG pattern
209+
210+
211+
grabPPG = follower.pathBuilder() //
212+
.addPath(new BezierLine(startPose, PPGPose))
213+
.setLinearHeadingInterpolation(startPose.getHeading(), PPGPose.getHeading())
214+
.build();
215+
216+
// Move to the scoring pose from the first artifact pickup pose
217+
scorePPG = follower.pathBuilder()
218+
.addPath(new BezierLine(PPGPose, scorePose))
219+
.setLinearHeadingInterpolation(PPGPose.getHeading(), scorePose.getHeading())
220+
.build();
221+
}
222+
223+
public void buildPathsPGP() {
224+
// basically just plotting the points for the lines that score the PGP pattern
225+
226+
// Move to the first artifact pickup pose from the start pose
227+
grabPGP = follower.pathBuilder() // Changed from scorePGP to grabPGP
228+
.addPath(new BezierLine(startPose, PGPPose))
229+
.setLinearHeadingInterpolation(startPose.getHeading(), PGPPose.getHeading())
230+
.build();
231+
232+
// Move to the scoring pose from the first artifact pickup pose
233+
scorePGP = follower.pathBuilder()
234+
.addPath(new BezierLine(PGPPose, scorePose))
235+
.setLinearHeadingInterpolation(PGPPose.getHeading(), scorePose.getHeading())
236+
.build();
237+
}
238+
239+
public void buildPathsGPP() {
240+
// basically just plotting the points for the lines that score the GPP pattern
241+
242+
// Move to the first artifact pickup pose from the start pose
243+
grabGPP = follower.pathBuilder()
244+
.addPath(new BezierLine(startPose, GPPPose))
245+
.setLinearHeadingInterpolation(startPose.getHeading(), GPPPose.getHeading())
246+
.build();
247+
248+
// Move to the scoring pose from the first artifact pickup pose
249+
scoreGPP = follower.pathBuilder()
250+
.addPath(new BezierLine(GPPPose, scorePose))
251+
.setLinearHeadingInterpolation(GPPPose.getHeading(), scorePose.getHeading())
252+
.build();
253+
}
254+
255+
//below is the state machine or each pattern
256+
257+
public void updateStateMachinePPG() {
258+
switch (pathStatePPG) {
259+
case 0:
260+
// Move to the scoring position from the start position
261+
follower.followPath(grabPPG);
262+
setpathStatePPG(1); // Call the setter method
263+
break;
264+
case 1:
265+
// Wait until we have passed all path constraints
266+
if (!follower.isBusy()) {
267+
268+
// Move to the first artifact pickup location from the scoring position
269+
follower.followPath(scorePPG);
270+
setpathStatePPG(-1); //set it to -1 so it stops the state machine execution
271+
}
272+
break;
273+
}
274+
}
275+
276+
277+
public void updateStateMachinePGP() {
278+
switch (pathStatePGP) {
279+
case 0:
280+
// Move to the scoring position from the start position
281+
follower.followPath(grabPGP);
282+
setpathStatePGP(1); // Call the setter method
283+
break;
284+
case 1:
285+
// Wait until we have passed all path constraints
286+
if (!follower.isBusy()) {
287+
288+
// Move to the first artifact pickup location from the scoring position
289+
follower.followPath(scorePGP);
290+
setpathStatePGP(-1); // Call the setter for PGP
291+
}
292+
break;
293+
}
294+
}
295+
296+
297+
public void updateStateMachineGPP() {
298+
switch (pathStateGPP) {
299+
case 0:
300+
// Move to the scoring position from the start position
301+
follower.followPath(grabGPP);
302+
setpathStateGPP(1); // Call the setter method
303+
break;
304+
case 1:
305+
// Wait until we have passed all path constraints
306+
if (!follower.isBusy()) {
307+
308+
// Move to the first artifact pickup location from the scoring position
309+
follower.followPath(scoreGPP);
310+
setpathStateGPP(-1); //set it to -1 so it stops the state machine execution
311+
}
312+
break;
313+
}
314+
}
315+
316+
// Setter methods for pathState variables placed at the class level
317+
void setpathStatePPG(int newPathState) {
318+
this.pathStatePPG = newPathState;
319+
}
320+
321+
void setpathStatePGP(int newPathState) {
322+
this.pathStatePGP = newPathState;
323+
}
324+
325+
void setpathStateGPP(int newPathState) {
326+
this.pathStateGPP = newPathState;
327+
}
328+
329+
330+
/**
331+
* start the AprilTag processor.
332+
*/
333+
private void initAprilTag() {
334+
// Create the AprilTag processor by using a builder.
335+
aprilTag = new AprilTagProcessor.Builder().build();
336+
337+
// Adjust Image Decimation to trade-off detection-range for detection-rate.
338+
aprilTag.setDecimation(2);
339+
340+
// Create the vision portal by using a builder.
341+
if (USE_WEBCAM) {
342+
visionPortal = new VisionPortal.Builder()
343+
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
344+
.addProcessor(aprilTag)
345+
.build();
346+
} else {
347+
visionPortal = new VisionPortal.Builder()
348+
.setCamera(BuiltinCameraDirection.BACK)
349+
.addProcessor(aprilTag)
350+
.build();
351+
}
352+
}
353+
354+
/*
355+
Manually set the camera gain and exposure.
356+
This can only be called AFTER calling initAprilTag(), and only works for Webcams;
357+
*/
358+
private void setManualExposure(int exposureMS, int gain) {
359+
// Wait for the camera to be open, then use the controls
360+
361+
if (visionPortal == null) {
362+
return;
363+
}
364+
365+
// Make sure camera is streaming before we try to set the exposure controls
366+
if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
367+
telemetry.addData("Camera", "Waiting");
368+
telemetry.update();
369+
while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
370+
sleep(20);
371+
}
372+
telemetry.addData("Camera", "Ready");
373+
telemetry.update();
374+
}
375+
// ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
376+
// if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
377+
// exposureControl.setMode(ExposureControl.Mode.Manual);
378+
// sleep(50);
379+
// }
380+
// exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
381+
// sleep(20);
382+
// GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
383+
// gainControl.setGain(gain);
384+
// sleep(20);
385+
}
386+
}

0 commit comments

Comments
 (0)