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