eaglesfe
/
ielib
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit

This commit is contained in:
Scott Barnes 2024-02-17 16:20:02 -06:00
commit fc13916f0b
13 changed files with 642 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/build

80
build.gradle Normal file
View File

@ -0,0 +1,80 @@
import java.text.SimpleDateFormat
buildscript {
repositories {
mavenCentral()
google()
}
dependencies {
classpath 'com.android.tools.build:gradle:8.2.1'
}
}
plugins {
id 'com.android.library'
id 'maven-publish'
id 'signing'
}
java {
sourceCompatibility = JavaVersion.VERSION_1_8
targetCompatibility = JavaVersion.VERSION_1_8
}
group = 'com.tearabite.ftctearabits'
version = '1.0'
sourceSets {
main {
java { srcDirs = ["src/java"] }
resources { srcDir "src/resources" }
}
}
android {
namespace = 'com.tearabite.ftctearabits'
defaultConfig {
minSdkVersion 24
productFlavors {
biscuit {
aarMetadata {
minCompileSdk 24
}
}
}
}
testFixtures {
enable true
}
}
publishing {
publications {
mavenJava(MavenPublication) {
groupId = 'com.tearabite.ftctearabits'
artifactId = 'ftctearabits'
version = '1.0'
}
}
}
signing {
sign publishing.publications.mavenJava
}
repositories {
mavenCentral()
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
implementation 'org.firstinspires.ftc:RobotCore:9.0.1'
implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta6'
implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta6'
implementation 'org.firstinspires.ftc:Vision:9.0.1'
testImplementation 'org.junit.jupiter:junit-jupiter:5.9.1'
compileOnly 'org.projectlombok:lombok:1.18.30'
annotationProcessor 'org.projectlombok:lombok:1.18.30'
}

21
proguard-rules.pro vendored Normal file
View File

@ -0,0 +1,21 @@
# Add project specific ProGuard rules here.
# You can control the set of applied configuration files using the
# proguardFiles setting in build.gradle.
#
# For more details, see
# http://developer.android.com/guide/developing/tools/proguard.html
# If your project uses WebView with JS, uncomment the following
# and specify the fully qualified class name to the JavaScript interface
# class:
#-keepclassmembers class fqcn.of.javascript.interface.for.webview {
# public *;
#}
# Uncomment this to preserve the line number information for
# debugging stack traces.
#-keepattributes SourceFile,LineNumberTable
# If you keep the line number information, uncomment this to
# hide the original source file name.
#-renamesourcefileattribute SourceFile

11
src/main/AndroidManifest.xml Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- Note: the actual manifest file used in your APK merges this file with contributions
from the modules on which your app depends (such as FtcRobotController, etc).
So it won't ultimately be as empty as it might here appear to be :-) -->
<!-- The package name here determines the package for your R class and your BuildConfig class -->
<manifest
xmlns:android="http://schemas.android.com/apk/res/android">
<application/>
</manifest>

4
src/main/java/com/tearabite/ftctearabits/Alliance.java Normal file
View File

@ -0,0 +1,4 @@
package com.tearabite.ftctearabits;
public enum Alliance { Blue, Red }

82
src/main/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimator.java Normal file
View File

