From cd49f1dd3efab6bbe44f323c030a7ce63c3b441b Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Sat, 30 Mar 2024 23:53:10 -0400 Subject: [PATCH] ShootWhenReadyAuton and Container work --- src/main/java/frc/robot/RobotContainer.java | 9 +--- .../robot/commands/ShootWhenReadyAuton.java | 41 ++++++++----------- src/main/java/frc/robot/subsystems/Gyro.java | 6 +-- 3 files changed, 20 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0d3a677..76b2559 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -129,9 +129,8 @@ public RobotContainer() { NamedCommands.registerCommand("ShootClose", new ShootClose(arm, index, intake, shooter)); NamedCommands.registerCommand("ShootFromStage", shootFromStage); NamedCommands.registerCommand("Intake", new IntakeIndex(index, intake)); - NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton); // Autonomous + NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton); NamedCommands.registerCommand("SetFlywheelToIdle", setShooterIdle); - // Constants.ArmConstants.ARM_MID_POSE)); /* * Auto Chooser @@ -204,16 +203,13 @@ private void configureBindings() { // TODO Test "setShooterVelocity" only, then remove // m_driverController.rightTrigger().whileTrue(setShooterVelocity); - // TODO Testing "autonShootWhenReady" Remove later - m_driverController.leftTrigger().whileTrue(shootWhenReadyAuton); - // TODO Test MotionMagicVelocityVoltage" only, then remove // m_driverController.leftTrigger().whileTrue(new InstantCommand(() -> shooter2.setFlywheelVelocityMM(shooter2.FLYWHEEL_VELOCITY))); // TODO Remove comment out after testing the MotionMagic Velocity Voltage // m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75)); - /* TODO These are the rrevious event commands and bindings */ + /* TODO These are the previous event commands and bindings */ // m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter)); // m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0))); @@ -243,7 +239,6 @@ public void DebugMethodSingle() { // #endregion Testing } - public Command getAutonomousCommand() { return autoChooser.getSelected(); } diff --git a/src/main/java/frc/robot/commands/ShootWhenReadyAuton.java b/src/main/java/frc/robot/commands/ShootWhenReadyAuton.java index 141e557..1a958cc 100644 --- a/src/main/java/frc/robot/commands/ShootWhenReadyAuton.java +++ b/src/main/java/frc/robot/commands/ShootWhenReadyAuton.java @@ -30,33 +30,20 @@ public void initialize() { @Override public void execute() { - /* - Turn on the shooter with setTargetFlywheelVelocity - After the flywheel is at target velocity, index the game piece forward - After index.HasCargo() is false, stop the shooter and index - */ - + // Turn on the shooter with `setFlywheelVelocity` shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_VELOCITY); // Check if the flywheel is at target velocity AND if a game piece is loaded if (shooter2.isFlywheelNominal3() && notesensor.isNoteLoaded()) { - // If the flywheel is at target velocity AND a game piece is loaded, index the game piece forward + // If the flywheel is at target velocity AND a game piece is loaded, index the game piece forward into the flywheel index.setPower(Constants.IndexConstants.INDEX_POWER); - /* If a game piece is not loaded, - * Set flywheel to idle speed - * Stop the indexer */ - /* - } else if (!notesensor.isNoteLoaded()) { - index.setPower(0); - shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_IDLE_VELOCITY); - */ - - // Has to be called a second time } else { + // Stop the indexer and continue to power the flywheel index.stopIndexing(); + shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_VELOCITY); } // end of if statement @@ -64,19 +51,23 @@ public void execute() { @Override public void end(boolean interrupted) { + /* After the game piece is launched, the setFlywheelVelocity should be 0 and the index should have power 0 */ shooter2.setFlywheelVelocity(0); + index.stopIndexing(); } @Override public boolean isFinished() { - // If a note is not loaded, i.e. it has been fired, then finish the command - if (!notesensor.isNoteLoaded()) { - index.setPower(0); - shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_IDLE_VELOCITY); - return true; - } - // Note is still loaded, keep running the command - return false; + /* + // If a note is not loaded, i.e. it has been fired, then finish the command + if (!notesensor.isNoteLoaded()) { + index.setPower(0); + shooter2.setFlywheelVelocity(shooter2.FLYWHEEL_IDLE_VELOCITY); + return true; + } + // Note is still loaded, keep running the command + */ + return !notesensor.isNoteLoaded(); } } // End of AutonShootWhenReady \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java index 1c76706..b15671f 100644 --- a/src/main/java/frc/robot/subsystems/Gyro.java +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -1,13 +1,11 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.MountPoseConfigs; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; + +@SuppressWarnings("unused") public class Gyro {