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

Fix robot heading for AprilTagPoseEstimator

This commit is contained in:
Scott Barnes 2024-02-28 17:04:07 -06:00
parent deed5942d5
commit fb5c3174f1
1 changed files with 8 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/main/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimator.java
View File

@ -76,18 +76,17 @@ public class AprilTagPoseEstimator {
* @return The estimated pose of the robot. Heading is reported in radians. * @return The estimated pose of the robot. Heading is reported in radians.
*/ */
private Pose2d estimatePose(double yaw, double bearing, double range, double Tx, double Ty, double Ta, double Tb, double Tc, double Td) { 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 Tyaw = getTYaw(Ta, Tb, Tc, Td);
double cameraRotation = ((PI / 2) - aprilTagHeading + yaw); double ch = (PI / 2) - Tyaw + yaw;
double cx = Tx + range * cos(bearing) * cos(cameraRotation) + range * sin(bearing) * sin(cameraRotation); double cx = Tx + range * cos(bearing) * cos(ch) + range * sin(bearing) * sin(ch);
double cy = Ty + range * cos(bearing) * sin(cameraRotation) - range * sin(bearing) * cos(cameraRotation); double cy = Ty + range * cos(bearing) * sin(ch) - range * sin(bearing) * cos(ch);
double offsetX = this.robotOffset.position.x; double offsetX = this.robotOffset.position.x;
double offsetY = this.robotOffset.position.y; double offsetY = this.robotOffset.position.y;
double cameraRotationOnRobot = this.robotOffset.heading.toDouble(); double Cyaw = this.robotOffset.heading.toDouble();
double robotRotation = (PI / 2) + aprilTagHeading + yaw - cameraRotationOnRobot; double rh = (PI / 2) + Tyaw + yaw - Cyaw;
double rx = cx + offsetX * cos(robotRotation) - offsetY * sin(robotRotation); double rx = cx + offsetX * cos(rh) - offsetY * sin(rh);
double ry = cy + offsetX * sin(robotRotation) + offsetY * cos(robotRotation); double ry = cy + offsetX * sin(rh) + offsetY * cos(rh);
double rh = (yaw - cameraRotationOnRobot + 2 * PI) % (2 * PI);
return new Pose2d(rx, ry, rh); return new Pose2d(rx, ry, rh);
} }