From ba5afdc5908bf0c7efff4b58dedc4a2711ea3665 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 14 Feb 2026 14:33:26 -0500 Subject: [PATCH 01/12] Started implementation of Controller Subsystem --- src/main/java/frc/robot/RobotContainer.java | 28 ++- .../frc/robot/constants/GameConstants.java | 2 + .../robot/subsystems/ControllerSubsystem.java | 214 ++++++++++++++++++ 3 files changed, 236 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/ControllerSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 299445d..1560677 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,13 +22,13 @@ import frc.robot.commands.drive.FakeVision; import frc.robot.commands.intake.SpinIntake; import frc.robot.autochooser.AutoChooser; -import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.RunAnglerToReverseLimit; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; import frc.robot.constants.Constants; import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.ApriltagSubsystem; +import frc.robot.subsystems.ControllerSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.constants.ShootingState; @@ -69,6 +69,7 @@ public class RobotContainer { private final FeederSubsystem feederSubsystem; private final ApriltagSubsystem apriltagSubsystem; private final ShooterSubsystem shooterSubsystem; + private final ControllerSubsystem controllerSubsystem; private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; private SwerveSubsystem drivebase = null; @@ -102,6 +103,8 @@ public RobotContainer() { SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro); drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(() -> drivebase.getPose(), shootState) : null; + } case REPLAY -> { //rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); @@ -115,6 +118,7 @@ public RobotContainer() { // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(() -> drivebase.getPose(), shootState) : null; } case SIM -> { robotVisualizer = new RobotVisualizer(); @@ -129,6 +133,7 @@ public RobotContainer() { // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(() -> drivebase.getPose(), shootState) : null; } default -> { @@ -160,13 +165,20 @@ private void configureBindings() { // TODO: Clean this up a little - create command in method and only create the one actually needed - //example default command for angler- disabled for now - if (false) { - new AimAngler( - anglerSubsystem, - () -> drivebase != null ? drivebase.getPose() : null, - shootState); - } + anglerSubsystem.setDefaultCommand(new RunCommand( + () -> anglerSubsystem.setAngle(controllerSubsystem.getTargetAnglerAngleDegrees()), + anglerSubsystem)); + + shooterSubsystem.setDefaultCommand(new RunCommand( + () -> { + double targetShooterVelocity = controllerSubsystem.getTargetShooterVelocityRpm(); + if (targetShooterVelocity > 0.0) { + shooterSubsystem.setPidVelocity(targetShooterVelocity); + } else { + shooterSubsystem.stopMotors(); + } + }, + shooterSubsystem)); if(!Constants.TESTBED){ SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index b485a27..c9cf0c1 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -83,11 +83,13 @@ public enum Mode { public static final double ANGLER_D = 0.0; public static final double ANGLER_FF = 0.0; public static final double ANGLER_HOME_ROTATIONS = 0.0; + public static final double ANGLER_HOME_ANGLE = 0.0; public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler public static final double ANGLER_ENCODER_HIGH = 100; //Highest encoder position of Angler public static final double ANGLER_ANGLE_LOW = 0; //Lowest angle position of Angler public static final double ANGLER_ANGLE_HIGH = 45; //Highest angle position of Angler public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState + public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_LIMIT_SPEED = 0.2; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java new file mode 100644 index 0000000..b5c7dce --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -0,0 +1,214 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.ShootingState; +import frc.robot.constants.ShootingState.ShootState; + +public class ControllerSubsystem extends SubsystemBase { + + private static final double STOP_DELAY_SECONDS = 0.5; + + // Placeholder target poses until real field target values are finalized + private static final Pose2d HUB_TARGET_POSE = new Pose2d(4.0, 4.0, Rotation2d.kZero); + private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero); + + private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX"; + private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY"; + private static final String USING_MANUAL_POSE_KEY = "controller/UsingManualPose"; + private static final String CURRENT_SHOOT_STATE_KEY = "controller/CurrentShootState"; + private static final String DISTANCE_METERS_KEY = "controller/CalculatedDistanceMeters"; + private static final String TARGET_ANGLER_ANGLE_KEY = "controller/TargetAnglerAngleDegrees"; + private static final String TARGET_SHOOTER_VELOCITY_KEY = "controller/TargetShooterVelocity"; + private static final String TARGET_TURRET_ANGLE_KEY = "controller/TargetTurretAngleDegrees"; + + // Placeholder fixed-state settings. + private static final ShotTargets STOPPED_TARGETS = + new ShotTargets(Constants.ANGLER_HOME_ANGLE, 0.0, 0.0, 0.0); + private static final ShotTargets FIXED_TARGETS = + new ShotTargets(10.0, 120.0, 5.0, 0.0); + private static final ShotTargets FIXED_2_TARGETS = + new ShotTargets(22.0, 180.0, -5.0, 0.0); + + // Placeholder pose-driven profiles. + private static final PoseControlProfile HUB_PROFILE = + new PoseControlProfile(HUB_TARGET_POSE, 32.0, 230.0, 14.0); + private static final PoseControlProfile SHUTTLE_PROFILE = + new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, -14.0); + + private final Supplier poseSupplier; + private final ShootingState shootingState; + private final Timer stopDelayTimer = new Timer(); + + private ShootState previousState; + private double targetAnglerAngleDegrees; + private double targetShooterVelocityRpm; + private double targetTurretAngleDegrees; + private double distanceMeters; + + public ControllerSubsystem(Supplier poseSupplier, ShootingState shootingState) { + this.poseSupplier = poseSupplier; + this.shootingState = shootingState; + this.previousState = shootingState.getShootState(); + this.targetAnglerAngleDegrees = Constants.ANGLER_HOME_ANGLE; + this.targetShooterVelocityRpm = 0.0; + this.targetTurretAngleDegrees = 0.0; + this.distanceMeters = 0.0; + + SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); + SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); + } + + @Override + public void periodic() { + Pose2d robotPose = getRobotPose(); + ShootState currentState = shootingState.getShootState(); + + updateStopDelayState(currentState); + updateTargets(currentState, robotPose); + + SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); + SmartDashboard.putNumber(DISTANCE_METERS_KEY, distanceMeters); + SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, targetAnglerAngleDegrees); + SmartDashboard.putNumber(TARGET_SHOOTER_VELOCITY_KEY, targetShooterVelocityRpm); + SmartDashboard.putNumber(TARGET_TURRET_ANGLE_KEY, targetTurretAngleDegrees); + + previousState = currentState; + } + + private Pose2d getRobotPose() { + SmartDashboard.putBoolean(USING_MANUAL_POSE_KEY, shouldUseManualPose()); + if (shouldUseManualPose()) { + return getManualPose(); + } + return poseSupplier.get(); + } + + private boolean shouldUseManualPose() { + return (Constants.currentMode == Constants.Mode.SIM) || Constants.TESTBED; + } + + private Pose2d getManualPose() { + double x = SmartDashboard.getNumber(MANUAL_POSE_X_KEY, 0.0); + double y = SmartDashboard.getNumber(MANUAL_POSE_Y_KEY, 0.0); + return new Pose2d(x, y, Rotation2d.kZero); + } + + private void updateStopDelayState(ShootState currentState) { + if (currentState == ShootState.STOPPED && previousState != ShootState.STOPPED) { + stopDelayTimer.restart(); + } + } + + private void updateTargets(ShootState state, Pose2d robotPose) { + switch (state) { + case STOPPED -> updateStoppedTargets(); + case FIXED -> useShotTargets(FIXED_TARGETS); + case FIXED_2 -> useShotTargets(FIXED_2_TARGETS); + case SHOOTING_HUB -> useShotTargets(calculateTargetsFromPose(HUB_PROFILE, robotPose)); + case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose)); + } + } + + private void updateStoppedTargets() { + targetAnglerAngleDegrees = STOPPED_TARGETS.anglerAngleDegrees; + targetTurretAngleDegrees = STOPPED_TARGETS.turretAngleDegrees; + distanceMeters = STOPPED_TARGETS.distanceMeters; + + if (stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS)) { + targetShooterVelocityRpm = 0.0; + } + } + + private void useShotTargets(ShotTargets shotTargets) { + targetAnglerAngleDegrees = shotTargets.anglerAngleDegrees; + targetShooterVelocityRpm = shotTargets.shooterVelocityRpm; + targetTurretAngleDegrees = shotTargets.turretAngleDegrees; + distanceMeters = shotTargets.distanceMeters; + } + + private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) { + double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose); + double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); + double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); + double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters); + } + + private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { + return robotPose.getTranslation().getDistance(targetPose.getTranslation()); + } + + private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { + // TODO: Replace with distance angler angle calculation. + return profile.defaultAnglerAngleDegrees; + } + + private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { + // TODO: Replace with distance shooter velocity calculation. + return profile.defaultShooterVelocityRpm; + } + + private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { + // TODO: Replace with distance turret angle calculation. + return profile.defaultTurretAngleDegrees; + } + + //Getters for all the other subsystems + public double getTargetAnglerAngleDegrees() { + return targetAnglerAngleDegrees; + } + + public double getTargetShooterVelocityRpm() { + return targetShooterVelocityRpm; + } + + public double getTargetTurretAngleDegrees() { + return targetTurretAngleDegrees; + } + + public double getDistanceMeters() { + return distanceMeters; + } + + + //Class to save all the fixed targets + private static final class ShotTargets { + private final double anglerAngleDegrees; + private final double shooterVelocityRpm; + private final double turretAngleDegrees; + private final double distanceMeters; + + private ShotTargets(double anglerAngleDegrees, double shooterVelocityRpm, double turretAngleDegrees, double distanceMeters) { + this.anglerAngleDegrees = anglerAngleDegrees; + this.shooterVelocityRpm = shooterVelocityRpm; + this.turretAngleDegrees = turretAngleDegrees; + this.distanceMeters = distanceMeters; + } + } + + //Class to save all the target poses and each subsystem position at that point so we can calculate true values later on + private static final class PoseControlProfile { + private final Pose2d targetPose; + private final double defaultAnglerAngleDegrees; + private final double defaultShooterVelocityRpm; + private final double defaultTurretAngleDegrees; + + private PoseControlProfile( + Pose2d targetPose, + double defaultAnglerAngleDegrees, + double defaultShooterVelocityRpm, + double defaultTurretAngleDegrees) { + this.targetPose = targetPose; + this.defaultAnglerAngleDegrees = defaultAnglerAngleDegrees; + this.defaultShooterVelocityRpm = defaultShooterVelocityRpm; + this.defaultTurretAngleDegrees = defaultTurretAngleDegrees; + } + } +} From 8351e5610995ca7316515c20cf2f79bae0414966 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 14 Feb 2026 14:53:17 -0500 Subject: [PATCH 02/12] comments --- src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index b5c7dce..950db83 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -160,7 +160,7 @@ private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile return profile.defaultTurretAngleDegrees; } - //Getters for all the other subsystems + //Getters for all the subsystems to set posistion. public double getTargetAnglerAngleDegrees() { return targetAnglerAngleDegrees; } From 73036023c97ae1ea461c5e5f3daa7dcc2a243532 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 14 Feb 2026 15:45:04 -0500 Subject: [PATCH 03/12] Default Commands and Feeder and Hopper usage --- src/main/java/frc/robot/RobotContainer.java | 52 ++++++++++++++----- .../robot/subsystems/ControllerSubsystem.java | 21 ++++++++ 2 files changed, 59 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1560677..76007df 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -165,20 +165,44 @@ private void configureBindings() { // TODO: Clean this up a little - create command in method and only create the one actually needed - anglerSubsystem.setDefaultCommand(new RunCommand( - () -> anglerSubsystem.setAngle(controllerSubsystem.getTargetAnglerAngleDegrees()), - anglerSubsystem)); - - shooterSubsystem.setDefaultCommand(new RunCommand( - () -> { - double targetShooterVelocity = controllerSubsystem.getTargetShooterVelocityRpm(); - if (targetShooterVelocity > 0.0) { - shooterSubsystem.setPidVelocity(targetShooterVelocity); - } else { - shooterSubsystem.stopMotors(); - } - }, - shooterSubsystem)); + if (controllerSubsystem != null) { + anglerSubsystem.setDefaultCommand(new RunCommand( + () -> anglerSubsystem.setAngle(controllerSubsystem.getTargetAnglerAngleDegrees()), + anglerSubsystem)); + + shooterSubsystem.setDefaultCommand(new RunCommand( + () -> { + double targetShooterVelocity = controllerSubsystem.getTargetShooterVelocityRpm(); + if (targetShooterVelocity > 0.0) { + shooterSubsystem.setPidVelocity(targetShooterVelocity); + } else { + shooterSubsystem.stopMotors(); + } + }, + shooterSubsystem)); + + feederSubsystem.setDefaultCommand(new RunCommand( + () -> { + double targetFeederSpeed = controllerSubsystem.getFeederSpeed(); + if (targetFeederSpeed != 0.0) { + feederSubsystem.setSpeed(targetFeederSpeed); + } else { + feederSubsystem.stopMotors(); + } + }, + feederSubsystem)); + + hopperSubsystem.setDefaultCommand(new RunCommand( + () -> { + double targetHopperSpeed = controllerSubsystem.getHopperSpeed(); + if (targetHopperSpeed != 0.0) { + hopperSubsystem.setSpeed(targetHopperSpeed); + } else { + hopperSubsystem.stopMotors(); + } + }, + hopperSubsystem)); + } if(!Constants.TESTBED){ SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 950db83..1feb5a2 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -27,6 +27,8 @@ public class ControllerSubsystem extends SubsystemBase { private static final String TARGET_ANGLER_ANGLE_KEY = "controller/TargetAnglerAngleDegrees"; private static final String TARGET_SHOOTER_VELOCITY_KEY = "controller/TargetShooterVelocity"; private static final String TARGET_TURRET_ANGLE_KEY = "controller/TargetTurretAngleDegrees"; + private static final String TARGET_FEEDER_SPEED_KEY = "controller/TargetFeederSpeed"; + private static final String TARGET_HOPPER_SPEED_KEY = "controller/TargetHopperSpeed"; // Placeholder fixed-state settings. private static final ShotTargets STOPPED_TARGETS = @@ -51,6 +53,9 @@ public class ControllerSubsystem extends SubsystemBase { private double targetShooterVelocityRpm; private double targetTurretAngleDegrees; private double distanceMeters; + private double feederSpeed; + private double hopperSpeed; + public ControllerSubsystem(Supplier poseSupplier, ShootingState shootingState) { this.poseSupplier = poseSupplier; @@ -60,6 +65,8 @@ public ControllerSubsystem(Supplier poseSupplier, ShootingState shooting this.targetShooterVelocityRpm = 0.0; this.targetTurretAngleDegrees = 0.0; this.distanceMeters = 0.0; + this.feederSpeed = 0.0; + this.hopperSpeed = 0.0; SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); @@ -78,6 +85,8 @@ public void periodic() { SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, targetAnglerAngleDegrees); SmartDashboard.putNumber(TARGET_SHOOTER_VELOCITY_KEY, targetShooterVelocityRpm); SmartDashboard.putNumber(TARGET_TURRET_ANGLE_KEY, targetTurretAngleDegrees); + SmartDashboard.putNumber(TARGET_FEEDER_SPEED_KEY, feederSpeed); + SmartDashboard.putNumber(TARGET_HOPPER_SPEED_KEY, hopperSpeed); previousState = currentState; } @@ -120,6 +129,8 @@ private void updateStoppedTargets() { targetAnglerAngleDegrees = STOPPED_TARGETS.anglerAngleDegrees; targetTurretAngleDegrees = STOPPED_TARGETS.turretAngleDegrees; distanceMeters = STOPPED_TARGETS.distanceMeters; + feederSpeed = 0.0; + hopperSpeed = 0.0; if (stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS)) { targetShooterVelocityRpm = 0.0; @@ -131,6 +142,8 @@ private void useShotTargets(ShotTargets shotTargets) { targetShooterVelocityRpm = shotTargets.shooterVelocityRpm; targetTurretAngleDegrees = shotTargets.turretAngleDegrees; distanceMeters = shotTargets.distanceMeters; + feederSpeed = Constants.FEEDER_SPEED; + hopperSpeed = Constants.HOPPER_SPEED; } private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) { @@ -177,6 +190,14 @@ public double getDistanceMeters() { return distanceMeters; } + public double getFeederSpeed() { + return feederSpeed; + } + + public double getHopperSpeed() { + return hopperSpeed; + } + //Class to save all the fixed targets private static final class ShotTargets { From 8e2a2df5ff1f4edf945a10fd0aa67be8f7ac6485 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Mon, 16 Feb 2026 20:27:57 -0500 Subject: [PATCH 04/12] Fixes --- src/main/java/frc/robot/RobotContainer.java | 26 ++----------------- .../frc/robot/commands/feeder/SpinFeeder.java | 13 +++++++--- .../frc/robot/commands/hopper/SpinHopper.java | 11 ++++++-- .../robot/subsystems/ControllerSubsystem.java | 26 +++++++++---------- 4 files changed, 33 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 76007df..b20b3c6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -180,28 +180,6 @@ private void configureBindings() { } }, shooterSubsystem)); - - feederSubsystem.setDefaultCommand(new RunCommand( - () -> { - double targetFeederSpeed = controllerSubsystem.getFeederSpeed(); - if (targetFeederSpeed != 0.0) { - feederSubsystem.setSpeed(targetFeederSpeed); - } else { - feederSubsystem.stopMotors(); - } - }, - feederSubsystem)); - - hopperSubsystem.setDefaultCommand(new RunCommand( - () -> { - double targetHopperSpeed = controllerSubsystem.getHopperSpeed(); - if (targetHopperSpeed != 0.0) { - hopperSubsystem.setSpeed(targetHopperSpeed); - } else { - hopperSubsystem.stopMotors(); - } - }, - hopperSubsystem)); } if(!Constants.TESTBED){ @@ -295,11 +273,11 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Start Hopper", - new SpinHopper(hopperSubsystem)); + new SpinHopper(hopperSubsystem, shootState)); SmartDashboard.putData( "Spin Feeder", - new SpinFeeder(feederSubsystem)); + new SpinFeeder(feederSubsystem, shootState)); SmartDashboard.putData( "Spin Shooter", diff --git a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java index 9fd7796..0fbf6dd 100644 --- a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java +++ b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java @@ -2,17 +2,20 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; +import frc.robot.constants.ShootingState; +import frc.robot.constants.ShootingState.ShootState; import frc.robot.subsystems.FeederSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; public class SpinFeeder extends LoggableCommand { private final FeederSubsystem subsystem; private final Timer timer; + private final ShootingState state; - public SpinFeeder(FeederSubsystem subsystem) { + public SpinFeeder(FeederSubsystem subsystem, ShootingState state) { this.subsystem = subsystem; + this.state = state; timer = new Timer(); addRequirements(subsystem); } @@ -24,7 +27,11 @@ public void initialize() { @Override public void execute() { - subsystem.setSpeed(Constants.FEEDER_SPEED); + if (state.getShootState() != ShootState.STOPPED) { + subsystem.setSpeed(Constants.FEEDER_SPEED); + } else { + subsystem.stopMotors(); + } } @Override diff --git a/src/main/java/frc/robot/commands/hopper/SpinHopper.java b/src/main/java/frc/robot/commands/hopper/SpinHopper.java index 72f8d59..79fc958 100644 --- a/src/main/java/frc/robot/commands/hopper/SpinHopper.java +++ b/src/main/java/frc/robot/commands/hopper/SpinHopper.java @@ -2,6 +2,8 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; +import frc.robot.constants.ShootingState; +import frc.robot.constants.ShootingState.ShootState; import frc.robot.subsystems.HopperSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; @@ -9,11 +11,12 @@ public class SpinHopper extends LoggableCommand{ public final HopperSubsystem subsystem; public final Timer timer; - + public final ShootingState state; - public SpinHopper(HopperSubsystem subsystem){ + public SpinHopper(HopperSubsystem subsystem, ShootingState state){ timer = new Timer(); this.subsystem = subsystem; + this.state = state; addRequirements(subsystem); } @@ -24,7 +27,11 @@ public void initialize() { @Override public void execute() { + if (state.getShootState() != ShootState.STOPPED) { subsystem.setSpeed(Constants.HOPPER_SPEED); + } else { + subsystem.stopMotors(); + } } @Override diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 1feb5a2..75e46b2 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -53,8 +53,8 @@ public class ControllerSubsystem extends SubsystemBase { private double targetShooterVelocityRpm; private double targetTurretAngleDegrees; private double distanceMeters; - private double feederSpeed; - private double hopperSpeed; + private boolean feederSpin; + private boolean hopperSpin; public ControllerSubsystem(Supplier poseSupplier, ShootingState shootingState) { @@ -65,8 +65,6 @@ public ControllerSubsystem(Supplier poseSupplier, ShootingState shooting this.targetShooterVelocityRpm = 0.0; this.targetTurretAngleDegrees = 0.0; this.distanceMeters = 0.0; - this.feederSpeed = 0.0; - this.hopperSpeed = 0.0; SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); @@ -85,8 +83,8 @@ public void periodic() { SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, targetAnglerAngleDegrees); SmartDashboard.putNumber(TARGET_SHOOTER_VELOCITY_KEY, targetShooterVelocityRpm); SmartDashboard.putNumber(TARGET_TURRET_ANGLE_KEY, targetTurretAngleDegrees); - SmartDashboard.putNumber(TARGET_FEEDER_SPEED_KEY, feederSpeed); - SmartDashboard.putNumber(TARGET_HOPPER_SPEED_KEY, hopperSpeed); + SmartDashboard.putBoolean(TARGET_FEEDER_SPEED_KEY, feederSpin); + SmartDashboard.putBoolean(TARGET_HOPPER_SPEED_KEY, hopperSpin); previousState = currentState; } @@ -129,8 +127,8 @@ private void updateStoppedTargets() { targetAnglerAngleDegrees = STOPPED_TARGETS.anglerAngleDegrees; targetTurretAngleDegrees = STOPPED_TARGETS.turretAngleDegrees; distanceMeters = STOPPED_TARGETS.distanceMeters; - feederSpeed = 0.0; - hopperSpeed = 0.0; + feederSpin = false; + hopperSpin = false; if (stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS)) { targetShooterVelocityRpm = 0.0; @@ -142,8 +140,8 @@ private void useShotTargets(ShotTargets shotTargets) { targetShooterVelocityRpm = shotTargets.shooterVelocityRpm; targetTurretAngleDegrees = shotTargets.turretAngleDegrees; distanceMeters = shotTargets.distanceMeters; - feederSpeed = Constants.FEEDER_SPEED; - hopperSpeed = Constants.HOPPER_SPEED; + feederSpin = true; + hopperSpin = true; } private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) { @@ -190,12 +188,12 @@ public double getDistanceMeters() { return distanceMeters; } - public double getFeederSpeed() { - return feederSpeed; + public boolean shouldFeederSpin() { + return feederSpin; } - public double getHopperSpeed() { - return hopperSpeed; + public boolean shouldHopperSpin() { + return hopperSpin; } From c863ae9770bf01c6d9bd086d97392a109a2a8b54 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Mon, 16 Feb 2026 20:46:24 -0500 Subject: [PATCH 05/12] Merge changes --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- src/main/java/frc/robot/commands/feeder/SpinFeeder.java | 4 ++-- src/main/java/frc/robot/commands/hopper/SpinHopper.java | 4 ++-- src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 4 ++-- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7703dab..2a47714 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.commands.intakeDeployment.InitalRunDeployment; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.autochooser.AutoChooser; +import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.RunAnglerToReverseLimit; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; @@ -326,7 +327,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Spin Feeder", - new SpinFeeder(feederSubsystem)); + new SpinFeeder(feederSubsystem, shootState)); SmartDashboard.putData( "intakedeployer/InitlizeDeployer", new InitalRunDeployment(intakeDeployer)); @@ -336,7 +337,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Start Hopper", - new SpinHopper(hopperSubsystem)); + new SpinHopper(hopperSubsystem, shootState)); SmartDashboard.putData( "Climber Up", diff --git a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java index 0fbf6dd..8a2f327 100644 --- a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java +++ b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java @@ -2,8 +2,8 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; -import frc.robot.constants.ShootingState; -import frc.robot.constants.ShootingState.ShootState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.FeederSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; diff --git a/src/main/java/frc/robot/commands/hopper/SpinHopper.java b/src/main/java/frc/robot/commands/hopper/SpinHopper.java index 79fc958..5e78de7 100644 --- a/src/main/java/frc/robot/commands/hopper/SpinHopper.java +++ b/src/main/java/frc/robot/commands/hopper/SpinHopper.java @@ -2,8 +2,8 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; -import frc.robot.constants.ShootingState; -import frc.robot.constants.ShootingState.ShootState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.HopperSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 75e46b2..6c9dd54 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -8,8 +8,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; -import frc.robot.constants.ShootingState; -import frc.robot.constants.ShootingState.ShootState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; public class ControllerSubsystem extends SubsystemBase { From 7f2825f4e45010dc7867ad198d18b35aa0db9a02 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 14:14:21 -0500 Subject: [PATCH 06/12] Some fixes --- src/main/java/frc/robot/RobotContainer.java | 30 ++++++-------- .../commands/angler/DefaultAnglerControl.java | 27 +++++++++++++ .../frc/robot/commands/feeder/SpinFeeder.java | 11 +++--- .../frc/robot/commands/hopper/SpinHopper.java | 39 +++++++++---------- .../shooter/DefaultShooterControl.java | 32 +++++++++++++++ 5 files changed, 95 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java create mode 100644 src/main/java/frc/robot/commands/shooter/DefaultShooterControl.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2a47714..0dcfa27 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,7 +27,9 @@ import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; +import frc.robot.commands.angler.DefaultAnglerControl; import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.shooter.DefaultShooterControl; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.SpinShooter; import frc.robot.constants.Constants; @@ -82,7 +84,7 @@ public class RobotContainer { private final ApriltagSubsystem apriltagSubsystem; private final ShooterSubsystem shooterSubsystem; private final ControllerSubsystem controllerSubsystem; - private RobotVisualizer robotVisualizer = null; + private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; private final IntakeDeployerSubsystem intakeDeployer; private SwerveSubsystem drivebase = null; @@ -210,20 +212,10 @@ private void configureBindings() { } if (controllerSubsystem != null) { - anglerSubsystem.setDefaultCommand(new RunCommand( - () -> anglerSubsystem.setAngle(controllerSubsystem.getTargetAnglerAngleDegrees()), - anglerSubsystem)); - - shooterSubsystem.setDefaultCommand(new RunCommand( - () -> { - double targetShooterVelocity = controllerSubsystem.getTargetShooterVelocityRpm(); - if (targetShooterVelocity > 0.0) { - shooterSubsystem.setPidVelocity(targetShooterVelocity); - } else { - shooterSubsystem.stopMotors(); - } - }, - shooterSubsystem)); + anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); + shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); + hopperSubsystem.setDefaultCommand(new SpinHopper(hopperSubsystem, controllerSubsystem)); + feederSubsystem.setDefaultCommand(new SpinFeeder(feederSubsystem, controllerSubsystem)); } if (!Constants.TESTBED) { @@ -323,11 +315,11 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Start Hopper", - new SpinHopper(hopperSubsystem, shootState)); + new SpinHopper(hopperSubsystem, controllerSubsystem)); SmartDashboard.putData( "Spin Feeder", - new SpinFeeder(feederSubsystem, shootState)); + new SpinFeeder(feederSubsystem, controllerSubsystem)); SmartDashboard.putData( "intakedeployer/InitlizeDeployer", new InitalRunDeployment(intakeDeployer)); @@ -337,7 +329,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Start Hopper", - new SpinHopper(hopperSubsystem, shootState)); + new SpinHopper(hopperSubsystem, controllerSubsystem)); SmartDashboard.putData( "Climber Up", @@ -349,7 +341,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Spin Feeder", - new SpinFeeder(feederSubsystem, shootState)); + new SpinFeeder(feederSubsystem, controllerSubsystem)); SmartDashboard.putData( "Spin Shooter", diff --git a/src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java b/src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java new file mode 100644 index 0000000..474b6a6 --- /dev/null +++ b/src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java @@ -0,0 +1,27 @@ +package frc.robot.commands.angler; + +import frc.robot.subsystems.AnglerSubsystem; +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultAnglerControl extends LoggableCommand { + + private final AnglerSubsystem anglerSubsystem; + private final ControllerSubsystem controllerSubsystem; + + public DefaultAnglerControl(AnglerSubsystem anglerSubsystem, ControllerSubsystem controllerSubsystem) { + this.anglerSubsystem = anglerSubsystem; + this.controllerSubsystem = controllerSubsystem; + addRequirements(anglerSubsystem); + } + + @Override + public void execute() { + anglerSubsystem.setAngle(controllerSubsystem.getTargetAnglerAngleDegrees()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java index 8a2f327..f3641f6 100644 --- a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java +++ b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java @@ -4,18 +4,19 @@ import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ControllerSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; -public class SpinFeeder extends LoggableCommand { +public class SpinFeeder extends LoggableCommand { private final FeederSubsystem subsystem; private final Timer timer; - private final ShootingState state; + private final ControllerSubsystem controllerSubsystem; - public SpinFeeder(FeederSubsystem subsystem, ShootingState state) { + public SpinFeeder(FeederSubsystem subsystem, ControllerSubsystem controllerSubsystem) { this.subsystem = subsystem; - this.state = state; + this.controllerSubsystem = controllerSubsystem; timer = new Timer(); addRequirements(subsystem); } @@ -27,7 +28,7 @@ public void initialize() { @Override public void execute() { - if (state.getShootState() != ShootState.STOPPED) { + if (controllerSubsystem.shouldFeederSpin()) { subsystem.setSpeed(Constants.FEEDER_SPEED); } else { subsystem.stopMotors(); diff --git a/src/main/java/frc/robot/commands/hopper/SpinHopper.java b/src/main/java/frc/robot/commands/hopper/SpinHopper.java index 5e78de7..02c2107 100644 --- a/src/main/java/frc/robot/commands/hopper/SpinHopper.java +++ b/src/main/java/frc/robot/commands/hopper/SpinHopper.java @@ -4,51 +4,50 @@ import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; -public class SpinHopper extends LoggableCommand{ +public class SpinHopper extends LoggableCommand { - public final HopperSubsystem subsystem; - public final Timer timer; - public final ShootingState state; - - public SpinHopper(HopperSubsystem subsystem, ShootingState state){ - timer = new Timer(); + private final HopperSubsystem subsystem; + private final Timer timer; + private final ControllerSubsystem controllerSubsystem; + + public SpinHopper(HopperSubsystem subsystem, ControllerSubsystem controllerSubsystem) { this.subsystem = subsystem; - this.state = state; + this.controllerSubsystem = controllerSubsystem; + timer = new Timer(); addRequirements(subsystem); } @Override public void initialize() { - timer.restart(); - } + timer.restart(); + } @Override public void execute() { - if (state.getShootState() != ShootState.STOPPED) { + if (controllerSubsystem.shouldHopperSpin()) { subsystem.setSpeed(Constants.HOPPER_SPEED); } else { subsystem.stopMotors(); } } - @Override - public void end(boolean interrupted) { - subsystem.stopMotors(); - } - @Override public boolean isFinished() { - if (timer.hasElapsed(Constants.HOPPER_TIMEOUT)){ + if (timer.hasElapsed(Constants.HOPPER_TIMEOUT)) { return true; - } - else{ + } else { return false; } + } + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); } - } diff --git a/src/main/java/frc/robot/commands/shooter/DefaultShooterControl.java b/src/main/java/frc/robot/commands/shooter/DefaultShooterControl.java new file mode 100644 index 0000000..a37176e --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/DefaultShooterControl.java @@ -0,0 +1,32 @@ +package frc.robot.commands.shooter; + +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultShooterControl extends LoggableCommand { + + private final ShooterSubsystem shooterSubsystem; + private final ControllerSubsystem controllerSubsystem; + + public DefaultShooterControl(ShooterSubsystem shooterSubsystem, ControllerSubsystem controllerSubsystem) { + this.shooterSubsystem = shooterSubsystem; + this.controllerSubsystem = controllerSubsystem; + addRequirements(shooterSubsystem); + } + + @Override + public void execute() { + double targetShooterVelocity = controllerSubsystem.getTargetShooterVelocityRpm(); + if (targetShooterVelocity > 0.0) { + shooterSubsystem.setPidVelocity(targetShooterVelocity); + } else { + shooterSubsystem.stopMotors(); + } + } + + @Override + public boolean isFinished() { + return false; + } +} From f567e52fac3f227fb2989b06dc47a7375ffe4cd2 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 15:18:12 -0500 Subject: [PATCH 07/12] Comment resolves --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../robot/subsystems/ControllerSubsystem.java | 123 ++++++++++-------- 2 files changed, 72 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0dcfa27..5005e51 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -124,7 +124,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(() -> drivebase.getPose(), shootState) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; } case REPLAY -> { @@ -143,7 +143,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(() -> drivebase.getPose(), shootState) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; } case SIM -> { robotVisualizer = new RobotVisualizer(); @@ -165,7 +165,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(() -> drivebase.getPose(), shootState) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; } default -> { diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 6c9dd54..f0e8ed0 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,7 +1,8 @@ package frc.robot.subsystems; -import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import frc.robot.RobotContainer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Timer; @@ -10,6 +11,7 @@ import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; public class ControllerSubsystem extends SubsystemBase { @@ -32,11 +34,11 @@ public class ControllerSubsystem extends SubsystemBase { // Placeholder fixed-state settings. private static final ShotTargets STOPPED_TARGETS = - new ShotTargets(Constants.ANGLER_HOME_ANGLE, 0.0, 0.0, 0.0); + new ShotTargets(Constants.ANGLER_HOME_ANGLE, 0.0, 0.0, 0.0, false, false); private static final ShotTargets FIXED_TARGETS = - new ShotTargets(10.0, 120.0, 5.0, 0.0); + new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true); private static final ShotTargets FIXED_2_TARGETS = - new ShotTargets(22.0, 180.0, -5.0, 0.0); + new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); // Placeholder pose-driven profiles. private static final PoseControlProfile HUB_PROFILE = @@ -44,27 +46,19 @@ public class ControllerSubsystem extends SubsystemBase { private static final PoseControlProfile SHUTTLE_PROFILE = new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, -14.0); - private final Supplier poseSupplier; - private final ShootingState shootingState; + private final SwerveSubsystem drivebase; + private final RobotContainer robotContainer; private final Timer stopDelayTimer = new Timer(); private ShootState previousState; - private double targetAnglerAngleDegrees; - private double targetShooterVelocityRpm; - private double targetTurretAngleDegrees; - private double distanceMeters; - private boolean feederSpin; - private boolean hopperSpin; - - - public ControllerSubsystem(Supplier poseSupplier, ShootingState shootingState) { - this.poseSupplier = poseSupplier; - this.shootingState = shootingState; - this.previousState = shootingState.getShootState(); - this.targetAnglerAngleDegrees = Constants.ANGLER_HOME_ANGLE; - this.targetShooterVelocityRpm = 0.0; - this.targetTurretAngleDegrees = 0.0; - this.distanceMeters = 0.0; + private ShotTargets activeTargets; + + + public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) { + this.drivebase = drivebase; + this.robotContainer = robotContainer; + this.previousState = getCurrentShootState(); + this.activeTargets = STOPPED_TARGETS; SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); @@ -73,28 +67,41 @@ public ControllerSubsystem(Supplier poseSupplier, ShootingState shooting @Override public void periodic() { Pose2d robotPose = getRobotPose(); - ShootState currentState = shootingState.getShootState(); + ShootState currentState = getCurrentShootState(); updateStopDelayState(currentState); updateTargets(currentState, robotPose); SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); - SmartDashboard.putNumber(DISTANCE_METERS_KEY, distanceMeters); - SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, targetAnglerAngleDegrees); - SmartDashboard.putNumber(TARGET_SHOOTER_VELOCITY_KEY, targetShooterVelocityRpm); - SmartDashboard.putNumber(TARGET_TURRET_ANGLE_KEY, targetTurretAngleDegrees); - SmartDashboard.putBoolean(TARGET_FEEDER_SPEED_KEY, feederSpin); - SmartDashboard.putBoolean(TARGET_HOPPER_SPEED_KEY, hopperSpin); + SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); + SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, activeTargets.anglerAngleDegrees); + SmartDashboard.putNumber(TARGET_SHOOTER_VELOCITY_KEY, activeTargets.shooterVelocityRpm); + SmartDashboard.putNumber(TARGET_TURRET_ANGLE_KEY, activeTargets.turretAngleDegrees); + SmartDashboard.putBoolean(TARGET_FEEDER_SPEED_KEY, activeTargets.feederSpin); + SmartDashboard.putBoolean(TARGET_HOPPER_SPEED_KEY, activeTargets.hopperSpin); + + Logger.recordOutput(CURRENT_SHOOT_STATE_KEY, currentState.toString()); + Logger.recordOutput(DISTANCE_METERS_KEY, activeTargets.distanceMeters); + Logger.recordOutput(TARGET_ANGLER_ANGLE_KEY, activeTargets.anglerAngleDegrees); + Logger.recordOutput(TARGET_SHOOTER_VELOCITY_KEY, activeTargets.shooterVelocityRpm); + Logger.recordOutput(TARGET_TURRET_ANGLE_KEY, activeTargets.turretAngleDegrees); + Logger.recordOutput(TARGET_FEEDER_SPEED_KEY, activeTargets.feederSpin); + Logger.recordOutput(TARGET_HOPPER_SPEED_KEY, activeTargets.hopperSpin); previousState = currentState; } + private ShootState getCurrentShootState() { + return robotContainer.getShootingState().getShootState(); + } + private Pose2d getRobotPose() { SmartDashboard.putBoolean(USING_MANUAL_POSE_KEY, shouldUseManualPose()); + Logger.recordOutput(USING_MANUAL_POSE_KEY, shouldUseManualPose()); if (shouldUseManualPose()) { return getManualPose(); } - return poseSupplier.get(); + return drivebase.getPose(); } private boolean shouldUseManualPose() { @@ -124,24 +131,22 @@ private void updateTargets(ShootState state, Pose2d robotPose) { } private void updateStoppedTargets() { - targetAnglerAngleDegrees = STOPPED_TARGETS.anglerAngleDegrees; - targetTurretAngleDegrees = STOPPED_TARGETS.turretAngleDegrees; - distanceMeters = STOPPED_TARGETS.distanceMeters; - feederSpin = false; - hopperSpin = false; - - if (stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS)) { - targetShooterVelocityRpm = 0.0; - } + //This makes the shooter wait half a second before stopping + double shooterVelocity = stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS) + ? 0.0 + : activeTargets.shooterVelocityRpm; + + activeTargets = new ShotTargets( + STOPPED_TARGETS.anglerAngleDegrees, + shooterVelocity, + STOPPED_TARGETS.turretAngleDegrees, + STOPPED_TARGETS.distanceMeters, + STOPPED_TARGETS.feederSpin, + STOPPED_TARGETS.hopperSpin); } private void useShotTargets(ShotTargets shotTargets) { - targetAnglerAngleDegrees = shotTargets.anglerAngleDegrees; - targetShooterVelocityRpm = shotTargets.shooterVelocityRpm; - targetTurretAngleDegrees = shotTargets.turretAngleDegrees; - distanceMeters = shotTargets.distanceMeters; - feederSpin = true; - hopperSpin = true; + activeTargets = shotTargets; } private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) { @@ -149,7 +154,7 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); - return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true); } private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { @@ -173,27 +178,27 @@ private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile //Getters for all the subsystems to set posistion. public double getTargetAnglerAngleDegrees() { - return targetAnglerAngleDegrees; + return activeTargets.anglerAngleDegrees; } public double getTargetShooterVelocityRpm() { - return targetShooterVelocityRpm; + return activeTargets.shooterVelocityRpm; } public double getTargetTurretAngleDegrees() { - return targetTurretAngleDegrees; + return activeTargets.turretAngleDegrees; } public double getDistanceMeters() { - return distanceMeters; + return activeTargets.distanceMeters; } public boolean shouldFeederSpin() { - return feederSpin; + return activeTargets.feederSpin; } public boolean shouldHopperSpin() { - return hopperSpin; + return activeTargets.hopperSpin; } @@ -203,12 +208,22 @@ private static final class ShotTargets { private final double shooterVelocityRpm; private final double turretAngleDegrees; private final double distanceMeters; - - private ShotTargets(double anglerAngleDegrees, double shooterVelocityRpm, double turretAngleDegrees, double distanceMeters) { + private final boolean feederSpin; + private final boolean hopperSpin; + + private ShotTargets( + double anglerAngleDegrees, + double shooterVelocityRpm, + double turretAngleDegrees, + double distanceMeters, + boolean feederSpin, + boolean hopperSpin) { this.anglerAngleDegrees = anglerAngleDegrees; this.shooterVelocityRpm = shooterVelocityRpm; this.turretAngleDegrees = turretAngleDegrees; this.distanceMeters = distanceMeters; + this.feederSpin = feederSpin; + this.hopperSpin = hopperSpin; } } From fc2adeb5258640b59b67641150434fafbeb489d1 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 15:55:02 -0500 Subject: [PATCH 08/12] More comment resolves --- src/main/java/frc/robot/RobotContainer.java | 6 +++-- .../commands/turret/DefaultTurretControl.java | 27 +++++++++++++++++++ 2 files changed, 31 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/turret/DefaultTurretControl.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ddb4d4b..dfd347a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import frc.robot.commands.shooter.DefaultShooterControl; import frc.robot.commands.auto.ExampleAuto; import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.turret.DefaultTurretControl; import frc.robot.commands.turret.RunTurretToFwdLimit; import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.commands.turret.SetTurretAngle; @@ -292,6 +293,7 @@ private void configureBindings() { if (controllerSubsystem != null) { anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); + turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); hopperSubsystem.setDefaultCommand(new SpinHopper(hopperSubsystem, controllerSubsystem)); feederSubsystem.setDefaultCommand(new SpinFeeder(feederSubsystem, controllerSubsystem)); } @@ -402,11 +404,11 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "turret/Run Turret to Rev Limit", - new RunTurretToRevLimit(turretSubsystem, controllerSubsystem)); + new RunTurretToRevLimit(turretSubsystem)); SmartDashboard.putData( "turret/Run Turret to Fwd Limit", - new RunTurretToFwdLimit(turretSubsystem, controllerSubsystem)); + new RunTurretToFwdLimit(turretSubsystem)); SmartDashboard.putData( "intakedeployer/InitlizeDeployer", diff --git a/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java new file mode 100644 index 0000000..1fb25cd --- /dev/null +++ b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java @@ -0,0 +1,27 @@ +package frc.robot.commands.turret; + +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultTurretControl extends LoggableCommand { + + private final TurretSubsystem turretSubsystem; + private final ControllerSubsystem controllerSubsystem; + + public DefaultTurretControl(TurretSubsystem turretSubsystem, ControllerSubsystem controllerSubsystem) { + this.turretSubsystem = turretSubsystem; + this.controllerSubsystem = controllerSubsystem; + addRequirements(turretSubsystem); + } + + @Override + public void execute() { + turretSubsystem.setAngle(controllerSubsystem.getTargetTurretAngleDegrees()); + } + + @Override + public boolean isFinished() { + return false; + } +} From 3da6af9b17b3f91f99dd7ca46c002227212f206b Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 15:55:48 -0500 Subject: [PATCH 09/12] indent --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dfd347a..d004181 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,7 +95,7 @@ public class RobotContainer { private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; private final ClimberSubsystem climberSubsystem; - private final TurretSubsystem turretSubsystem; + private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; private SwerveSubsystem drivebase = null; private GyroSubsystem gyroSubsystem = null; From c927244388c923176447557f5eee03dc865cf1c5 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 19:22:38 -0500 Subject: [PATCH 10/12] Added shoot button --- src/main/java/frc/robot/RobotContainer.java | 4 +++ .../java/frc/robot/commands/ShootButton.java | 31 +++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 1 - .../robot/subsystems/ControllerSubsystem.java | 22 +++++++++++-- 4 files changed, 54 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ShootButton.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d004181..f6a4aa8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AddTunableApriltagReading; +import frc.robot.commands.ShootButton; import frc.robot.commands.AddApriltagReading; import frc.robot.commands.AddGarbageReading; import frc.robot.commands.climber.ClimberDown; @@ -271,6 +272,9 @@ private void setUpAutoFactory() { } private void configureBindings() { + if (controllerSubsystem != null) { + steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem)); + } // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) // .onTrue(new ExampleCommand(m_exampleSubsystem)); diff --git a/src/main/java/frc/robot/commands/ShootButton.java b/src/main/java/frc/robot/commands/ShootButton.java new file mode 100644 index 0000000..26fa28e --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootButton.java @@ -0,0 +1,31 @@ +package frc.robot.commands; + +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ShootButton extends LoggableCommand{ + + private final ControllerSubsystem controllerSubsystem; + + public ShootButton(ControllerSubsystem controllerSubsystem) { + this.controllerSubsystem = controllerSubsystem; + } + + @Override + public void end(boolean interrupted) { + controllerSubsystem.setDriverActivatedShooting(false); + } + + @Override + public void initialize() { + controllerSubsystem.setDriverActivatedShooting(true); + } + + @Override + public boolean isFinished() { + return false; + } + + + +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 0b7982a..da85500 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -97,7 +97,6 @@ public enum Mode { public static final double ANGLER_D = 0.0; public static final double ANGLER_FF = 0.0; public static final double ANGLER_HOME_ROTATIONS = 0.0; - public static final double ANGLER_HOME_ANGLE = 0.0; public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler public static final double ANGLER_ENCODER_HIGH = 100; //Highest encoder position of Angler public static final double ANGLER_ANGLE_LOW = 0; //Lowest angle position of Angler diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index f0e8ed0..6f2b0a2 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -34,7 +34,7 @@ public class ControllerSubsystem extends SubsystemBase { // Placeholder fixed-state settings. private static final ShotTargets STOPPED_TARGETS = - new ShotTargets(Constants.ANGLER_HOME_ANGLE, 0.0, 0.0, 0.0, false, false); + new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, false); private static final ShotTargets FIXED_TARGETS = new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true); private static final ShotTargets FIXED_2_TARGETS = @@ -52,6 +52,7 @@ public class ControllerSubsystem extends SubsystemBase { private ShootState previousState; private ShotTargets activeTargets; + private boolean driverActivatedShooting = false; public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) { @@ -135,7 +136,7 @@ private void updateStoppedTargets() { double shooterVelocity = stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS) ? 0.0 : activeTargets.shooterVelocityRpm; - + activeTargets = new ShotTargets( STOPPED_TARGETS.anglerAngleDegrees, shooterVelocity, @@ -146,7 +147,14 @@ private void updateStoppedTargets() { } private void useShotTargets(ShotTargets shotTargets) { - activeTargets = shotTargets; + boolean driverEnabled = driverActivatedShootingEnabled(); + activeTargets = new ShotTargets( + shotTargets.anglerAngleDegrees, + shotTargets.shooterVelocityRpm, + shotTargets.turretAngleDegrees, + shotTargets.distanceMeters, + driverEnabled, + driverEnabled); } private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) { @@ -201,6 +209,14 @@ public boolean shouldHopperSpin() { return activeTargets.hopperSpin; } + public void setDriverActivatedShooting(boolean set) { + driverActivatedShooting = set; + } + + public boolean driverActivatedShootingEnabled() { + return driverActivatedShooting; + } + //Class to save all the fixed targets private static final class ShotTargets { From eeb25b76640d6d36369499eb42d066f3f6270fe6 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Fri, 20 Feb 2026 21:36:11 -0500 Subject: [PATCH 11/12] Resolved comments --- src/main/java/frc/robot/commands/feeder/SpinFeeder.java | 6 +----- src/main/java/frc/robot/commands/hopper/SpinHopper.java | 6 +----- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java index bbd3d85..17650e9 100644 --- a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java +++ b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java @@ -39,11 +39,7 @@ public void execute() { @Override public boolean isFinished() { - if (timer.hasElapsed(Constants.FEEDER_TIMEOUT)) { - return true; - } else { - return false; - } + return false; } @Override diff --git a/src/main/java/frc/robot/commands/hopper/SpinHopper.java b/src/main/java/frc/robot/commands/hopper/SpinHopper.java index 02c2107..4c7c365 100644 --- a/src/main/java/frc/robot/commands/hopper/SpinHopper.java +++ b/src/main/java/frc/robot/commands/hopper/SpinHopper.java @@ -38,11 +38,7 @@ public void execute() { @Override public boolean isFinished() { - if (timer.hasElapsed(Constants.HOPPER_TIMEOUT)) { - return true; - } else { - return false; - } + return false; } @Override From 7b530dcfb864204fc8f7c742af1bb9efac0ea9c7 Mon Sep 17 00:00:00 2001 From: ohad Date: Fri, 20 Feb 2026 21:52:25 -0500 Subject: [PATCH 12/12] Rebase from main, create default commands --- src/main/java/frc/robot/RobotContainer.java | 10 ++-- .../commands/feeder/DefaultSpinFeeder.java | 46 +++++++++++++++++++ .../frc/robot/commands/feeder/SpinFeeder.java | 21 +++------ .../commands/hopper/DefaultSpinHopper.java | 46 +++++++++++++++++++ .../frc/robot/commands/hopper/SpinHopper.java | 40 ++++++++-------- 5 files changed, 124 insertions(+), 39 deletions(-) create mode 100644 src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java create mode 100644 src/main/java/frc/robot/commands/hopper/DefaultSpinHopper.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 213ef8f..3763253 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,8 @@ import frc.robot.commands.AddGarbageReading; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.DefaultSpinFeeder; +import frc.robot.commands.hopper.DefaultSpinHopper; import frc.robot.commands.hopper.SpinHopper; import frc.robot.commands.drive.DriveDirectionTime; import frc.robot.commands.feeder.SpinFeeder; @@ -315,8 +317,8 @@ private void configureBindings() { anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); - hopperSubsystem.setDefaultCommand(new SpinHopper(hopperSubsystem, controllerSubsystem)); - feederSubsystem.setDefaultCommand(new SpinFeeder(feederSubsystem, controllerSubsystem)); + hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem)); + feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem)); } if (!Constants.TESTBED) { @@ -443,7 +445,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Start Hopper", - new SpinHopper(hopperSubsystem, controllerSubsystem)); + new SpinHopper(hopperSubsystem)); SmartDashboard.putData( "Climber Up", @@ -455,7 +457,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Spin Feeder", - new SpinFeeder(feederSubsystem, controllerSubsystem)); + new SpinFeeder(feederSubsystem)); SmartDashboard.putData( "Spins Shooter", diff --git a/src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java b/src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java new file mode 100644 index 0000000..d413869 --- /dev/null +++ b/src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java @@ -0,0 +1,46 @@ +package frc.robot.commands.feeder; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultSpinFeeder extends LoggableCommand { + + private final FeederSubsystem subsystem; + private final Timer timer; + private final ControllerSubsystem controllerSubsystem; + + public DefaultSpinFeeder(FeederSubsystem subsystem, ControllerSubsystem controllerSubsystem) { + this.subsystem = subsystem; + this.controllerSubsystem = controllerSubsystem; + timer = new Timer(); + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + if (controllerSubsystem.shouldFeederSpin()) { + subsystem.setSpeed(Constants.FEEDER_SPEED); + } else { + subsystem.stopMotors(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + +} diff --git a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java index 17650e9..442f5a6 100644 --- a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java +++ b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java @@ -2,23 +2,16 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; -import frc.robot.constants.enums.ShootingState; -import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.ControllerSubsystem; import frc.robot.subsystems.FeederSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.utils.logging.TimeoutLogger; import frc.robot.utils.logging.commands.LoggableCommand; public class SpinFeeder extends LoggableCommand { private final FeederSubsystem subsystem; private final Timer timer; - private final ControllerSubsystem controllerSubsystem; - public SpinFeeder(FeederSubsystem subsystem, ControllerSubsystem controllerSubsystem) { + public SpinFeeder(FeederSubsystem subsystem) { this.subsystem = subsystem; - this.controllerSubsystem = controllerSubsystem; timer = new Timer(); addRequirements(subsystem); } @@ -30,16 +23,16 @@ public void initialize() { @Override public void execute() { - if (controllerSubsystem.shouldFeederSpin()) { - subsystem.setSpeed(Constants.FEEDER_SPEED); - } else { - subsystem.stopMotors(); - } + subsystem.setSpeed(Constants.FEEDER_SPEED); } @Override public boolean isFinished() { - return false; + if (timer.hasElapsed(Constants.FEEDER_TIMEOUT)) { + return true; + } else { + return false; + } } @Override diff --git a/src/main/java/frc/robot/commands/hopper/DefaultSpinHopper.java b/src/main/java/frc/robot/commands/hopper/DefaultSpinHopper.java new file mode 100644 index 0000000..1a18e54 --- /dev/null +++ b/src/main/java/frc/robot/commands/hopper/DefaultSpinHopper.java @@ -0,0 +1,46 @@ +package frc.robot.commands.hopper; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultSpinHopper extends LoggableCommand { + + private final HopperSubsystem subsystem; + private final Timer timer; + private final ControllerSubsystem controllerSubsystem; + + public DefaultSpinHopper(HopperSubsystem subsystem, ControllerSubsystem controllerSubsystem) { + this.subsystem = subsystem; + this.controllerSubsystem = controllerSubsystem; + timer = new Timer(); + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + if (controllerSubsystem.shouldHopperSpin()) { + subsystem.setSpeed(Constants.HOPPER_SPEED); + } else { + subsystem.stopMotors(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + +} diff --git a/src/main/java/frc/robot/commands/hopper/SpinHopper.java b/src/main/java/frc/robot/commands/hopper/SpinHopper.java index 4c7c365..0a69757 100644 --- a/src/main/java/frc/robot/commands/hopper/SpinHopper.java +++ b/src/main/java/frc/robot/commands/hopper/SpinHopper.java @@ -2,48 +2,46 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; -import frc.robot.constants.enums.ShootingState; -import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.ControllerSubsystem; -import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; public class SpinHopper extends LoggableCommand { - private final HopperSubsystem subsystem; - private final Timer timer; - private final ControllerSubsystem controllerSubsystem; - - public SpinHopper(HopperSubsystem subsystem, ControllerSubsystem controllerSubsystem) { - this.subsystem = subsystem; - this.controllerSubsystem = controllerSubsystem; + public final HopperSubsystem subsystem; + public final Timer timer; + + + public SpinHopper(HopperSubsystem subsystem){ timer = new Timer(); + this.subsystem = subsystem; addRequirements(subsystem); } @Override public void initialize() { - timer.restart(); - } + timer.restart(); + } @Override public void execute() { - if (controllerSubsystem.shouldHopperSpin()) { subsystem.setSpeed(Constants.HOPPER_SPEED); - } else { - subsystem.stopMotors(); - } } @Override - public boolean isFinished() { - return false; + public void end(boolean interrupted) { + subsystem.stopMotors(); } @Override - public void end(boolean interrupted) { - subsystem.stopMotors(); + public boolean isFinished() { + if (timer.hasElapsed(Constants.HOPPER_TIMEOUT)){ + return true; + } + else{ + return false; + } + } + }