From a6449604ec6af1976632fd4609bd8b902609d350 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Mon, 23 Mar 2026 21:03:00 -0400 Subject: [PATCH] Auto Chassis align works --- src/main/java/frc/robot/Constants.java | 7 +++-- src/main/java/frc/robot/RobotContainer.java | 10 +++---- .../AutoShootCommandChassisTurret.java | 29 ++++++++++++++----- .../frc/robot/subsystems/DriveSubsystem.java | 11 +++++-- .../frc/robot/subsystems/TurretSubsystem.java | 9 ++---- 5 files changed, 42 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 34f7d79..a22dac4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,14 +26,15 @@ public final class Constants { public static class IDFilters { public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 }; public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 }; + // public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 18, 19, 20, 21 }; } - public static final String BACK_CAMERA_NAME = "back"; + public static final String BACK_CAMERA_NAME = "limelight-back"; public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0; public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1; public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2; - public static final String FRONT_CAMERA_NAME = "front"; + public static final String FRONT_CAMERA_NAME = "limelight-front"; public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0; } @@ -101,7 +102,7 @@ public final class Constants { public static final double TURRET_GEAR_RATIO = 1; public static final int HOOD_SERVO_CHANNEL = 0; - public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this + public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this public static final int HOOD_ENCODER_ID = 4; public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 08dd26a..1a2646d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,17 +58,17 @@ public class RobotContainer { */ private void configureBindings() { - turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID())); - hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, m_driverController.getHID())); - intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); + // turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID())); + // hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, m_driverController.getHID())); + // intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); - m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); - m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); + // m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); + // m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index c29196a..220ce48 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -4,9 +4,12 @@ import java.util.Arrays; import java.util.List; import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; @@ -41,15 +44,21 @@ public class AutoShootCommandChassisTurret extends Command{ this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); - autoCenterController = new PIDController(0.1, 0, 0); - autoCenterController.setTolerance(1); // Units is pixels + autoCenterController = new PIDController(0.1, 0, 0.0); + autoCenterController.setTolerance(0.2); // Units is pixels autoCenterController.setSetpoint(0); - - tagFilters = DriverStation.getAlliance().get() == Alliance.Blue ? Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS : Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; } @Override public void initialize() { + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; + } else { + tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; + } + }); + turretSubsystem.setLauncher(2500); hopperSubsystem.spindexerOn(); @@ -63,8 +72,11 @@ public class AutoShootCommandChassisTurret extends Command{ public void execute() { // Need to compute shoot speed and angle + Logger.recordOutput("AutoAlign-CenteringError", autoCenterController.getError()); + // 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"); Pose3d botPose = driveSubsystem.getBotPose(); // Find closest target to us List tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> filedTags.getTagPose(tagId).get()).toList(); @@ -77,14 +89,17 @@ public class AutoShootCommandChassisTurret extends Command{ } else // We have an april tag, center it to the camera frame { + Logger.recordOutput("AutoAlignState", "Center on tag"); // [tx, ty, tz, pitch, yaw, roll] double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); - - driveSubsystem.rotateChassis(autoCenterController.calculate(targetsPose[0])); // This may need to be negated + if(targetsPose.length >= 1){ + driveSubsystem.rotateChassis(-autoCenterController.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(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){ + Logger.recordOutput("AutoAlignState", "FIRE!"); hopperSubsystem.feedKicker(); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index b8a831c..d16224d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -15,6 +15,7 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -134,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem { // Set vision measurement confidence values - driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999)); + // driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); this.setDefaultCommand(Commands.run(new Runnable() { @@ -191,13 +192,17 @@ public class DriveSubsystem extends TestableSubsystem { super.periodic(); driveSystem.periodic(); + Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d()); + LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); - if(frontCameraPose.tagCount >= 2){ + if(frontCameraPose.tagCount >= 1){ driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); + Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); } LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); - if(backCameraPose.tagCount >= 2){ + if(backCameraPose.tagCount >= 1){ driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); + Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 185f357..6e93d9f 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -27,6 +27,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; +import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DigitalInput; @@ -106,12 +107,8 @@ public class TurretSubsystem extends TestableSubsystem { public void setTargetElevation(Angle elev) { - if(Constants.ShooterConstants.HOOD_SERVO_INVERTED){ - elev=Units.Degrees.of(300).minus(elev); - } - elev=elev.div(1.5); - elev=elev.div(Constants.ShooterConstants.DEFLECTOR_SERVO_RATIO); - hood.setAngle(elev.in(Units.Degrees)); + double angle = elev.in(Units.Degrees); + hood.setAngle(300/angle); } public Command revSpeedCommand()