From f2415195ce9462f3232fba3bac086875545f48ca Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 11 Feb 2026 15:18:12 -0700 Subject: [PATCH] Implement field positions, tolerance, LED states --- .../java/frc4388/robot/RobotContainer.java | 18 +++-- .../robot/constants/BuildConstants.java | 16 ++--- .../frc4388/robot/constants/Constants.java | 19 +++-- .../robot/subsystems/climber/Climber.java | 27 ++------ .../robot/subsystems/shooter/Shooter.java | 69 ++++++++++++++++--- .../subsystems/shooter/ShooterConstants.java | 14 ++-- .../utility/compute/FieldPositions.java | 21 ++++++ 7 files changed, 125 insertions(+), 59 deletions(-) create mode 100644 src/main/java/frc4388/utility/compute/FieldPositions.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2682829..9a6b70e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,7 @@ import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.FieldPositions; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.controller.DeadbandedXboxController; // Autos @@ -112,24 +113,29 @@ public class RobotContainer { configureButtonBindings(); - DeferredBlock.addBlock(() -> { // Called on first robot enable + // Called on first robot enable + DeferredBlock.addBlock(() -> { m_robotSwerveDrive.resetGyro(); }, false); - DeferredBlock.addBlock(() -> { // Called on every robot enable + + // Called on every robot enable + DeferredBlock.addBlock(() -> { TimesNegativeOne.update(); + FieldPositions.update(); }, true); DriverStation.silenceJoystickConnectionWarning(true); + // Drive normally m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // Drive normally m_robotSwerveDrive.driveWithInput( getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(),true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); + m_robotSwerveDrive.setToSlow(); makeAutoChooser(); @@ -196,6 +202,7 @@ public class RobotContainer { m_robotShooter.io.updateGains(); })); + // IF the driver is holding the left trigger, intake driving new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) .whileTrue(new RunCommand( @@ -205,9 +212,6 @@ public class RobotContainer { false ); }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> , m_robotSwerveDrive)); - - .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Extended); })) @@ -222,7 +226,7 @@ public class RobotContainer { () -> { m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), - FieldConstants.BLUE_HUB_POS); + FieldPositions.HUB_POSITION); }, m_robotSwerveDrive) ) .onTrue(new InstantCommand(() -> { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 764bf0b..abf86f9 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,15 +5,15 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 38; - public static final String GIT_SHA = "6ce6d0eb0b8faec5af448ec23ffca156303cbed5"; - public static final String GIT_DATE = "2026-02-09 19:38:55 MST"; - public static final String GIT_BRANCH = "arm-test"; - public static final String BUILD_DATE = "2026-02-10 17:12:07 MST"; - public static final long BUILD_UNIX_TIME = 1770768727439L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 40; + public static final String GIT_SHA = "983b95fdc704ef35b6f5e2dada2c6348f3c67190"; + public static final String GIT_DATE = "2026-02-10 19:42:47 MST"; + public static final String GIT_BRANCH = "shoot-button"; + public static final String BUILD_DATE = "2026-02-11 12:41:33 MST"; + public static final long BUILD_UNIX_TIME = 1770838893524L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 4c461a8..9ff5234 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -98,13 +98,20 @@ public final class Constants { public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; - public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; - public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; - public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; + // LED color for when the intake is out + public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED; + // LED color for when the intake is out, but the driver conditions are bad + public static final LEDPatterns INTAKE_OUT_BADPHYS = LEDPatterns.RED_STROBE; - public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; - public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; + // LED color for when the flywheel speed isn't in tolarance + public static final LEDPatterns BAD_FLYWEEL = LEDPatterns.SOLID_GOLD; + // LED color for when the flywheel speed isn't in tolarance, but the driver conditions are bad + public static final LEDPatterns BAD_FLYWEEL_BADPHYS = LEDPatterns.GOLD_STROBE; + + // Operator ready to shoot + public static final LEDPatterns OPREADY = LEDPatterns.SOLID_WHITE; + // Operator ready to shoot, but the driver conditions are bad + public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/subsystems/climber/Climber.java b/src/main/java/frc4388/robot/subsystems/climber/Climber.java index f667f1b..78c06eb 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/climber/Climber.java @@ -1,7 +1,5 @@ package frc4388.robot.subsystems.climber; -import static edu.wpi.first.units.Units.Rotation; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -9,20 +7,19 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.status.FaultReporter; public class Climber extends SubsystemBase { ClimberIO io; ClimberStateAutoLogged state = new ClimberStateAutoLogged(); - Supplier m_swervePoseSupplier; + // Supplier m_swervePoseSupplier; public Climber( - ClimberIO io, - Supplier swervePoseSupplier + ClimberIO io + // Supplier swervePoseSupplier ) { this.io = io; - this.m_swervePoseSupplier = swervePoseSupplier; + // this.m_swervePoseSupplier = swervePoseSupplier; } // public enum ClimberMode { @@ -44,19 +41,6 @@ public class Climber extends SubsystemBase { // } - // public enum FieldZone { - // // The robot should aim at the hub - // InShootZone, - // // The robot should aim towards the wall - // AimAtWall, - - // } - - // // Calculate what should be done based off of the position of the robot - // // TODO: Implement field zones - // public FieldZone getTarget(Pose2d position) { - // return FieldZone.InShootZone; - // } @Override public void periodic() { @@ -68,9 +52,6 @@ public class Climber extends SubsystemBase { Logger.processInputs("Climber", state); - Pose2d pose = m_swervePoseSupplier.get(); - Angle robotRot = pose.getRotation().getMeasure(); - io.updateInputs(state); } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 35e2501..44c9b2c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -2,15 +2,21 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; +import java.text.FieldPosition; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.utility.compute.FieldPositions; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -57,10 +63,6 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.NotReady; - // public void setMode(ShooterMode mode) { - // this.mode = mode; - // } - public void setShooterReady() { if(this.mode == ShooterMode.NotReady) { this.mode = ShooterMode.Ready; @@ -86,14 +88,65 @@ public class Shooter extends SubsystemBase { io.updateInputs(state); if(this.mode != ShooterMode.NotReady) { - double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2)); - double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond); - double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + + ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + + Pose2d robotPose2d = m_SwerveDrive.getPose2d(); + + double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm(); + + + // TODO: get if the robot is within the angle of the hub + + boolean driverError = + XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | + AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | + distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); + + double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + + boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; + + int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 4 : 0); + switch (bitmask) { + case 0b000: // No Errors + m_robotLED.setMode(Constants.LEDConstants.OPREADY); + break; + case 0b001: // No op err, yes driver err + m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); + break; + case 0b010: // Bad flywheel, no driver err + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + break; + case 0b011: // Bad flywheel, yes driver err + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + break; + case 0b100: // Bad intake, no driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + break; + case 0b101: // Bad intake, yes driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + break; + case 0b110: // Bad intake and shooter (intake is more important), no driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + break; + case 0b111: // Bad intake and shooter (intake is more important), yes driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + break; + } + // We set the shooter mode to ready if there are no errors + mode = ( + bitmask == 0 ? + ShooterMode.Ready : + ShooterMode.NotReady + ); - // TODO: get if the robot is within the correct distance of the hub } switch (mode) { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 531ed12..108ef6e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -31,21 +31,21 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); - // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10); public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10); + // Tolerances - public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1); - public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1); + public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0); + public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99); - public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang tolerance", 1); + public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); - public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1); - public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1); + public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); + public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3); - public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1); + public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1); diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java new file mode 100644 index 0000000..19457d8 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -0,0 +1,21 @@ +package frc4388.utility.compute; + +import edu.wpi.first.math.geometry.Translation2d; + +public class FieldPositions { + public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + + // We set the default position to one just in case it doesn't update + // Setting to null could cause a null pointer, and setting to (0,0) could cause problems + // I would rather have the 50/50 chance than a code error + public static Translation2d HUB_POSITION = RED_HUB_POSITION; + + public static void update() { + if(TimesNegativeOne.isRed) { + HUB_POSITION = RED_HUB_POSITION; + } else { + HUB_POSITION = BLUE_HUB_POSITION; + } + } +}