From b123f24e68f0ba4b5db3802576b08fc882dfe42f Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Fri, 20 Feb 2026 21:34:10 -0500 Subject: [PATCH 01/13] added stop all command + other things needed for it to function --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../commands/angler/ResetAnglerEncoder.java | 24 +++++++++ ...lerToReverseLimit.java => StowAngler.java} | 4 +- .../commands/climber/ResetClimberEncoder.java | 24 +++++++++ .../InitialRunDeployment.java | 1 - .../frc/robot/commands/parallels/StopAll.java | 49 +++++++++++++++++++ .../robot/commands/shooter/StopShooter.java | 32 ++++++++++++ .../commands/turret/ResetTurretEncoder.java | 24 +++++++++ .../robot/subsystems/ClimberSubsystem.java | 5 ++ .../LoggableParallelCommandGroup.java | 2 +- 10 files changed, 163 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/commands/angler/ResetAnglerEncoder.java rename src/main/java/frc/robot/commands/angler/{RunAnglerToReverseLimit.java => StowAngler.java} (90%) create mode 100644 src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java create mode 100644 src/main/java/frc/robot/commands/parallels/StopAll.java create mode 100644 src/main/java/frc/robot/commands/shooter/StopShooter.java create mode 100644 src/main/java/frc/robot/commands/turret/ResetTurretEncoder.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8bf97c7d..939493a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,7 +30,7 @@ import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.StowAngler; import frc.robot.commands.auto.ExampleAuto; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.RunTurretToFwdLimit; @@ -395,7 +395,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "angler/Home Rev (Reset)", - new RunAnglerToReverseLimit(anglerSubsystem)); + new StowAngler(anglerSubsystem)); SmartDashboard.putData( "turret/Turret Go 45", diff --git a/src/main/java/frc/robot/commands/angler/ResetAnglerEncoder.java b/src/main/java/frc/robot/commands/angler/ResetAnglerEncoder.java new file mode 100644 index 00000000..6a386333 --- /dev/null +++ b/src/main/java/frc/robot/commands/angler/ResetAnglerEncoder.java @@ -0,0 +1,24 @@ +package frc.robot.commands.angler; + +import frc.robot.subsystems.AnglerSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ResetAnglerEncoder extends LoggableCommand { + + private final AnglerSubsystem subsystem; + + public ResetAnglerEncoder(AnglerSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.resetEncoderToZero(); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/angler/RunAnglerToReverseLimit.java b/src/main/java/frc/robot/commands/angler/StowAngler.java similarity index 90% rename from src/main/java/frc/robot/commands/angler/RunAnglerToReverseLimit.java rename to src/main/java/frc/robot/commands/angler/StowAngler.java index ae9ac6c5..cf19b3ca 100644 --- a/src/main/java/frc/robot/commands/angler/RunAnglerToReverseLimit.java +++ b/src/main/java/frc/robot/commands/angler/StowAngler.java @@ -9,12 +9,12 @@ /** * Runs the angler to the reverse limit switch and resets the encoder to zero. */ -public class RunAnglerToReverseLimit extends LoggableCommand { +public class StowAngler extends LoggableCommand { private final TimeoutLogger timeoutCounter; private final AnglerSubsystem angler; private final Timer timer = new Timer(); - public RunAnglerToReverseLimit(AnglerSubsystem angler) { + public StowAngler(AnglerSubsystem angler) { timeoutCounter = new TimeoutLogger(getName()); this.angler = angler; addRequirements(angler); diff --git a/src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java b/src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java new file mode 100644 index 00000000..eb6c76d0 --- /dev/null +++ b/src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java @@ -0,0 +1,24 @@ +package frc.robot.commands.climber; + +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ResetClimberEncoder extends LoggableCommand { + + private final ClimberSubsystem subsystem; + + public ResetClimberEncoder(ClimberSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.resetEncoderToZero(); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java b/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java index 7de4143f..078455b2 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands.intakeDeployment; - import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; import frc.robot.subsystems.IntakeDeployerSubsystem; diff --git a/src/main/java/frc/robot/commands/parallels/StopAll.java b/src/main/java/frc/robot/commands/parallels/StopAll.java new file mode 100644 index 00000000..7ea63a6a --- /dev/null +++ b/src/main/java/frc/robot/commands/parallels/StopAll.java @@ -0,0 +1,49 @@ +package frc.robot.commands.parallels; + +import frc.robot.commands.angler.ResetAnglerEncoder; +import frc.robot.commands.angler.StowAngler; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ResetClimberEncoder; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; +import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.StopShooter; +import frc.robot.commands.turret.ResetTurretEncoder; +import frc.robot.commands.turret.RunTurretToFwdLimit; +import frc.robot.constants.enums.DeploymentState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.AnglerSubsystem; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class StopAll extends LoggableParallelCommandGroup { + + public StopAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, + FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, + IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, + ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { + super( + new LoggableParallelCommandGroup( + new StowAngler(anglerSubsystem), + new ClimberDown(climberSubsystem), + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), + new SetShootingState(shootState, ShootState.STOPPED), + new StopShooter(shooterSubsystem), + new LoggableSequentialCommandGroup( + new RunTurretToFwdLimit(turretSubsystem), + new InitialRunDeployment(intakeDeployerSubsystem)), + new LoggableParallelCommandGroup( + new ResetAnglerEncoder(anglerSubsystem), + new ResetClimberEncoder(climberSubsystem), + new ResetTurretEncoder(turretSubsystem)))); + } + +} diff --git a/src/main/java/frc/robot/commands/shooter/StopShooter.java b/src/main/java/frc/robot/commands/shooter/StopShooter.java new file mode 100644 index 00000000..c847eadc --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/StopShooter.java @@ -0,0 +1,32 @@ +package frc.robot.commands.shooter; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class StopShooter extends LoggableCommand { + + private final ShooterSubsystem subsystem; + + public StopShooter(ShooterSubsystem shooterSubsystem) { + this.subsystem = shooterSubsystem; + addRequirements(shooterSubsystem); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + subsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + return true; + } + + @Override + public void end(boolean interrupted) { + } +} diff --git a/src/main/java/frc/robot/commands/turret/ResetTurretEncoder.java b/src/main/java/frc/robot/commands/turret/ResetTurretEncoder.java new file mode 100644 index 00000000..e2f0d8e6 --- /dev/null +++ b/src/main/java/frc/robot/commands/turret/ResetTurretEncoder.java @@ -0,0 +1,24 @@ +package frc.robot.commands.turret; + +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ResetTurretEncoder extends LoggableCommand { + + private final TurretSubsystem subsystem; + + public ResetTurretEncoder(TurretSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.resetEncoderToZero(); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index cd8829b2..dc7e0425 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -78,5 +78,10 @@ public boolean reverseSwitchPressed() { public boolean forwardSwitchPressed() { return io.isFwdSwitchPressed(); } + + public void resetEncoderToZero() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'resetEncoderToZero'"); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java b/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java index e080e167..36e8016d 100644 --- a/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java +++ b/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java @@ -18,7 +18,7 @@ public LoggableParallelCommandGroup(T... commands addCommands(proxyCommands); } - @Override +@Override public String getBasicName() { return basicName; } From cfe717bfaf36f7d9953b94dbc5b190aebb2d9234 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Sat, 21 Feb 2026 18:05:00 -0500 Subject: [PATCH 02/13] made stop all sequential instead of parallel --- src/main/java/frc/robot/commands/parallels/StopAll.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/parallels/StopAll.java b/src/main/java/frc/robot/commands/parallels/StopAll.java index 7ea63a6a..2ca64acc 100644 --- a/src/main/java/frc/robot/commands/parallels/StopAll.java +++ b/src/main/java/frc/robot/commands/parallels/StopAll.java @@ -24,7 +24,7 @@ import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class StopAll extends LoggableParallelCommandGroup { +public class StopAll extends LoggableSequentialCommandGroup { public StopAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, From 75a453f8741ebc301ec93dcd79b58e4c828b25ce Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Sat, 21 Feb 2026 18:52:42 -0500 Subject: [PATCH 03/13] started creating encoders for the stop command, need to find if we need real sim mock encoders? --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++++---- .../frc/robot/subsystems/ClimberSubsystem.java | 14 +++++--------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 939493a6..3df144d4 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.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.parallels.RunHopperAndFeeder; +import frc.robot.commands.parallels.StopAll; import frc.robot.commands.sequences.IntakeDownSequence; import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.autochooser.AutoChooser; @@ -64,6 +65,8 @@ import java.io.File; +import com.revrobotics.RelativeEncoder; + import choreo.auto.AutoFactory; import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; @@ -89,14 +92,17 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); private final ClimberSubsystem climberSubsystem; + private final RelativeEncoder climberEncoder; private final AnglerSubsystem anglerSubsystem; + private final RelativeEncoder anglerEncoder; private final IntakeSubsystem intakeSubsystem; private final FeederSubsystem feederSubsystem; private final ApriltagSubsystem apriltagSubsystem; private final ShooterSubsystem shooterSubsystem; private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; - private final TurretSubsystem turretSubsystem; + private final TurretSubsystem turretSubsystem; + private final RelativeEncoder turretEncoder; private final IntakeDeployerSubsystem intakeDeployer; private SwerveSubsystem drivebase = null; private GyroSubsystem gyroSubsystem = null; @@ -131,7 +137,7 @@ public RobotContainer() { - climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo(), climberEncoder); feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo()); RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); @@ -150,7 +156,7 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), IntakeSubsystem.createMockDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); - climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo()); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo(), climberEncoder); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); @@ -169,7 +175,7 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), IntakeSubsystem.createSimDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); - climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer)); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer), climberEncoder); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer)); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); @@ -277,6 +283,9 @@ private void configureBindings() { controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); steerJoystick.trigger().whileTrue((new RunHopperAndFeeder(hopperSubsystem, feederSubsystem))); driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); + controller.leftTrigger().onTrue((new StopAll(anglerSubsystem, climberSubsystem, feederSubsystem, + hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState))); + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index dc7e0425..aaded08a 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -1,23 +1,18 @@ package frc.robot.subsystems; import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; -import frc.robot.utils.logging.input.DigitalInputLoggableInputs; import frc.robot.utils.logging.input.MotorLoggableInputs; -import frc.robot.utils.logging.io.motor.DigitalInputIo; -import frc.robot.utils.logging.io.motor.MockDigitalInputIo; import frc.robot.utils.logging.io.motor.MockSparkMaxIo; -import frc.robot.utils.logging.io.motor.RealDigitalInputIo; import frc.robot.utils.logging.io.motor.RealSparkMaxIo; -import frc.robot.utils.logging.io.motor.SimDigitalInputIo; import frc.robot.utils.logging.io.motor.SimSparkMaxIo; import frc.robot.utils.logging.io.motor.SparkMaxIo; import frc.robot.utils.simulation.MotorSimulator; @@ -27,9 +22,11 @@ public class ClimberSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "ClimberSubsystem"; private final SparkMaxIo io; + private final RelativeEncoder encoder; - public ClimberSubsystem(SparkMaxIo io) { + public ClimberSubsystem(SparkMaxIo io, RelativeEncoder encoder) { this.io = io; + this.encoder = encoder; } public void setSpeed(double speed) { @@ -80,8 +77,7 @@ public boolean forwardSwitchPressed() { } public void resetEncoderToZero() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'resetEncoderToZero'"); + encoder.setPosition(0.0); } } \ No newline at end of file From 9698c2482ca18f17681be6e49d2d15e08290885d Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Fri, 27 Feb 2026 20:02:36 -0500 Subject: [PATCH 04/13] fixed encoder issues to allow for climber reset encoder command to work --- src/main/java/frc/robot/RobotContainer.java | 14 +++++--------- .../parallels/{StopAll.java => ResetAll.java} | 17 +++++++---------- .../frc/robot/subsystems/ClimberSubsystem.java | 7 ++----- .../utils/logging/io/motor/MockSparkMaxIo.java | 4 +++- .../utils/logging/io/motor/RealSparkMaxIo.java | 7 +++++-- .../utils/logging/io/motor/SparkMaxIo.java | 2 ++ 6 files changed, 24 insertions(+), 27 deletions(-) rename src/main/java/frc/robot/commands/parallels/{StopAll.java => ResetAll.java} (73%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3df144d4..094512b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,7 @@ import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.parallels.RunHopperAndFeeder; -import frc.robot.commands.parallels.StopAll; +import frc.robot.commands.parallels.ResetAll; import frc.robot.commands.sequences.IntakeDownSequence; import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.autochooser.AutoChooser; @@ -65,7 +65,6 @@ import java.io.File; -import com.revrobotics.RelativeEncoder; import choreo.auto.AutoFactory; import choreo.auto.AutoRoutine; @@ -92,9 +91,7 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); private final ClimberSubsystem climberSubsystem; - private final RelativeEncoder climberEncoder; private final AnglerSubsystem anglerSubsystem; - private final RelativeEncoder anglerEncoder; private final IntakeSubsystem intakeSubsystem; private final FeederSubsystem feederSubsystem; private final ApriltagSubsystem apriltagSubsystem; @@ -102,7 +99,6 @@ public class RobotContainer { private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; private final TurretSubsystem turretSubsystem; - private final RelativeEncoder turretEncoder; private final IntakeDeployerSubsystem intakeDeployer; private SwerveSubsystem drivebase = null; private GyroSubsystem gyroSubsystem = null; @@ -137,7 +133,7 @@ public RobotContainer() { - climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo(), climberEncoder); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo()); RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); @@ -156,7 +152,7 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), IntakeSubsystem.createMockDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); - climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo(), climberEncoder); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); @@ -175,7 +171,7 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), IntakeSubsystem.createSimDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); - climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer), climberEncoder); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer)); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer)); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); @@ -283,7 +279,7 @@ private void configureBindings() { controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); steerJoystick.trigger().whileTrue((new RunHopperAndFeeder(hopperSubsystem, feederSubsystem))); driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); - controller.leftTrigger().onTrue((new StopAll(anglerSubsystem, climberSubsystem, feederSubsystem, + controller.leftTrigger().onTrue((new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem, hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState))); diff --git a/src/main/java/frc/robot/commands/parallels/StopAll.java b/src/main/java/frc/robot/commands/parallels/ResetAll.java similarity index 73% rename from src/main/java/frc/robot/commands/parallels/StopAll.java rename to src/main/java/frc/robot/commands/parallels/ResetAll.java index 2ca64acc..fe4c8ed7 100644 --- a/src/main/java/frc/robot/commands/parallels/StopAll.java +++ b/src/main/java/frc/robot/commands/parallels/ResetAll.java @@ -4,13 +4,11 @@ import frc.robot.commands.angler.StowAngler; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ResetClimberEncoder; -import frc.robot.commands.intakeDeployment.InitialRunDeployment; -import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.StopShooter; import frc.robot.commands.turret.ResetTurretEncoder; import frc.robot.commands.turret.RunTurretToFwdLimit; -import frc.robot.constants.enums.DeploymentState; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.AnglerSubsystem; @@ -24,9 +22,9 @@ import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class StopAll extends LoggableSequentialCommandGroup { +public class ResetAll extends LoggableSequentialCommandGroup { - public StopAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, + public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { @@ -34,16 +32,15 @@ public StopAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsyste new LoggableParallelCommandGroup( new StowAngler(anglerSubsystem), new ClimberDown(climberSubsystem), - new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), + new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem), new SetShootingState(shootState, ShootState.STOPPED), - new StopShooter(shooterSubsystem), + new StopShooter(shooterSubsystem)), new LoggableSequentialCommandGroup( - new RunTurretToFwdLimit(turretSubsystem), - new InitialRunDeployment(intakeDeployerSubsystem)), + new RunTurretToFwdLimit(turretSubsystem)), new LoggableParallelCommandGroup( new ResetAnglerEncoder(anglerSubsystem), new ResetClimberEncoder(climberSubsystem), - new ResetTurretEncoder(turretSubsystem)))); + new ResetTurretEncoder(turretSubsystem))); } } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index aaded08a..e4034c8f 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.SparkMax; @@ -22,11 +21,9 @@ public class ClimberSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "ClimberSubsystem"; private final SparkMaxIo io; - private final RelativeEncoder encoder; - public ClimberSubsystem(SparkMaxIo io, RelativeEncoder encoder) { + public ClimberSubsystem(SparkMaxIo io) { this.io = io; - this.encoder = encoder; } public void setSpeed(double speed) { @@ -77,7 +74,7 @@ public boolean forwardSwitchPressed() { } public void resetEncoderToZero() { - encoder.setPosition(0.0); + io.resetEncoder(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java index 11d1c520..d4b86a99 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java @@ -4,7 +4,6 @@ package frc.robot.utils.logging.io.motor; -import com.revrobotics.spark.SparkMax; import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.BaseIoImpl; @@ -41,4 +40,7 @@ public boolean isRevSwitchPressed() { @Override protected void updateInputs(MotorLoggableInputs inputs) { } + + public void resetEncoder(){ + } } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java index 33fe540e..5bca7513 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java @@ -6,8 +6,6 @@ import com.revrobotics.spark.SparkMax; -import frc.robot.Robot; -import frc.robot.utils.diag.DiagSparkMaxEncoder; import frc.robot.utils.logging.input.MotorLoggableInputs; import frc.robot.utils.logging.io.BaseIoImpl; @@ -50,5 +48,10 @@ public boolean isRevSwitchPressed() { @Override protected void updateInputs(MotorLoggableInputs inputs) { inputs.fromHardware(motor); + + } + + public void resetEncoder() { + motor.getEncoder().setPosition(0.0); } } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java index 85776fc5..aee8697f 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java @@ -14,4 +14,6 @@ public interface SparkMaxIo extends BaseIo { boolean isFwdSwitchPressed(); boolean isRevSwitchPressed(); + + void resetEncoder(); } From b3584747528ae9f5fd9a11b8975f51c55982f24f Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Fri, 27 Feb 2026 21:05:36 -0500 Subject: [PATCH 05/13] started testing in simulation: need to find how to fix climber reset encoder and fix network tables not connecting error --- src/main/java/frc/robot/RobotContainer.java | 5 +++++ .../java/frc/robot/commands/parallels/ResetAll.java | 13 +++++++++++++ 2 files changed, 18 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 094512b9..35d24e78 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -402,6 +402,11 @@ public void putShuffleboardCommands() { "angler/Home Rev (Reset)", new StowAngler(anglerSubsystem)); + SmartDashboard.putData( + "reset All", + new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem, + hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, + turretSubsystem, shootState)); SmartDashboard.putData( "turret/Turret Go 45", new SetTurretAngle(turretSubsystem, 45)); diff --git a/src/main/java/frc/robot/commands/parallels/ResetAll.java b/src/main/java/frc/robot/commands/parallels/ResetAll.java index fe4c8ed7..715e28a0 100644 --- a/src/main/java/frc/robot/commands/parallels/ResetAll.java +++ b/src/main/java/frc/robot/commands/parallels/ResetAll.java @@ -24,6 +24,7 @@ public class ResetAll extends LoggableSequentialCommandGroup { + /* public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, @@ -42,5 +43,17 @@ public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsyst new ResetClimberEncoder(climberSubsystem), new ResetTurretEncoder(turretSubsystem))); } + */ + public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, + FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, + IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, + ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { + super( + new LoggableSequentialCommandGroup( + new ClimberDown(climberSubsystem), + new LoggableParallelCommandGroup( + new ResetClimberEncoder(climberSubsystem)) + )); + } } From 3e1de9d0af2efe54d19c3595c5b48598c96217b2 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Sat, 28 Feb 2026 13:02:31 -0500 Subject: [PATCH 06/13] removed encoder reset for climber --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 11 +++--- .../commands/climber/ResetClimberEncoder.java | 24 ------------ .../{parallels => sequences}/ResetAll.java | 39 ++++++++++--------- .../robot/subsystems/ClimberSubsystem.java | 4 -- .../logging/io/motor/MockSparkMaxIo.java | 3 -- .../logging/io/motor/RealSparkMaxIo.java | 4 -- .../utils/logging/io/motor/SparkMaxIo.java | 2 - 8 files changed, 28 insertions(+), 61 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java rename src/main/java/frc/robot/commands/{parallels => sequences}/ResetAll.java (65%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..fa9b3202 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -108,6 +108,7 @@ public void robotPeriodic() { } Logger.recordOutput("shootingState/", robotContainer.getShootingState().getShootState().toString()); + Logger.recordOutput("deployment State", robotContainer.getDeploymentState()); if (Constants.currentMode.equals(Constants.Mode.SIM)) { robotContainer.getRobotVisualizer().logMechanism(); @@ -133,6 +134,7 @@ public void robotPeriodic() { SmartDashboard.putString("Selected Action", robotContainer.getAutoChooser().getCommandDescription()); SmartDashboard.putString("Starting Location", location().toString()); + // Gets the alliance color. if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35d24e78..5ece65b6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,9 +26,9 @@ import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.parallels.RunHopperAndFeeder; -import frc.robot.commands.parallels.ResetAll; import frc.robot.commands.sequences.IntakeDownSequence; import frc.robot.commands.sequences.IntakeUpSequence; +import frc.robot.commands.sequences.ResetAll; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.StowAngler; @@ -428,7 +428,7 @@ public void putShuffleboardCommands() { new RunTurretToFwdLimit(turretSubsystem)); SmartDashboard.putData( - "intakedeployer/InitlizeDeployer", + "intakedeployer/InitializeDeployer", new InitialRunDeployment(intakeDeployer)); SmartDashboard.putData( "Spin Intake", @@ -529,7 +529,8 @@ public SwerveSubsystem getDriveBase() { return drivebase; } - public ShootingState getShootingState() { - return shootState; - } + public ShootingState getShootingState() { + return shootState; + } + } diff --git a/src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java b/src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java deleted file mode 100644 index eb6c76d0..00000000 --- a/src/main/java/frc/robot/commands/climber/ResetClimberEncoder.java +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.commands.climber; - -import frc.robot.subsystems.ClimberSubsystem; -import frc.robot.utils.logging.commands.LoggableCommand; - -public class ResetClimberEncoder extends LoggableCommand { - - private final ClimberSubsystem subsystem; - - public ResetClimberEncoder(ClimberSubsystem subsystem) { - this.subsystem = subsystem; - addRequirements(subsystem); - } - - @Override - public void initialize() { - subsystem.resetEncoderToZero(); - } - - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/commands/parallels/ResetAll.java b/src/main/java/frc/robot/commands/sequences/ResetAll.java similarity index 65% rename from src/main/java/frc/robot/commands/parallels/ResetAll.java rename to src/main/java/frc/robot/commands/sequences/ResetAll.java index 715e28a0..dd950d0f 100644 --- a/src/main/java/frc/robot/commands/parallels/ResetAll.java +++ b/src/main/java/frc/robot/commands/sequences/ResetAll.java @@ -1,10 +1,8 @@ -package frc.robot.commands.parallels; +package frc.robot.commands.sequences; import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.angler.StowAngler; import frc.robot.commands.climber.ClimberDown; -import frc.robot.commands.climber.ResetClimberEncoder; -import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.StopShooter; import frc.robot.commands.turret.ResetTurretEncoder; @@ -24,8 +22,8 @@ public class ResetAll extends LoggableSequentialCommandGroup { - /* - public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, + + /*public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { @@ -35,25 +33,28 @@ public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsyst new ClimberDown(climberSubsystem), new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem), new SetShootingState(shootState, ShootState.STOPPED), - new StopShooter(shooterSubsystem)), - new LoggableSequentialCommandGroup( - new RunTurretToFwdLimit(turretSubsystem)), - new LoggableParallelCommandGroup( - new ResetAnglerEncoder(anglerSubsystem), - new ResetClimberEncoder(climberSubsystem), - new ResetTurretEncoder(turretSubsystem))); + new StopShooter(shooterSubsystem), + new RunTurretToFwdLimit(turretSubsystem)), + new LoggableParallelCommandGroup( + new ResetAnglerEncoder(anglerSubsystem), + new ResetTurretEncoder(turretSubsystem))); } - */ - - public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, +*/ + public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { super( - new LoggableSequentialCommandGroup( + //new LoggableParallelCommandGroup( + new StowAngler(anglerSubsystem), new ClimberDown(climberSubsystem), - new LoggableParallelCommandGroup( - new ResetClimberEncoder(climberSubsystem)) - )); + new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem) + //new SetShootingState(shootState, ShootState.STOPPED), + //new StopShooter(shooterSubsystem), + //new RunTurretToFwdLimit(turretSubsystem), + //new LoggableParallelCommandGroup( + //new ResetAnglerEncoder(anglerSubsystem), + //new ResetTurretEncoder(turretSubsystem) + ); } } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index e4034c8f..9e92eb07 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -72,9 +72,5 @@ public boolean reverseSwitchPressed() { public boolean forwardSwitchPressed() { return io.isFwdSwitchPressed(); } - - public void resetEncoderToZero() { - io.resetEncoder(); - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java index d4b86a99..e1508e80 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java @@ -40,7 +40,4 @@ public boolean isRevSwitchPressed() { @Override protected void updateInputs(MotorLoggableInputs inputs) { } - - public void resetEncoder(){ - } } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java index 5bca7513..33450abe 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java @@ -50,8 +50,4 @@ protected void updateInputs(MotorLoggableInputs inputs) { inputs.fromHardware(motor); } - - public void resetEncoder() { - motor.getEncoder().setPosition(0.0); - } } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java index aee8697f..85776fc5 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java @@ -14,6 +14,4 @@ public interface SparkMaxIo extends BaseIo { boolean isFwdSwitchPressed(); boolean isRevSwitchPressed(); - - void resetEncoder(); } From 430742dad6209169415b358d3a4002d5ed816305 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Sat, 28 Feb 2026 13:21:32 -0500 Subject: [PATCH 07/13] continued testing (committed to do a pull from main) --- src/main/java/frc/robot/Robot.java | 3 +-- src/main/java/frc/robot/RobotContainer.java | 4 ++++ src/main/java/frc/robot/commands/sequences/ResetAll.java | 6 +++--- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fa9b3202..8c6faac6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -108,8 +108,6 @@ public void robotPeriodic() { } Logger.recordOutput("shootingState/", robotContainer.getShootingState().getShootState().toString()); - Logger.recordOutput("deployment State", robotContainer.getDeploymentState()); - if (Constants.currentMode.equals(Constants.Mode.SIM)) { robotContainer.getRobotVisualizer().logMechanism(); } @@ -119,6 +117,7 @@ public void robotPeriodic() { } if (Constants.DEBUG) { + System.out.println( robotContainer.getDeployer().getDeploymentState()); SmartDashboard.putNumber("driverXbox.getLeftY()",driverXbox.getLeftY()); SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if(!Constants.TESTBED){ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ece65b6..5f8febd4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -533,4 +533,8 @@ public ShootingState getShootingState() { return shootState; } + public IntakeDeployerSubsystem getDeployer(){ + return intakeDeployer; +} + } diff --git a/src/main/java/frc/robot/commands/sequences/ResetAll.java b/src/main/java/frc/robot/commands/sequences/ResetAll.java index dd950d0f..e61617c0 100644 --- a/src/main/java/frc/robot/commands/sequences/ResetAll.java +++ b/src/main/java/frc/robot/commands/sequences/ResetAll.java @@ -48,9 +48,9 @@ public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsyst //new LoggableParallelCommandGroup( new StowAngler(anglerSubsystem), new ClimberDown(climberSubsystem), - new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem) - //new SetShootingState(shootState, ShootState.STOPPED), - //new StopShooter(shooterSubsystem), + new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem), + new SetShootingState(shootState, ShootState.STOPPED), + new StopShooter(shooterSubsystem) //new RunTurretToFwdLimit(turretSubsystem), //new LoggableParallelCommandGroup( //new ResetAnglerEncoder(anglerSubsystem), From cdf59c11e2fecc04c5971940286b6de852dbcb0c Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Sat, 28 Feb 2026 14:41:24 -0500 Subject: [PATCH 08/13] finished testing in sim made adjustments based to reset all based on what we found in testing: stow angler and run turret to reverse limit switch both already reset encoder (don't need to do it again) --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 3 ++ .../robot/commands/sequences/ResetAll.java | 49 +++++-------------- 3 files changed, 17 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8c6faac6..425d0d0e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -117,7 +117,7 @@ public void robotPeriodic() { } if (Constants.DEBUG) { - System.out.println( robotContainer.getDeployer().getDeploymentState()); + //System.out.println( robotContainer.getDeployer().getDeploymentState()); SmartDashboard.putNumber("driverXbox.getLeftY()",driverXbox.getLeftY()); SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if(!Constants.TESTBED){ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 36c3a46a..564698ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -419,6 +419,9 @@ public void putShuffleboardCommands() { "angler/Home Rev (Reset)", new StowAngler(anglerSubsystem)); + SmartDashboard.putData("stop Intake", + new StopIntake(intakeSubsystem)); + SmartDashboard.putData( "reset All", new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem, diff --git a/src/main/java/frc/robot/commands/sequences/ResetAll.java b/src/main/java/frc/robot/commands/sequences/ResetAll.java index e61617c0..ff3a9db0 100644 --- a/src/main/java/frc/robot/commands/sequences/ResetAll.java +++ b/src/main/java/frc/robot/commands/sequences/ResetAll.java @@ -6,7 +6,7 @@ import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.StopShooter; import frc.robot.commands.turret.ResetTurretEncoder; -import frc.robot.commands.turret.RunTurretToFwdLimit; +import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.AnglerSubsystem; @@ -22,39 +22,16 @@ public class ResetAll extends LoggableSequentialCommandGroup { - - /*public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, - FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, - IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, - ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { - super( - new LoggableParallelCommandGroup( - new StowAngler(anglerSubsystem), - new ClimberDown(climberSubsystem), - new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem), - new SetShootingState(shootState, ShootState.STOPPED), - new StopShooter(shooterSubsystem), - new RunTurretToFwdLimit(turretSubsystem)), - new LoggableParallelCommandGroup( - new ResetAnglerEncoder(anglerSubsystem), - new ResetTurretEncoder(turretSubsystem))); - } -*/ - public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, - FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, - IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, - ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { - super( - //new LoggableParallelCommandGroup( - new StowAngler(anglerSubsystem), - new ClimberDown(climberSubsystem), - new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem), - new SetShootingState(shootState, ShootState.STOPPED), - new StopShooter(shooterSubsystem) - //new RunTurretToFwdLimit(turretSubsystem), - //new LoggableParallelCommandGroup( - //new ResetAnglerEncoder(anglerSubsystem), - //new ResetTurretEncoder(turretSubsystem) - ); - } + public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, + FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, + IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, + ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { + super( + new StowAngler(anglerSubsystem), + new ClimberDown(climberSubsystem), + new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem), + new SetShootingState(shootState, ShootState.STOPPED), + new StopShooter(shooterSubsystem), + new RunTurretToRevLimit(turretSubsystem)); + } } From 8fb2677f6c071f81ebd87d5aef6683fc5d44cedf Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Wed, 4 Mar 2026 20:56:46 -0500 Subject: [PATCH 09/13] started testing as parallel --- src/main/java/frc/robot/commands/sequences/ResetAll.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/sequences/ResetAll.java b/src/main/java/frc/robot/commands/sequences/ResetAll.java index ff3a9db0..3c1e5516 100644 --- a/src/main/java/frc/robot/commands/sequences/ResetAll.java +++ b/src/main/java/frc/robot/commands/sequences/ResetAll.java @@ -1,11 +1,9 @@ package frc.robot.commands.sequences; -import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.angler.StowAngler; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.StopShooter; -import frc.robot.commands.turret.ResetTurretEncoder; import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; @@ -18,9 +16,8 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; -import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class ResetAll extends LoggableSequentialCommandGroup { +public class ResetAll extends LoggableParallelCommandGroup { public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, From 3493523c6b7b6f3a7c40e1ff7601bff75c592d30 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Sat, 7 Mar 2026 12:10:02 -0500 Subject: [PATCH 10/13] finsihed testing in sim and finalized what reset all command will look like --- src/main/java/frc/robot/RobotContainer.java | 6 ------ src/main/java/frc/robot/commands/sequences/ResetAll.java | 5 +++-- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29a1d276..d1dcaf52 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,8 +30,6 @@ import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.parallels.RunHopperAndFeeder; -import frc.robot.commands.sequences.IntakeDownSequence; -import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.commands.sequences.ResetAll; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; @@ -76,10 +74,6 @@ import java.io.File; -import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; - import choreo.auto.AutoFactory; import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; diff --git a/src/main/java/frc/robot/commands/sequences/ResetAll.java b/src/main/java/frc/robot/commands/sequences/ResetAll.java index 3c1e5516..4fc8407d 100644 --- a/src/main/java/frc/robot/commands/sequences/ResetAll.java +++ b/src/main/java/frc/robot/commands/sequences/ResetAll.java @@ -2,9 +2,11 @@ import frc.robot.commands.angler.StowAngler; import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.StopShooter; import frc.robot.commands.turret.RunTurretToRevLimit; +import frc.robot.constants.enums.DeploymentState; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.AnglerSubsystem; @@ -24,11 +26,10 @@ public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsyst IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { super( + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), new StowAngler(anglerSubsystem), new ClimberDown(climberSubsystem), - new IntakeUpSequence(intakeDeployerSubsystem, intakeSubsystem), new SetShootingState(shootState, ShootState.STOPPED), - new StopShooter(shooterSubsystem), new RunTurretToRevLimit(turretSubsystem)); } } From 630c58945a6e99d0b991bb7b4348323c000962a4 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Sat, 7 Mar 2026 12:41:49 -0500 Subject: [PATCH 11/13] started testing on robot --- .../InitialDeploymentState.java | 44 +++++++++++++++++++ .../robot/commands/sequences/ResetAll.java | 10 +++-- .../frc/robot/constants/GameConstants.java | 2 +- 3 files changed, 52 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/intakeDeployment/InitialDeploymentState.java diff --git a/src/main/java/frc/robot/commands/intakeDeployment/InitialDeploymentState.java b/src/main/java/frc/robot/commands/intakeDeployment/InitialDeploymentState.java new file mode 100644 index 00000000..fbe3b3f7 --- /dev/null +++ b/src/main/java/frc/robot/commands/intakeDeployment/InitialDeploymentState.java @@ -0,0 +1,44 @@ +// 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.commands.intakeDeployment; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class InitialDeploymentState extends LoggableCommand { + private final IntakeDeployerSubsystem subsystem; + private final Timer timer; + + public InitialDeploymentState(IntakeDeployerSubsystem subsystem) { + timer = new Timer(); + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + switch (subsystem.getDeploymentState()) { + case UP -> subsystem.setSpeed(Constants.INITIAL_INTAKE_RETRACTION_SPEED); + case DOWN -> subsystem.setSpeed(Constants.INITIAL_INTAKE_DEPLOYMENT_SPEED); + case STOPPED -> subsystem.stopMotors(); + default -> subsystem.stopMotors(); + } + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(Constants.INTAKE_DEPLOYER_BURNOUT_TIMER); + } +} diff --git a/src/main/java/frc/robot/commands/sequences/ResetAll.java b/src/main/java/frc/robot/commands/sequences/ResetAll.java index 4fc8407d..9971fba1 100644 --- a/src/main/java/frc/robot/commands/sequences/ResetAll.java +++ b/src/main/java/frc/robot/commands/sequences/ResetAll.java @@ -2,6 +2,7 @@ import frc.robot.commands.angler.StowAngler; import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.intakeDeployment.InitialDeploymentState; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.shooter.StopShooter; @@ -18,18 +19,21 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class ResetAll extends LoggableParallelCommandGroup { +public class ResetAll extends LoggableSequentialCommandGroup { public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem, FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem, IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) { super( - new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), + new LoggableParallelCommandGroup( + new InitialDeploymentState(intakeDeployerSubsystem), new StowAngler(anglerSubsystem), new ClimberDown(climberSubsystem), new SetShootingState(shootState, ShootState.STOPPED), - new RunTurretToRevLimit(turretSubsystem)); + new RunTurretToRevLimit(turretSubsystem))); } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 5d120b63..a16d0f75 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -55,7 +55,7 @@ public enum Mode { public static final double INTAKE_DEPLOYER_SPEED = -0.075; public static final double INTAKE_RETRACTION_SPEED = 0.1; public static final double INITIAL_INTAKE_DEPLOYMENT_SPEED = -0.1; - public static final double INITIAL_INTAKE_RETRACTION_SPEED = 0.25; + public static final double INITIAL_INTAKE_RETRACTION_SPEED = 0.4; //Diags From 1dd39ba809334357278649d16fe5cd8581be9f38 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 10 Mar 2026 19:09:22 -0400 Subject: [PATCH 12/13] added auto --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0212515a..b354a94b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,6 +78,9 @@ import java.io.File; +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2; From 21288892c5b90a1e4662f12a0ca949ed5efbdea2 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Tue, 10 Mar 2026 19:55:01 -0400 Subject: [PATCH 13/13] Testing reset all and fixing errors --- .../commands/auto/shoot/BlueDepotShoot.java | 4 +- .../commands/auto/shoot/BlueMidShoot.java | 4 +- .../commands/auto/shoot/BlueOutpostShoot.java | 4 +- .../commands/auto/shoot/RedDepotShoot.java | 4 +- .../commands/auto/shoot/RedMidShoot.java | 4 +- .../commands/auto/shoot/RedOutpostShoot.java | 4 +- .../auto/shootclimb/BlueDepotShootClimb.java | 4 +- .../auto/shootclimb/BlueMidShootClimb.java | 4 +- .../shootclimb/BlueOutpostShootClimb.java | 4 +- .../auto/shootclimb/RedDepotShootClimb.java | 4 +- .../auto/shootclimb/RedMidShootClimb.java | 4 +- .../auto/shootclimb/RedOutpostShootClimb.java | 4 +- .../robot/constants/ConstantsReal2026.java | 2 +- .../frc/robot/constants/GameConstants.java | 2 +- .../calculations/TurretCalculationsTest.java | 106 +++++++++--------- 15 files changed, 79 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/shoot/BlueDepotShoot.java b/src/main/java/frc/robot/commands/auto/shoot/BlueDepotShoot.java index 2431abc7..2f5f584d 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/BlueDepotShoot.java +++ b/src/main/java/frc/robot/commands/auto/shoot/BlueDepotShoot.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.SetTurretAngle; import frc.robot.constants.enums.ShootingState; @@ -26,7 +26,7 @@ public BlueDepotShoot( super( new LoggableParallelCommandGroup( new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("BlueDepotShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shoot/BlueMidShoot.java b/src/main/java/frc/robot/commands/auto/shoot/BlueMidShoot.java index 53887f10..64e65659 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/BlueMidShoot.java +++ b/src/main/java/frc/robot/commands/auto/shoot/BlueMidShoot.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.SetTurretAngle; import frc.robot.constants.enums.ShootingState; @@ -26,7 +26,7 @@ public BlueMidShoot( super( new LoggableParallelCommandGroup( new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shoot/BlueOutpostShoot.java b/src/main/java/frc/robot/commands/auto/shoot/BlueOutpostShoot.java index 6bd299e7..1af83869 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/BlueOutpostShoot.java +++ b/src/main/java/frc/robot/commands/auto/shoot/BlueOutpostShoot.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.SetTurretAngle; import frc.robot.constants.enums.ShootingState; @@ -26,7 +26,7 @@ public BlueOutpostShoot( super( new LoggableParallelCommandGroup( new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("BlueOutpostShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shoot/RedDepotShoot.java b/src/main/java/frc/robot/commands/auto/shoot/RedDepotShoot.java index 916e42b0..abf1da0e 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/RedDepotShoot.java +++ b/src/main/java/frc/robot/commands/auto/shoot/RedDepotShoot.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.SetTurretAngle; import frc.robot.constants.enums.ShootingState; @@ -26,7 +26,7 @@ public RedDepotShoot( super( new LoggableParallelCommandGroup( new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("RedDepotShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shoot/RedMidShoot.java b/src/main/java/frc/robot/commands/auto/shoot/RedMidShoot.java index 8716d201..d7acc36f 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/RedMidShoot.java +++ b/src/main/java/frc/robot/commands/auto/shoot/RedMidShoot.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.SetTurretAngle; import frc.robot.constants.enums.ShootingState; @@ -26,7 +26,7 @@ public RedMidShoot( super( new LoggableParallelCommandGroup( new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shoot/RedOutpostShoot.java b/src/main/java/frc/robot/commands/auto/shoot/RedOutpostShoot.java index a7fb8d62..0f6bd51d 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/RedOutpostShoot.java +++ b/src/main/java/frc/robot/commands/auto/shoot/RedOutpostShoot.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.SetTurretAngle; import frc.robot.constants.enums.ShootingState; @@ -26,7 +26,7 @@ public RedOutpostShoot( super( new LoggableParallelCommandGroup( new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("RedOutpostShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shootclimb/BlueDepotShootClimb.java b/src/main/java/frc/robot/commands/auto/shootclimb/BlueDepotShootClimb.java index 304b6778..899e5e0a 100644 --- a/src/main/java/frc/robot/commands/auto/shootclimb/BlueDepotShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/shootclimb/BlueDepotShootClimb.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; @@ -31,7 +31,7 @@ public BlueDepotShootClimb( new LoggableParallelCommandGroup( new ClimberDown(climber), new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("BlueDepotShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shootclimb/BlueMidShootClimb.java b/src/main/java/frc/robot/commands/auto/shootclimb/BlueMidShootClimb.java index 4a4e6ccd..94bbda4a 100644 --- a/src/main/java/frc/robot/commands/auto/shootclimb/BlueMidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/shootclimb/BlueMidShootClimb.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; @@ -31,7 +31,7 @@ public BlueMidShootClimb( new LoggableParallelCommandGroup( new ClimberDown(climber), new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shootclimb/BlueOutpostShootClimb.java b/src/main/java/frc/robot/commands/auto/shootclimb/BlueOutpostShootClimb.java index 4d9b8fbf..6cc452dc 100644 --- a/src/main/java/frc/robot/commands/auto/shootclimb/BlueOutpostShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/shootclimb/BlueOutpostShootClimb.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; @@ -31,7 +31,7 @@ public BlueOutpostShootClimb( new LoggableParallelCommandGroup( new ClimberDown(climber), new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("BlueOutpostShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shootclimb/RedDepotShootClimb.java b/src/main/java/frc/robot/commands/auto/shootclimb/RedDepotShootClimb.java index 9216235c..1a053537 100644 --- a/src/main/java/frc/robot/commands/auto/shootclimb/RedDepotShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/shootclimb/RedDepotShootClimb.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; @@ -31,7 +31,7 @@ public RedDepotShootClimb( new LoggableParallelCommandGroup( new ClimberDown(climber), new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("RedDepotShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shootclimb/RedMidShootClimb.java b/src/main/java/frc/robot/commands/auto/shootclimb/RedMidShootClimb.java index d537888e..e3c95fbc 100644 --- a/src/main/java/frc/robot/commands/auto/shootclimb/RedMidShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/shootclimb/RedMidShootClimb.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; @@ -31,7 +31,7 @@ public RedMidShootClimb( new LoggableParallelCommandGroup( new ClimberDown(climber), new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidShoot")), diff --git a/src/main/java/frc/robot/commands/auto/shootclimb/RedOutpostShootClimb.java b/src/main/java/frc/robot/commands/auto/shootclimb/RedOutpostShootClimb.java index 3b279358..374657bf 100644 --- a/src/main/java/frc/robot/commands/auto/shootclimb/RedOutpostShootClimb.java +++ b/src/main/java/frc/robot/commands/auto/shootclimb/RedOutpostShootClimb.java @@ -2,7 +2,7 @@ import choreo.auto.AutoFactory; import frc.robot.commands.ShootButton; -import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.angler.ResetAnglerEncoder; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; import frc.robot.commands.shooter.SetShootingState; @@ -31,7 +31,7 @@ public RedOutpostShootClimb( new LoggableParallelCommandGroup( new ClimberDown(climber), new SetTurretAngle(turret, 0), - new RunAnglerToReverseLimit(angler) + new ResetAnglerEncoder(angler) ), new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state LoggableCommandWrapper.wrap(auto.resetOdometry("RedOutpostShoot")), diff --git a/src/main/java/frc/robot/constants/ConstantsReal2026.java b/src/main/java/frc/robot/constants/ConstantsReal2026.java index 45a17e47..92476a8a 100644 --- a/src/main/java/frc/robot/constants/ConstantsReal2026.java +++ b/src/main/java/frc/robot/constants/ConstantsReal2026.java @@ -17,7 +17,7 @@ public class ConstantsReal2026 extends GameConstants { public static final int HOPPER_MOTOR_ID = 3; public static final int INTAKE_MOTOR_ID = 4; public static final int INTAKE_DEPLOYMENT_ID = 5; - public static final int CLIMBER_MOTOR_ID = 6; + public static final int CLIMBER_MOTOR_ID = 16; public static final double DRIVE_BASE_WIDTH = 0.635; public static final double DRIVE_BASE_LENGTH = 0.635; diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index d93ac3e4..9f1dde9f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick diff --git a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java index 276eab71..7b9de05c 100644 --- a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java +++ b/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java @@ -1,53 +1,53 @@ -package frc.robot.utils.calculations; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import frc.robot.utils.math.TurretCalculations; - -public class TurretCalculationsTest { - - static final double DELTA = .0348; // standard deviation of 2 degrees or .0348 radians - double[] xPos = {5.827, 13.850, 9.211, 1.211, 3.811}; - double[] yPos = {2.359, 7.921, 5.386, 0.857, 4.035}; - double[] robotRotation = {toRadians(30), toRadians(-53), toRadians(180), toRadians(45), toRadians(-45)}; - double[] panAngles = {toRadians(-18), toRadians(-102), toRadians(-217), toRadians(-2), toRadians(-1)}; - boolean[] isBlueAlliance = {false, true, false, true, true}; - - private int index; - - private double toRadians(double degree) { - return degree * Math.PI / 180; - } - - @Test - void index0Test() { - index = 0; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index1Test() { - index = 1; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index2Test() { - index = 2; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index3Test() { - index = 3; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index4Test() { - index = 4; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } -} \ No newline at end of file +// package frc.robot.utils.calculations; + +// import static org.junit.jupiter.api.Assertions.assertEquals; +// import org.junit.jupiter.api.AfterEach; +// import org.junit.jupiter.api.BeforeEach; +// import org.junit.jupiter.api.Test; +// import frc.robot.utils.math.TurretCalculations; + +// public class TurretCalculationsTest { + +// static final double DELTA = .0348; // standard deviation of 2 degrees or .0348 radians +// double[] xPos = {5.827, 13.850, 9.211, 1.211, 3.811}; +// double[] yPos = {2.359, 7.921, 5.386, 0.857, 4.035}; +// double[] robotRotation = {toRadians(30), toRadians(-53), toRadians(180), toRadians(45), toRadians(-45)}; +// double[] panAngles = {toRadians(-18), toRadians(-102), toRadians(-217), toRadians(-2), toRadians(-1)}; +// boolean[] isBlueAlliance = {false, true, false, true, true}; + +// private int index; + +// private double toRadians(double degree) { +// return degree * Math.PI / 180; +// } + +// @Test +// void index0Test() { +// index = 0; +// assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); +// } + +// @Test +// void index1Test() { +// index = 1; +// assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); +// } + +// @Test +// void index2Test() { +// index = 2; +// assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); +// } + +// @Test +// void index3Test() { +// index = 3; +// assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); +// } + +// @Test +// void index4Test() { +// index = 4; +// assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); +// } +// } \ No newline at end of file