@ -0,0 +1,82 @@
package com.tearabite.ftctearabits.localization;
import static java.lang.Math.PI;
import static java.lang.Math.asin;
import static java.lang.Math.atan2;
import static java.lang.Math.cos;
import static java.lang.Math.pow;
import static java.lang.Math.sin;
import static java.lang.Math.tan;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Vector2d;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
import java.security.InvalidParameterException;
public class AprilTagPoseEstimator {
private final Pose2d robotOffset;
public AprilTagPoseEstimator() {
this(new Pose2d(new Vector2d(0, 0), 0));
}
public AprilTagPoseEstimator(Pose2d robotOffset) {
this.robotOffset = robotOffset;
}
public Pose2d estimatePose(AprilTagDetection detection) {
if (detection == null || detection.metadata == null || detection.metadata.fieldPosition == null || detection.ftcPose == null) {
throw new InvalidParameterException();
}
AprilTagPoseFtc ftcPose = detection.ftcPose;
VectorF fieldPosition = detection.metadata.fieldPosition;
Quaternion fieldOrientation = detection.metadata.fieldOrientation;
return estimatePose(
Math.toRadians(ftcPose.yaw),
Math.toRadians(ftcPose.bearing),
ftcPose.range,
fieldPosition.get(0),
fieldPosition.get(1),
fieldOrientation.x,
fieldOrientation.y,
fieldOrientation.z,
fieldOrientation.w);
}
private Pose2d estimatePose(double yaw, double bearing, double range, double Tx, double Ty, double Ta, double Tb, double Tc, double Td) {
double aprilTagHeading = getTYaw(Ta, Tb, Tc, Td);
double cameraRotation = ((PI / 2) - aprilTagHeading + yaw);
double cx = Tx + range * cos(bearing) * cos(cameraRotation) + range * sin(bearing) * sin(cameraRotation);
double cy = Ty + range * cos(bearing) * sin(cameraRotation) - range * sin(bearing) * cos(cameraRotation);
double aprilTagX = this.robotOffset.position.x;
double aprilTagY = this.robotOffset.position.y;
double cameraRotationOnRobot = this.robotOffset.heading.toDouble();
double robotRotation = (PI / 2) + aprilTagHeading + yaw - cameraRotationOnRobot;
double rx = cx + aprilTagX * cos(robotRotation) - aprilTagY * sin(robotRotation);
double ry = cy + aprilTagX * sin(robotRotation) + aprilTagY * cos(robotRotation);
double rh = tan(yaw - cameraRotationOnRobot);
return new Pose2d(rx, ry, rh);
}
private static double getTPitch(double a, double b, double c, double d) {
return asin(2 * ((a * d) - (b * c)));
}
private static double getTRoll(double a, double b, double c, double d) {
return atan2(2 * ((a * c) + (b * d)), 1 - 2 * (pow(c, 2) + pow(d, 2)));
}
private static double getTYaw(double a, double b, double c, double d) {
return atan2(2 * ((a * b) + (c * d)), 1 - 2 * ((pow(b, 2) + pow(c, 2))));
}
}

79
src/main/java/com/tearabite/ftctearabits/vision/BasicColorDetectionVisionProcessor.java Normal file
View File

@ -0,0 +1,79 @@
package com.tearabite.ftctearabits.vision;
import static com.tearabite.ftctearabits.vision.Colors.WHITE;
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getLargestContour;
import android.graphics.Canvas;
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
import org.firstinspires.ftc.vision.VisionProcessor;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import lombok.Getter;
public class BasicColorDetectionVisionProcessor implements VisionProcessor {
public static final Size BLUR_SIZE = new Size(7, 7);
public static final int ERODE_DILATE_ITERATIONS = 2;
public static final Mat STRUCTURING_ELEMENT = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
public static final Point ANCHOR = new Point((STRUCTURING_ELEMENT.cols() / 2f), STRUCTURING_ELEMENT.rows() / 2f);
private final Mat blurred = new Mat();
private final ScalarRange[] colorRanges;
@Getter private Detection detection;
private final Mat hsv = new Mat();
private final double ignoreSmallerThan;
private final double ignoreLargerThan;
private final Mat mask = new Mat();
private final Mat tmpMask = new Mat();
public BasicColorDetectionVisionProcessor(double ignoreSmallerThan, double ignoreLargerThan, ScalarRange... colorRanges) {
this.ignoreSmallerThan = ignoreSmallerThan;
this.ignoreLargerThan = ignoreLargerThan;
this.colorRanges = colorRanges;
}
@Override
public void init(int width, int height, CameraCalibration calibration) {
this.detection = new Detection(new Size(width, height), ignoreSmallerThan, ignoreLargerThan);
}
@Override
public Object processFrame(Mat input, long captureTimeNanos) {
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
mask.release();
for (ScalarRange colorRange : this.colorRanges) {
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask);
if (mask.empty() || mask.rows() <= 0) {
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask);
}
Core.add(mask, tmpMask, mask);
}
Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
ArrayList<MatOfPoint> contours = new ArrayList<>();
Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
detection.setContour(getLargestContour(contours));
return input;
}
@Override
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
if (detection != null && detection.isValid()) {
Point center = detection.getCenterPx();
canvas.drawCircle((float) center.x, (float) center.y, 10, WHITE);
}
}
}

