Skip to content

Commit d4fefa5

Browse files
ExultantExaltedExultantExalted
authored
Merge pull request #44 from svhsrobotics/apriltag
AprilTag Robot Pose calculation and Camera Switching
2 parents d211f53 + 373c120 commit d4fefa5

File tree

2 files changed

+143
-86
lines changed

2 files changed

+143
-86
lines changed

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ApriltagTest.java

Lines changed: 128 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,9 @@
66
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
77
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
88

9+
import org.firstinspires.ftc.robotcore.external.ClassFactory;
910
import org.firstinspires.ftc.robotcore.external.Telemetry;
11+
import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName;
1012
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
1113
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
1214
import org.firstinspires.ftc.vision.VisionPortal;
@@ -18,102 +20,157 @@
1820

1921
@Autonomous(name = "AprilTag Test")
2022
public class ApriltagTest extends LinearOpMode {
21-
@Override
22-
public void runOpMode() throws InterruptedException {
23-
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
24-
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
23+
private VisionPortal visionPortal;
24+
private AprilTagProcessor aprilTag;
2525

26-
AprilTagProcessor aprilTag = new AprilTagProcessor.Builder().build();
27-
VisionPortal.Builder builder = new VisionPortal.Builder();
28-
builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"));
29-
builder.addProcessor(aprilTag);
30-
VisionPortal visionPortal = builder.build();
26+
/**
27+
* The array of webcams that will be used in the pipeline
28+
*/
29+
private WebcamName[] webcams;
3130

3231

32+
/**
33+
* Transform the angle into the first quadrant
34+
*/
35+
private double quadrantTransform(double angle) {
36+
if (angle < 0 || angle > 360) {
37+
throw new IllegalArgumentException("Angle must be between 0 and 360");
38+
}
3339

34-
waitForStart();
35-
36-
while (opModeIsActive()) {
37-
//drive.setPoseEstimate(new Pose2d(0, 0, 0));
38-
//drive.update();
39-
List<AprilTagDetection> currentDetections = aprilTag.getDetections();
40-
telemetry.addData("# AprilTags Detected", currentDetections.size());
41-
42-
for (AprilTagDetection detection : currentDetections) {
43-
if (detection.metadata != null) {
44-
//double theta_need = detection.ftcPose.yaw + detection.ftcPose.bearing;
45-
//double theta_need_rad = Math.toRadians(theta_need);
46-
telemetry.addData("Range", detection.ftcPose.range);
47-
//telemetry.addData("Theta Need", theta_need);
48-
telemetry.addData("Yaw", detection.ftcPose.yaw);
49-
telemetry.addData("Bearing", detection.ftcPose.bearing);
50-
telemetry.addData("X, Y", detection.ftcPose.x + ", " + detection.ftcPose.y);
51-
double thetaNeed = detection.ftcPose.yaw - detection.ftcPose.bearing;
52-
telemetry.addData("Theta Need", thetaNeed);
53-
double a = detection.ftcPose.range * Math.cos(Math.toRadians(thetaNeed));
54-
double b = detection.ftcPose.range * Math.sin(Math.toRadians(thetaNeed));
55-
telemetry.addData("a", a);
56-
telemetry.addData("b", b);
57-
//double absolutex = detection.metadata.fieldPosition.get(0) + a; // + for lower side
58-
//double absolutey = detection.metadata.fieldPosition.get(1) - b; // - for lower side
40+
if (angle < 90) {
41+
return angle;
42+
} else if (angle < 180) {
43+
return 180 - angle;
44+
} else if (angle < 270) {
45+
return angle - 180;
46+
} else {
47+
return 360 - angle;
48+
}
49+
}
5950

60-
//Vector2d absVector = new Vector2d(detection.metadata.fieldPosition.get(0) + a, detection.metadata.fieldPosition.get(1) - b);
61-
//absVector.rotated(detection.metadata.fieldOrientation.toOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.RADIANS).thirdAngle);
51+
/**
52+
* Switch to the camera at the given index in `webcams`
53+
* @param cameraIndex the index of the camera to switch to
54+
*/
55+
private void switchCamera(int cameraIndex) {
56+
// Wait for the visionPortal status to be STREAMING before we attempt to switch cameras
57+
android.util.Log.i("AprilTag Test", "Waiting for camera to start STREAMING before switching cameras");
58+
while (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
59+
sleep(10);
60+
}
61+
android.util.Log.i("AprilTag Test", "Camera is STREAMING, switching cameras...");
62+
// Get the current camera
63+
CameraName currentCamera = visionPortal.getActiveCamera();
64+
// Find the index of the current camera in the array
65+
int currentCameraIndex = -1;
66+
for (int i = 0; i < webcams.length; i++) {
67+
if (webcams[i] == currentCamera) {
68+
currentCameraIndex = i;
69+
break;
70+
}
71+
}
72+
// If the current camera is not the one we want to switch to, switch to it
73+
if (currentCameraIndex != cameraIndex) {
74+
visionPortal.setActiveCamera(webcams[cameraIndex]);
75+
// Flush any detections from the previous camera
76+
aprilTag.getFreshDetections();
77+
}
78+
}
6279

63-
//int[] NEED_TO_ROTATE = {1,2,3,4,5,6};
64-
List<Integer> NEED_TO_ROTATE = Arrays.asList(1,2,3,4,5,6);
80+
private Pose2d estimateCameraPoseFromAprilTags(int camera) {
81+
// Switch to the camera
82+
switchCamera(camera);
83+
// Get the detections
84+
List<AprilTagDetection> detections = aprilTag.getFreshDetections();
85+
// Wait for detections to not be null
86+
// TODO: Timeout
87+
while (detections == null) {
88+
detections = aprilTag.getFreshDetections();
89+
}
90+
// If there are no detections, return null
91+
// TODO: Multiple tries?
92+
if (detections.size() == 0) {
93+
return null;
94+
}
95+
// Get the first detection
96+
// TODO: Pick the best detection?
97+
AprilTagDetection detection = detections.get(0);
98+
99+
double thetaNeed = detection.ftcPose.yaw - detection.ftcPose.bearing;
100+
double a = detection.ftcPose.range * Math.cos(Math.toRadians(thetaNeed));
101+
double b = detection.ftcPose.range * Math.sin(Math.toRadians(thetaNeed));
102+
103+
List<Integer> NEED_TO_ROTATE = Arrays.asList(1, 2, 3, 4, 5, 6);
104+
105+
double absX, absY, absRot;
106+
if (NEED_TO_ROTATE.contains(detection.id)) {
107+
absX = detection.metadata.fieldPosition.get(0) - a; // + for lower side
108+
absY = detection.metadata.fieldPosition.get(1) + b; // - for lower side
109+
absRot = 180 - detection.ftcPose.yaw + 180;
110+
} else {
111+
absX = detection.metadata.fieldPosition.get(0) + a; // + for lower side
112+
absY = detection.metadata.fieldPosition.get(1) - b; // - for lower side
113+
absRot = 180 - detection.ftcPose.yaw;
114+
}
65115

66-
double absX, absY, absRot;
67-
if (NEED_TO_ROTATE.contains(detection.id)) {
68-
absX = detection.metadata.fieldPosition.get(0) - a; // + for lower side
69-
absY = detection.metadata.fieldPosition.get(1) + b; // - for lower side
70-
absRot = 180-detection.ftcPose.yaw + 180;
116+
absRot = absRot % 360;
71117

72-
} else {
73-
absX = detection.metadata.fieldPosition.get(0) + a; // + for lower side
74-
absY = detection.metadata.fieldPosition.get(1) - b; // - for lower side
75-
absRot = 180-detection.ftcPose.yaw;
76-
}
118+
return new Pose2d(absX, absY, Math.toRadians(absRot));
119+
}
120+
@Override
121+
public void runOpMode() throws InterruptedException {
122+
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
123+
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
77124

125+
aprilTag = new AprilTagProcessor.Builder().build();
78126

127+
// Add the cameras to the array
128+
webcams = new WebcamName[3];
129+
webcams[0] = hardwareMap.get(WebcamName.class, "Webcam 1");
130+
webcams[1] = hardwareMap.get(WebcamName.class, "Webcam 2");
131+
webcams[2] = hardwareMap.get(WebcamName.class, "Webcam 3");
79132

80-
//telemetry.addData("First Angle", detection.metadata.fieldOrientation.toOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle);
81-
//telemetry.addData("Second Angle", detection.metadata.fieldOrientation.toOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).secondAngle );
82-
//telemetry.addData("Third Angle", detection.metadata.fieldOrientation.toOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle);
133+
CameraName switchableCamera = ClassFactory.getInstance()
134+
.getCameraManager().nameForSwitchableCamera(webcams);
83135

136+
// Create the vision portal by using a builder.
137+
visionPortal = new VisionPortal.Builder()
138+
.setCamera(switchableCamera)
139+
.addProcessor(aprilTag)
140+
.build();
84141

142+
android.util.Log.i("AprilTag Test", "Waiting for start...");
143+
switchCamera(1);
144+
waitForStart();
85145

86-
//telemetry.addData("position x, position y", absolutex + "," + absolutey);
146+
while(opModeIsActive()) {
147+
Pose2d cameraPose = estimateCameraPoseFromAprilTags(1);
87148

149+
if (cameraPose != null) {
150+
android.util.Log.i("AprilTag Test", "Camera pose: " + cameraPose);
88151

89-
telemetry.addData("Field Centric Deg", 180 - detection.ftcPose.yaw);
152+
double CAMERA_OFFSET = 10; // Center camera
153+
double CAMERA_ANGLE = Math.toRadians(50);
90154

91-
Pose2d absPose = new Pose2d(absX, absY, Math.toRadians(absRot));
155+
double offsetX = CAMERA_OFFSET * Math.cos(cameraPose.getHeading());
156+
double offsetY = CAMERA_OFFSET * Math.sin(cameraPose.getHeading());
92157

93-
//Vector2d vec = new Vector2d(absolutex, absolutey).rotated(180);
94-
//Pose2d poseFlipped = new Pose2d(vec.getX(),vec.getY(), Math.toRadians(-detection.ftcPose.yaw));
158+
android.util.Log.i("AprilTag Test", "Camera offset: " + offsetX + ", " + offsetY);
95159

96-
drive.setPoseEstimate(absPose);
97-
//drive.update();
160+
double robotX = cameraPose.getX() - offsetX;
161+
double robotY = cameraPose.getY() - offsetY;
162+
double robotHeading = cameraPose.getHeading() - CAMERA_ANGLE;
98163

164+
Pose2d robotPose = new Pose2d(robotX, robotY, robotHeading);
99165

100-
//telemetry.addData("Relative X", detection.ftcPose.range * Math.cos(Math.toRadians(detection.ftcPose.yaw)));
101-
//telemetry.addData("Relative Y", detection.ftcPose.range * Math.sin(Math.toRadians(detection.ftcPose.yaw)));
166+
android.util.Log.i("AprilTag Test", "Robot pose: " + robotPose);
102167

103-
//Orientation.getOrientation()
104-
//telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
105-
//telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
168+
drive.setPoseEstimate(robotPose);
106169

107-
//telemetry.addLine(String.format("Field XYZ %6.1f %6.1f %6.1f (inch)", detection.metadata.fieldPosition.get(0), detection.metadata.fieldPosition.get(1), detection.metadata.fieldPosition.get(2)));
108-
}
109-
//telemetry.update();
170+
drive.update();
110171
}
111-
112-
drive.update();
113-
114172
}
115173

116174
visionPortal.close();
117-
118175
}
119-
}
176+
}

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RedLeftAuto.java

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -21,30 +21,30 @@ public void runOpMode() throws InterruptedException {
2121
TrajectorySequence autoDrive = drive.trajectorySequenceBuilder(startPose)
2222

2323
//relativistic driving that also includes turning to simulate april tags recognition
24-
.forward(24)
25-
.waitSeconds(1)
26-
.turn(Math.toRadians(90))
27-
.waitSeconds(3)
28-
.turn(Math.toRadians(-180))
29-
.forward(80)
30-
.build();
24+
// .forward(24)
25+
// .waitSeconds(1)
26+
// .turn(Math.toRadians(90))
27+
// .waitSeconds(3)
28+
// .turn(Math.toRadians(-180))
29+
// .forward(80)
30+
// .build();
31+
3132

3233

3334

3435

35-
/*
3636
//coordinate based driving
3737
.lineTo(new Vector2d(-34, -36))
3838
.strafeTo(new Vector2d(48, -36))
3939
.build();
40-
*/
41-
/*
40+
41+
4242
// also coordinate but spline and goes to corner instead of right in front of board
43-
.splineTo(new Vector2d(-34, -30), Math.toRadians(90))
44-
.splineTo(new Vector2d(34, -30), Math.toRadians(-90))
45-
.splineTo(new Vector2d(56, -56),Math.toRadians(0) )
46-
.build();
47-
*/
43+
// .splineTo(new Vector2d(-34, -30), Math.toRadians(90))
44+
// .splineTo(new Vector2d(34, -30), Math.toRadians(-90))
45+
// .splineTo(new Vector2d(56, -56),Math.toRadians(0) )
46+
// .build();
47+
//
4848

4949
waitForStart();
5050

0 commit comments

Comments
 (0)