FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Maybe fix swerve odemetry

This commit is contained in:
Tyler Wilcox 2026-03-23 22:06:05 -04:00
parent a6449604ec
commit ea3ff7afb5
2 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit ad53b44e2fb4c06b72ff2546b30e37446ac2fb8d Subproject commit 8eb0a76b2646235f173cd524fbf7462256ab1696

14
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -32,7 +32,7 @@ public class AutoShootCommandChassisTurret extends Command{
private int[] tagFilters; private int[] tagFilters;
private AprilTagFieldLayout filedTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
private final double chassisRotateSpeed = Math.PI * 0.1; private final double chassisRotateSpeed = Math.PI * 0.1;
@ -79,13 +79,11 @@ public class AutoShootCommandChassisTurret extends Command{
Logger.recordOutput("AutoAlignState", "Look for tag"); Logger.recordOutput("AutoAlignState", "Look for tag");
Pose3d botPose = driveSubsystem.getBotPose(); Pose3d botPose = driveSubsystem.getBotPose();
// Find closest target to us // Find closest target to us
List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> filedTags.getTagPose(tagId).get()).toList(); List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList();
for(int i=0;i<tagFilters.length; i++) Pose3d closest = botPose.nearest(tagPoses);
{ Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closest);
Pose3d closet = botPose.nearest(tagPoses); Transform3d delta = botPose.minus(closest);
Transform3d delta = botPose.minus(closet); driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? chassisRotateSpeed : -chassisRotateSpeed); // May need to flip this
driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? chassisRotateSpeed : -chassisRotateSpeed); // May need to flip this
}
} }
else // We have an april tag, center it to the camera frame else // We have an april tag, center it to the camera frame
{ {