21
src/main/java/com/tearabite/ftctearabits/vision/Colors.java Normal file
View File

@ -0,0 +1,21 @@
package com.tearabite.ftctearabits.vision;
import android.graphics.Color;
import org.opencv.core.Scalar;
public class Colors {
public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);
public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255);
public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER);
public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2]));
public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80);
public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255);
public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER);
public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40);
public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255);
public static OpenCVUtil.LinePaint RED = new OpenCVUtil.LinePaint(Color.RED);
public static OpenCVUtil.LinePaint BLUE = new OpenCVUtil.LinePaint(Color.BLUE);
public static OpenCVUtil.LinePaint BLACK = new OpenCVUtil.LinePaint(Color.BLACK);
public static OpenCVUtil.LinePaint WHITE = new OpenCVUtil.LinePaint(Color.WHITE);
}

15
src/main/java/com/tearabite/ftctearabits/vision/Constants.java Normal file
View File

@ -0,0 +1,15 @@
package com.tearabite.ftctearabits.vision;
import org.opencv.core.Scalar;
public class Constants {
public static Scalar RED = new Scalar(255, 0, 0);
public static Scalar GREEN = new Scalar(0, 255, 0);
public static Scalar BLUE = new Scalar(0, 0, 255);
public static Scalar WHITE = new Scalar(255, 255, 255);
public static Scalar GRAY = new Scalar(80, 80, 80);
public static Scalar BLACK = new Scalar(0, 0, 0);
public static Scalar ORANGE = new Scalar(255, 165, 0);
public static Scalar YELLOW = new Scalar(255, 255, 0);
public static Scalar PURPLE = new Scalar(128, 0, 128);
}

133
src/main/java/com/tearabite/ftctearabits/vision/Detection.java Normal file
View File

