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

Merge branch 'improve-chassis-align'

This commit is contained in:
Tyler Wilcox 2026-03-27 20:52:18 -04:00
parent 36eaabacf1 65e25ee9e3
commit 6f9fa74052
1 changed files with 35 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -11,7 +11,14 @@ import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
@ -28,12 +35,15 @@ public class AutoShootCommandChassisTurret extends Command{
private HopperSubsystem hopperSubsystem; private HopperSubsystem hopperSubsystem;
private TurretSubsystem turretSubsystem; private TurretSubsystem turretSubsystem;
private PIDController autoCenterController; private PIDController cameraAutoCenterController;
private PIDController deadreckoningAutoCenterController;
private int[] tagFilters; private int[] tagFilters;
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
private Pose3d closestTag;
private final double chassisRotateSpeed = Math.PI * 0.1; private final double chassisRotateSpeed = Math.PI * 0.1;
public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem)
@ -44,9 +54,14 @@ public class AutoShootCommandChassisTurret extends Command{
this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
autoCenterController = new PIDController(0.1, 0, 0.0); cameraAutoCenterController = new PIDController(0.1, 0, 0.0);
autoCenterController.setTolerance(0.2); // Units is pixels cameraAutoCenterController.setTolerance(0.2); // Units is meters
autoCenterController.setSetpoint(0); cameraAutoCenterController.setSetpoint(0);
deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0);
deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations
deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI);
} }
@Override @Override
@ -61,31 +76,36 @@ public class AutoShootCommandChassisTurret extends Command{
turretSubsystem.setLauncher(2500); turretSubsystem.setLauncher(2500);
hopperSubsystem.spindexerOn(); hopperSubsystem.spindexerOn();
autoCenterController.calculate(-100); cameraAutoCenterController.calculate(-100);
// Change to HUB tracking pipeline // Change to HUB tracking pipeline
LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER); LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER);
LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters); LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
Pose3d botPose = driveSubsystem.getBotPose();
// Find closest target to us
List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList();
closestTag = botPose.nearest(tagPoses);
Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closestTag);
Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
deadreckoningAutoCenterController.setSetpoint(delta.getAngle().getRadians());
} }
@Override @Override
public void execute() { public void execute() {
// Need to compute shoot speed and angle // Need to compute shoot speed and angle
Logger.recordOutput("AutoAlign-CenteringError", autoCenterController.getError()); Logger.recordOutput("AutoAlign-CenteringError", cameraAutoCenterController.getError());
// No april tag in camera view. Rotate towards our theoretical closest tag // No april tag in camera view. Rotate towards our theoretical closest tag
if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
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 driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // 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 = closest.minus(botPose);
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
{ {
@ -93,12 +113,12 @@ public class AutoShootCommandChassisTurret extends Command{
// [tx, ty, tz, pitch, yaw, roll] // [tx, ty, tz, pitch, yaw, roll]
double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(targetsPose.length >= 1){ if(targetsPose.length >= 1){
driveSubsystem.rotateChassis(-autoCenterController.calculate(targetsPose[0])); // This may need to be negated driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
} }
} }
// If robot is centered on HUB and shooter is up to speed. Run the kicker // If robot is centered on HUB and shooter is up to speed. Run the kicker
if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){ if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){
Logger.recordOutput("AutoAlignState", "FIRE!"); Logger.recordOutput("AutoAlignState", "FIRE!");
hopperSubsystem.feedKicker(); hopperSubsystem.feedKicker();
} }