From ad65dc07ceba2ccd1e7ff74f0a45f05e2cc0a2d7 Mon Sep 17 00:00:00 2001 From: TheRealGrayden Date: Thu, 9 Apr 2026 20:28:28 -0400 Subject: [PATCH] Zone check? I dunno, tired :3 --- src/main/java/frc/robot/Constants.java | 236 ++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 2 + .../{ZoneSwap.java => AnyTarget.java} | 4 +- .../java/frc/robot/commands/ZoneCheck.java | 79 ++++++ .../frc/robot/subsystems/DriveSubsystem.java | 15 +- 5 files changed, 216 insertions(+), 120 deletions(-) rename src/main/java/frc/robot/commands/{ZoneSwap.java => AnyTarget.java} (97%) create mode 100644 src/main/java/frc/robot/commands/ZoneCheck.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e0b4832..4fdb60e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,151 +1,167 @@ // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. - package frc.robot; +import com.ctre.phoenix6.signals.InvertedValue; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.units.Units; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.units.measure.Angle; -import com.ctre.phoenix6.signals.InvertedValue; - /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This class should not be used for any other + * purpose. All constants should be declared globally (i.e. public static). Do + * not put anything functional in this class. * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the constants are needed, to reduce verbosity. */ public final class Constants { - public static class OperatorConstants { - public static final int kDriverControllerPort = 0; - } - public static class CameraConstanst { + public static class OperatorConstants { - 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 kDriverControllerPort = 0; } - 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 class CameraConstanst { - public static final String FRONT_CAMERA_NAME = "limelight-front"; - public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0; - - } + public static class IDFilters { - public static class AdvantageKitConstants{ - public enum RobotType{ - SIM, - REAL, - REPLAY + 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 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 = "limelight-front"; + public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0; + public static final double[] BLUE_ZONE_COOR = new double[]{1}; + public static final double[] RED_ZONE_COOR = new double[]{1}; + public static final double[] NEUTRAL_ZONE_COOR = new double[]{1}; } - public static final RobotType CURRENT_MODE = RobotType.REAL; - } - // Swerve offsets as of 3/2/26 - public static class SwerveDriveOffsets { - public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f; - public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f; - public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f; - public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f; - } - /* + + public static class AdvantageKitConstants { + + public enum RobotType { + SIM, + REAL, + REPLAY + } + public static final RobotType CURRENT_MODE = RobotType.REAL; + } + // Swerve offsets as of 3/2/26 + + public static class SwerveDriveOffsets { + + public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f; + public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f; + public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f; + public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f; + } + + /* * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot * Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot * Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot * Laters numbers will be added for more subsystems * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon - */ - public static class SwerveDriveIDs { - public static final int FRONT_LEFT_STEER_ID = 11; - public static final int FRONT_LEFT_DRIVE_ID = 12; - public static final int FRONT_LEFT_ENCODER_ID = 13; - public static final int FRONT_RIGHT_STEER_ID = 21; - public static final int FRONT_RIGHT_DRIVE_ID = 22; - public static final int FRONT_RIGHT_ENCODER_ID = 23; - public static final int BACK_LEFT_STEER_ID = 31; - public static final int BACK_LEFT_DRIVE_ID = 32; - public static final int BACK_LEFT_ENCODER_ID = 33; - public static final int BACK_RIGHT_STEER_ID = 41; - public static final int BACK_RIGHT_DRIVE_ID = 42; - public static final int BACK_RIGHT_ENCODER_ID = 43; - } + */ + public static class SwerveDriveIDs { - public static class RobotChassisLimits{ - // Max velocity in meters per second - public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; - // In meters per second squared - public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; + public static final int FRONT_LEFT_STEER_ID = 11; + public static final int FRONT_LEFT_DRIVE_ID = 12; + public static final int FRONT_LEFT_ENCODER_ID = 13; + public static final int FRONT_RIGHT_STEER_ID = 21; + public static final int FRONT_RIGHT_DRIVE_ID = 22; + public static final int FRONT_RIGHT_ENCODER_ID = 23; + public static final int BACK_LEFT_STEER_ID = 31; + public static final int BACK_LEFT_DRIVE_ID = 32; + public static final int BACK_LEFT_ENCODER_ID = 33; + public static final int BACK_RIGHT_STEER_ID = 41; + public static final int BACK_RIGHT_DRIVE_ID = 42; + public static final int BACK_RIGHT_ENCODER_ID = 43; + } - // In radians per second - public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; - // In radians per second squared - public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI; - } + public static class RobotChassisLimits { + // Max velocity in meters per second - //Bird - public static class ExtraIDEntities { - public static final int PIGEON_ID = 1; - } - - public static class IntakeConstants { - public static final int DEPLOY_MOTOR_ID = 17; - public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this + public static final double MAX_ROBOT_LINEAR_VELOCITY = 3; + // In meters per second squared + public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2; - public static final int INTAKE_MOTOR_ID = 2; - public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this - } + // In radians per second + public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI; + // In radians per second squared + public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI; + } - public static class ShooterConstants { - public static final int LAUNCHER_MOTOR_ID = 3; - public static final boolean LAUNCHER_MOTOR_INVERTED = false; + //Bird + public static class ExtraIDEntities { - public static final int TURRET_MOTOR_ID = 4; - public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; - public static final int TURRET_LIMIT_CHANNEL = 9; - public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); - public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); - public static final double HOMING_SPEED = .35; - public static final double TURRET_GEAR_RATIO = 6; + public static final int PIGEON_ID = 1; + } - public static final int HOOD_SERVO_CHANNEL = 0; - 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); - public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); - public static final double DEFLECTOR_STORED_ANGLE = 0; - public static final double DEFLECTOR_SET_ANGLE1 = 100; - public static final double DEFLECTOR_SET_ANGLE2 = 200; - public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed - public static final double DEFLECTOR_SERVO_RATIO = 1.0; - } - - public static class HopperConstants { - public static final int KICKER_MOTOR_ID = 9; - public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; + public static class IntakeConstants { - public static final int SPINDEXER_MOTOR_ID = 8; - public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; - } + public static final int DEPLOY_MOTOR_ID = 17; + public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this - public static class FieldPoses { - private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + public static final int INTAKE_MOTOR_ID = 2; + public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this + } - public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); - public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + public static class ShooterConstants { - } + public static final int LAUNCHER_MOTOR_ID = 3; + public static final boolean LAUNCHER_MOTOR_INVERTED = false; + + public static final int TURRET_MOTOR_ID = 4; + public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final int TURRET_LIMIT_CHANNEL = 9; + public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); + public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); + public static final double HOMING_SPEED = .35; + public static final double TURRET_GEAR_RATIO = 6; + + public static final int HOOD_SERVO_CHANNEL = 0; + 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); + public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); + public static final double DEFLECTOR_STORED_ANGLE = 0; + public static final double DEFLECTOR_SET_ANGLE1 = 100; + public static final double DEFLECTOR_SET_ANGLE2 = 200; + public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed + public static final double DEFLECTOR_SERVO_RATIO = 1.0; + } + + public static class HopperConstants { + + public static final int KICKER_MOTOR_ID = 9; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; + + public static final int SPINDEXER_MOTOR_ID = 8; + public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + } + + public static class FieldPoses { + + private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4e970a3..f441446 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner; import frc.robot.commands.RelayCommand; import frc.robot.commands.SetDeflectorCommand; +import frc.robot.commands.ZoneCheck; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -83,6 +84,7 @@ public class RobotContainer { m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.b().toggleOnTrue(new ZoneCheck()); } /** diff --git a/src/main/java/frc/robot/commands/ZoneSwap.java b/src/main/java/frc/robot/commands/AnyTarget.java similarity index 97% rename from src/main/java/frc/robot/commands/ZoneSwap.java rename to src/main/java/frc/robot/commands/AnyTarget.java index 4f28ee1..9604e33 100644 --- a/src/main/java/frc/robot/commands/ZoneSwap.java +++ b/src/main/java/frc/robot/commands/AnyTarget.java @@ -21,7 +21,7 @@ import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.TurretSubsystem; -public class ZoneSwap extends Command { +public class AnyTarget extends Command { // For Chassis Control private DriveSubsystem driveSubsystem; @@ -40,7 +40,7 @@ public class ZoneSwap extends Command { private final double chassisRotateSpeed = Math.PI * 0.1; - public ZoneSwap(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) { + public AnyTarget(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) { this.driveSubsystem = driveSubsystem; this.hopperSubsystem = hopperSubsystem; this.turretSubsystem = turretSubsystem; diff --git a/src/main/java/frc/robot/commands/ZoneCheck.java b/src/main/java/frc/robot/commands/ZoneCheck.java new file mode 100644 index 0000000..62f5359 --- /dev/null +++ b/src/main/java/frc/robot/commands/ZoneCheck.java @@ -0,0 +1,79 @@ +package frc.robot.commands; + +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; + +public class ZoneCheck extends Command { + + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + private Zone zone; + + public enum Zone { + Blue, + Red, + Neutral; + } + + public ZoneCheck() { + getBotPose(); + } + + @Override + public void initialize() { + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + switch (zone) { + case Blue: + System.out.println("Im Home"); + case Neutral: + System.out.println("Uhhhhhhhh not red"); + ; + default: + System.out.println("BOOOOO, BOOOO RED"); + ; + } + } else { + switch (zone) { + case Blue: + System.out.println("Screw Blue"); + ; + case Neutral: + System.out.println("Uhhhhhhhh not red"); + ; + default: + System.out.println("Im Home"); + } + } + }); + } + + @Override + public void execute() { + + } + + @Override + public boolean isFinished() { + return false; + } + + private double[] getBotPose() { + return LimelightHelpers.getBotPose_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); + } + + private void getZone() { + if (getBotPose()[0] >= Constants.CameraConstanst.RED_ZONE_COOR[0]) { + zone = Zone.Red; + } else if (getBotPose()[0] >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) { + zone = Zone.Neutral; + } else { + zone = Zone.Blue; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d5066ca..85b5c8e 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -135,7 +135,6 @@ public class DriveSubsystem extends TestableSubsystem { .build()) .build(); - // Set vision measurement confidence values driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999)); @@ -166,7 +165,7 @@ public class DriveSubsystem extends TestableSubsystem { (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()), new PPHolonomicDriveController( new PIDConstants(1.5, 0, 0), - new PIDConstants(Math.PI/2, 0, 0)), + new PIDConstants(Math.PI / 2, 0, 0)), config, () -> { var alliance = DriverStation.getAlliance(); @@ -195,12 +194,12 @@ public class DriveSubsystem extends TestableSubsystem { Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d()); LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); - if(frontCameraPose.tagCount >= 1){ + 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 >= 1){ + if (backCameraPose.tagCount >= 1) { driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); } @@ -212,15 +211,15 @@ public class DriveSubsystem extends TestableSubsystem { driveSystem.simulationPeriodic(); } - public Command getAutoCommand(){ + public Command getAutoCommand() { return autoChooser.getSelected(); } - public void stopDrive(){ - driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); + public void stopDrive() { + driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); } - public void rotateChassis(double rotSpeed){ + public void rotateChassis(double rotSpeed) { driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero); }