@ -0,0 +1,133 @@
package com.tearabite.ftctearabits.vision;
import static com.tearabite.ftctearabits.vision.Constants.GREEN;
import static com.tearabite.ftctearabits.vision.OpenCVUtil.drawConvexHull;
import static com.tearabite.ftctearabits.vision.OpenCVUtil.drawPoint;
import static com.tearabite.ftctearabits.vision.OpenCVUtil.fillConvexHull;
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getBottomLeftOfContour;
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getBottomRightOfContour;
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getCenterOfContour;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
// Class for a Detection
public class Detection {
public static final Point INVALID_POINT = new Point(Double.MIN_VALUE, Double.MIN_VALUE);
public static final double INVALID_AREA = -1;
public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0);
private double minAreaPx;
private double maxAreaPx;
private final Size maxSizePx;
private double areaPx = INVALID_AREA;
private Point centerPx = INVALID_POINT;
private Point bottomLeftPx = INVALID_POINT;
private Point bottomRightPx = INVALID_POINT;
private MatOfPoint contour;
// Constructor
public Detection(Size frameSize, double minAreaFactor) {
this.maxSizePx = frameSize;
this.minAreaPx = frameSize.area() * minAreaFactor;
this.maxAreaPx = frameSize.area();
}
public Detection(Size frameSize, double minAreaFactor, double maxAreaFactor) {
this.maxSizePx = frameSize;
this.minAreaPx = frameSize.area() * minAreaFactor;
this.maxAreaPx = frameSize.area() * maxAreaFactor;
}
public void setMinArea(double minAreaFactor) {
this.minAreaPx = maxSizePx.area() * minAreaFactor;
}
public void setMaxArea(double maxAreaFactor) {
this.minAreaPx = maxSizePx.area() * maxAreaFactor;
}
// Draw a convex hull around the current detection on the given image
public void draw(Mat img, Scalar color) {
if (isValid()) {
drawConvexHull(img, contour, color);
drawPoint(img, centerPx, GREEN);
}
}
// Draw a convex hull around the current detection on the given image
public void fill(Mat img, Scalar color) {
if (isValid()) {
fillConvexHull(img, contour, color);
drawPoint(img, centerPx, GREEN);
}
}
// Check if the current Detection is valid
public boolean isValid() {
return (this.contour != null) && (this.areaPx != INVALID_AREA);
}
// Get the current contour
public MatOfPoint getContour() {
return contour;
}
// Set the values of the current contour
public void setContour(MatOfPoint contour) {
this.contour = contour;
double area;
if (contour != null && (area = Imgproc.contourArea(contour)) > minAreaPx && area < maxAreaPx) {
this.areaPx = area;
this.centerPx = getCenterOfContour(contour);
this.bottomLeftPx = getBottomLeftOfContour(contour);
this.bottomRightPx = getBottomRightOfContour(contour);
} else {
this.areaPx = INVALID_AREA;
this.centerPx = INVALID_POINT;
this.bottomLeftPx = INVALID_POINT;
this.bottomRightPx = INVALID_POINT;
}
}
// Returns the center of the Detection, normalized so that the width and height of the frame is from [-50,50]
public Point getCenter() {
if (!isValid()) {
return INVALID_POINT;
}
double normalizedX = ((centerPx.x / maxSizePx.width) * 100) - 50;
double normalizedY = ((centerPx.y / maxSizePx.height) * -100) + 50;
return new Point(normalizedX, normalizedY);
}
// Get the center point in pixels
public Point getCenterPx() {
return centerPx;
}
// Get the area of the Detection, normalized so that the area of the frame is 100
public double getArea() {
if (!isValid()) {
return INVALID_AREA;
}
return (areaPx / (maxSizePx.width * maxSizePx.height)) * 100;
}
// Get the leftmost bottom corner of the detection
public Point getBottomLeftCornerPx() {
return bottomLeftPx;
}
// Get the rightmost bottom corner of the detection
public Point getBottomRightCornerPx() {
return bottomRightPx;
}
}

102
src/main/java/com/tearabite/ftctearabits/vision/OpenCVUtil.java Normal file
View File

@ -0,0 +1,102 @@
package com.tearabite.ftctearabits.vision;
import android.graphics.Paint;
import org.opencv.core.Mat;
import org.opencv.core.MatOfInt;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
import java.util.Collections;
import java.util.List;
// CV Helper Functions
public class OpenCVUtil {
public static String telem = "nothing";
// Draw a point
public static void drawPoint(Mat img, Point point, Scalar color) {
Imgproc.circle(img, point, 3, color, -1);
}
// Get the center of a contour
public static Point getCenterOfContour(MatOfPoint contour) {
Moments moments = Imgproc.moments(contour);
return new Point(moments.m10 / moments.m00, moments.m01/ moments.m00);
}
// Get the bottom left of a contour
public static Point getBottomLeftOfContour(MatOfPoint contour) {
Rect boundingRect = Imgproc.boundingRect(contour);
return new Point(boundingRect.x, boundingRect.y+boundingRect.height);
}
// Get the bottom right of a contour
public static Point getBottomRightOfContour(MatOfPoint contour) {
Rect boundingRect = Imgproc.boundingRect(contour);
return new Point(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height);
}
// Draw a contour
public static void drawContour(Mat img, MatOfPoint contour, Scalar color) {
Imgproc.drawContours(img, Collections.singletonList(contour), 0, color, 2);
}
// Draw a convex hull around a contour
public static void drawConvexHull(Mat img, MatOfPoint contour, Scalar color) {
MatOfInt hull = new MatOfInt();
Imgproc.convexHull(contour, hull);
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, 2);
}
// Draw a filled in convex hull around a contour
public static void fillConvexHull(Mat img, MatOfPoint contour, Scalar color) {
MatOfInt hull = new MatOfInt();
Imgproc.convexHull(contour, hull);
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, -1);
}
// Convert indexes to points that is used in order to draw the contours
public static MatOfPoint convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
int[] arrIndex = indexes.toArray();
Point[] arrContour = contour.toArray();
Point[] arrPoints = new Point[arrIndex.length];
for (int i=0;i<arrIndex.length;i++) {
arrPoints[i] = arrContour[arrIndex[i]];
}
MatOfPoint hull = new MatOfPoint();
hull.fromArray(arrPoints);
return hull;
}
// Get the largest contour out of a list
public static MatOfPoint getLargestContour(List<MatOfPoint> contours) {
if (contours.size() == 0) {
return null;
}
return getLargestContours(contours, 1).get(0);
}
// Get the top largest contours
public static List<MatOfPoint> getLargestContours(List<MatOfPoint> contours, int numContours) {
Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a));
return contours.subList(0, Math.min(numContours, contours.size()));
}
public static class LinePaint extends Paint
{
public LinePaint(int color)
{
setColor(color);
setAntiAlias(true);
setStrokeCap(Paint.Cap.ROUND);
}
}
}

