diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5c5e5232..b354a94b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,10 +32,11 @@ import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.parallels.RunHopperAndFeeder; +import frc.robot.commands.sequences.ResetAll; 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.angler.StowAngler; import frc.robot.commands.shooter.DefaultShooterControl; import frc.robot.commands.auto.ExampleAuto; import frc.robot.commands.shooter.SetShootingState; @@ -299,6 +300,9 @@ private void configureBindings() { controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); + controller.leftTrigger().onTrue((new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem, + hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState))); + if (controllerSubsystem != null) { steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem)); } @@ -419,8 +423,16 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "angler/Home Rev (Reset)", - new RunAnglerToReverseLimit(anglerSubsystem)); + new StowAngler(anglerSubsystem)); + + SmartDashboard.putData("stop Intake", + new StopIntake(intakeSubsystem)); + SmartDashboard.putData( + "reset All", + new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem, + hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, + turretSubsystem, shootState)); SmartDashboard.putData( "turret/TurretTest/Turret Go 45", new SetTurretAngle(turretSubsystem, 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/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/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/intakeDeployment/ToggleDeployment.java b/src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.java index 256ec20a..c2a90e7e 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.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/sequences/ResetAll.java b/src/main/java/frc/robot/commands/sequences/ResetAll.java new file mode 100644 index 00000000..9971fba1 --- /dev/null +++ b/src/main/java/frc/robot/commands/sequences/ResetAll.java @@ -0,0 +1,39 @@ +package frc.robot.commands.sequences; + +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; +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; +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 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 LoggableParallelCommandGroup( + new InitialDeploymentState(intakeDeployerSubsystem), + new StowAngler(anglerSubsystem), + new ClimberDown(climberSubsystem), + new SetShootingState(shootState, ShootState.STOPPED), + new RunTurretToRevLimit(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/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/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index f08649dd..a84d40aa 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -7,7 +7,6 @@ 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.Robot; import frc.robot.constants.Constants; @@ -16,12 +15,8 @@ import frc.robot.utils.diag.DiagSparkMaxSwitch; 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; 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; } 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..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 @@ -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; 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..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 @@ -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,6 @@ public boolean isRevSwitchPressed() { @Override protected void updateInputs(MotorLoggableInputs inputs) { inputs.fromHardware(motor); + } } 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