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

Chassis Align improvements

This commit is contained in:
Tyler Wilcox 2026-03-25 22:02:50 -04:00
parent da438acf39
commit d6824df962
3 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/RoboLib

@ -1 +1 @@
Subproject commit aad75b9b559825aaf60ad5bb550bf8263d3d203d Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12

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

@ -83,8 +83,9 @@ public class AutoShootCommandChassisTurret extends Command{
List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList(); List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList();
Pose3d closest = botPose.nearest(tagPoses); Pose3d closest = botPose.nearest(tagPoses);
Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closest); Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closest);
Transform3d delta = botPose.minus(closest); Transform3d delta = closest.minus(botPose);
driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? -chassisRotateSpeed : chassisRotateSpeed); // May need to flip this Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
driveSubsystem.rotateChassis(delta.getRotation().toRotation2d().getDegrees() > 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
{ {

4
src/main/java/frc/robot/subsystems/DriveSubsystem.java
View File

@ -135,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem {
// Set vision measurement confidence values // Set vision measurement confidence values
// driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999));
this.setDefaultCommand(Commands.run(new Runnable() { this.setDefaultCommand(Commands.run(new Runnable() {
@ -146,8 +146,6 @@ public class DriveSubsystem extends TestableSubsystem {
-(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())), -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())),
-(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4)))); -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4))));
// driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0));
Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates()); Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates()); Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d()); Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());