13
src/main/java/com/tearabite/ftctearabits/vision/ScalarRange.java Normal file
View File

@ -0,0 +1,13 @@
package com.tearabite.ftctearabits.vision;
import org.opencv.core.Scalar;
import lombok.AllArgsConstructor;
import lombok.Data;
@Data
@AllArgsConstructor
public class ScalarRange {
private Scalar upper;
private Scalar lower;
}

80
src/test/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimatorTest.java Normal file
View File

@ -0,0 +1,80 @@
package com.tearabite.ftctearabits.localization;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.fail;
import com.acmerobotics.roadrunner.Pose2d;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagMetadata;
import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
import java.security.InvalidParameterException;
import java.util.stream.Stream;
class AprilTagPoseEstimatorTest {
@Test
public void estimatePose_null_throws() {
AprilTagPoseEstimator estimator = new AprilTagPoseEstimator();
assertThrows(InvalidParameterException.class, () -> estimator.estimatePose(null));
}
@ParameterizedTest
@MethodSource("provideEstimatePosTestValues")
public void estimatePos_notNull_returnsPose(AprilTagMetadata metadata, AprilTagPoseFtc poseFtc, Pose2d expectedPose) {
AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(new Pose2d(-7.77, 0.505, 0));
AprilTagDetection detection = new AprilTagDetection(1, 0, 0, null, null, metadata, poseFtc, null, 0);
Pose2d estimatedPose = estimator.estimatePose(detection);
assertIsClose(estimatedPose, expectedPose);
}
private boolean isClose(double a, double b) {
return Math.abs(a - b) < 0.1;
}
private void assertIsClose(Pose2d a, Pose2d b) {
boolean isClose = isClose(a.position.x, b.position.x)
&& isClose(a.position.y, b.position.y)
&& isClose(a.heading.toDouble(), b.heading.toDouble());
if (!isClose) {
fail(String.format("Expected (%.1f, %.1f, %.1f) to be close to (%.1f, %.1f, %.1f)",
a.position.x, a.position.y, a.heading.toDouble(),
b.position.x, b.position.y, b.heading.toDouble()));
}
}
private static Stream<Arguments> provideEstimatePosTestValues() {
final AprilTagMetadata metadata = new AprilTagMetadata(
2,
"testTag",
0,
new VectorF(60.25f, 35.41f, 4f), DistanceUnit.INCH,
new Quaternion(0.3536f, -0.6124f, 0.6124f, -0.3536f, 0));
return Stream.of(
Arguments.of(
metadata,
new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0),
new Pose2d(28.5, 35.9, 0)),
Arguments.of(
metadata,
new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, -45, 0),
new Pose2d(35.5, 18.9, 0)),
Arguments.of(
metadata,
new AprilTagPoseFtc(0, 0, 0, -45, 0, 0, 24, -45, 0),
new Pose2d(31.1, 41.3, -1))
);
}
}