From 5d3974355a3db09c0696632ec9ba6f2d60e1ed93 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Mon, 30 Mar 2026 20:27:47 -0400 Subject: [PATCH 01/10] Co-authored-by: Brandon Co-authored-by: TheGreatPintoJ --- motor-ids.txt | 9 +++ src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 45 ++++++++++----- .../subsystems/indexer/IndexerConstants.java | 17 ++---- .../subsystems/indexer/IndexerSubsystem.java | 57 ++++++++++--------- .../subsystems/shooter/ShooterConstants.java | 27 ++++++--- .../subsystems/shooter/ShooterSubsystem.java | 4 +- 7 files changed, 99 insertions(+), 66 deletions(-) create mode 100644 motor-ids.txt diff --git a/motor-ids.txt b/motor-ids.txt new file mode 100644 index 0000000..fc2ebeb --- /dev/null +++ b/motor-ids.txt @@ -0,0 +1,9 @@ +Intake + Roller 2 + Extension 1 +Indexer + Main: 4 + Accelerator: 3 +Shooter + Flywheel - x3: 5,6,7 + Hood: 8 \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68f19c6..7dd70f9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -106,6 +106,7 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + /* m_autonomousCommand = m_robotContainer.getAutonomousCommand(); LaunchState.getInstance().setTargetPose3d(Constants.FieldConstants.getHubTarget()); @@ -113,6 +114,7 @@ public void autonomousInit() { if (m_autonomousCommand != null) { CommandScheduler.getInstance().schedule(m_autonomousCommand); } + */ } /** This function is called periodically during autonomous. */ @@ -140,8 +142,8 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); SignalLogger.stop(); - m_robotContainer.removeSubsystemDefaultCommands(); - m_robotContainer.configureTestBindings(); + //m_robotContainer.removeSubsystemDefaultCommands(); + //m_robotContainer.configureTestBindings(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c342680..716c65e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,45 +4,47 @@ package frc.robot; -import static edu.wpi.first.units.Units.RadiansPerSecond; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.indexer.IndexerSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; +/* +import static edu.wpi.first.units.Units.RadiansPerSecond; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.statemachines.DriveState; import frc.robot.statemachines.LaunchState; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DrivePreferences; import frc.robot.subsystems.drive.DrivetrainSubsystem; -import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakePreferences; import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.LaunchRequest; -import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.ui.UISubsystem; import frc.robot.subsystems.vision.VisionSubsystem; +*/ @Logged public class RobotContainer { - private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); + //private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); private final CommandXboxController driverJoystick = new CommandXboxController(0); - private final CommandXboxController operatorJoystick = new CommandXboxController(1); + //private final CommandXboxController operatorJoystick = new CommandXboxController(1); - @Logged(name = "Drivetrain") - public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); + //@Logged(name = "Drivetrain") + //public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); - @Logged(name = "Intake") - public final IntakeSubsystem intake = new IntakeSubsystem(); + //@Logged(name = "Intake") + //public final IntakeSubsystem intake = new IntakeSubsystem(); @Logged(name = "Indexer") public final IndexerSubsystem indexer = new IndexerSubsystem(); @@ -50,18 +52,20 @@ public class RobotContainer { @Logged(name = "Shooter") public final ShooterSubsystem shooter = new ShooterSubsystem(); - // @Logged(name = "Climber") - // public final ClimberSubsystem climber = new ClimberSubsystem(); + /* + @Logged(name = "Climber") + public final ClimberSubsystem climber = new ClimberSubsystem(); @Logged(name = "Vision") public final VisionSubsystem vision = new VisionSubsystem(); @Logged(name = "UI Feedback") public final UISubsystem uiFeedback = - new UISubsystem(driverJoystick.getHID(), operatorJoystick.getHID()); + new UISubsystem(driverJoystick.getHID(), operatorJoystick.getHID()); private final DriveState driveState = DriveState.getInstance(); private final LaunchState launchState = LaunchState.getInstance(); + private final Command driveAndLaunchCommand = drivetrain @@ -84,8 +88,11 @@ public class RobotContainer { .andThen(shooter.stowHood()); private final SendableChooser autoChooser; + */ + public RobotContainer() { + /* NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric)); NamedCommands.registerCommand("AutonShoot", autonShootCommand); NamedCommands.registerCommand("StopShot", stopShotCommand); @@ -125,9 +132,11 @@ public RobotContainer() { .withName("Rumble & Set Pose")); configureSubsystemDefaultCommands(); + */ configureTeleopBindings(); } + /* public void configureSubsystemDefaultCommands() { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically @@ -165,6 +174,7 @@ public void configureSubsystemDefaultCommands() { public void removeSubsystemDefaultCommands() { drivetrain.removeDefaultCommand(); } + public void configureTestBindings() { // Idle while the robot is disabled. This ensures the configured @@ -202,9 +212,10 @@ public void configureTestBindings() { driverJoystick.x().onTrue(intake.spinRollerCommand()).onFalse(intake.stopRollerCommand()); } + */ public void configureTeleopBindings() { - + /* driverJoystick .rightBumper() // .whileTrue(intake.setExtendNoPID()) @@ -302,12 +313,15 @@ public void configureTeleopBindings() { .withName("Rumble Driver Controller")); drivetrain.registerTelemetry(logger::telemeterize); + */ } + /* public Command getAutonomousCommand() { return autoChooser.getSelected(); } + private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { LaunchRequest launchRequest = launchState.getLaunchRequest(); double rotationalRate = @@ -333,4 +347,5 @@ private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { .withRotationalRate(rotationalRate) .withDeadband(DriveConstants.MAX_DRIVE_SPEED * 0.1); } + */ } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index c6823a6..588418a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -10,12 +10,10 @@ final class IndexerConstants { private IndexerConstants() {} - // TODO Replace with real id - protected static final int INDEXER_MOTOR_LEADER_ID = 3; - protected static final int INDEXER_MOTOR_FOLLOWER_ID = 9; - protected static final int ACCELERATOR_MOTOR_ID = 8; + protected static final int INDEXER_MOTOR_LEADER_ID = 4; + protected static final int ACCELERATOR_MOTOR_ID = 3; - // TODO: Tune motor + // TODO: PID tune motor protected static final double INDEXER_KS = 0; protected static final double INDEXER_KV = 0; @@ -45,20 +43,13 @@ protected static Slot0Configs createAcceleratorMotorSlot0Configs() { return slot; } - public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + public static MotorOutputConfigs createIndexerMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Brake; return newConfigs; } - public static MotorOutputConfigs createFollowerMotorOutputConfigs() { - MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.Clockwise_Positive; - newConfigs.NeutralMode = NeutralModeValue.Brake; - return newConfigs; - } - public static final double INDEXER_CURRENT_LIMIT = 40; public static CurrentLimitsConfigs createIndexerCurrentLimitsConfigs() { diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 6e5bd60..31dcc09 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -18,8 +18,7 @@ @Logged public class IndexerSubsystem extends SubsystemBase { - private final TalonFX indexerMotorLeader; - private final TalonFX indexerMotorFollower; + private final TalonFX indexerMotor; private final TalonFX acceleratorMotor; @Logged(name = "Indexer Velocity Target", importance = Importance.CRITICAL) @@ -53,26 +52,17 @@ public class IndexerSubsystem extends SubsystemBase { output -> setAcceleratorVoltage(output.magnitude()), null, this)); public IndexerSubsystem() { - indexerMotorLeader = new TalonFX(IndexerConstants.INDEXER_MOTOR_LEADER_ID); - indexerMotorFollower = new TalonFX(IndexerConstants.INDEXER_MOTOR_FOLLOWER_ID); + indexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR_LEADER_ID); acceleratorMotor = new TalonFX(IndexerConstants.ACCELERATOR_MOTOR_ID); - indexerMotorLeader.getConfigurator().apply(IndexerConstants.createLeaderMotorOutputConfigs()); - indexerMotorFollower - .getConfigurator() - .apply(IndexerConstants.createFollowerMotorOutputConfigs()); - - indexerMotorLeader.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); - indexerMotorFollower.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); + indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerMotorOutputConfigs()); + indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); acceleratorMotor.getConfigurator().apply(IndexerConstants.createAcceleratorMotorSlot0Configs()); acceleratorMotor .getConfigurator() .apply(IndexerConstants.createAcceleratorMotorOutputsConfigs()); - indexerMotorFollower.setControl( - new Follower(indexerMotorLeader.getDeviceID(), MotorAlignmentValue.Opposed)); - indexerVelocityTarget = RotationsPerSecond.of(0); acceleratorVelocityTarget = RotationsPerSecond.of(0); @@ -82,13 +72,13 @@ public IndexerSubsystem() { @Override public void periodic() { - // TODO: removed for testing only. PUT IT BACK! + // TODO: removed for pre-pidtuning. PUT IT BACK! // indexerMotor.setControl( // indexerControl.withVelocity(indexerVelocityTarget.in(RotationsPerSecond))); } private void setIndexerVoltage(double magnitude) { - indexerMotorLeader.setVoltage(magnitude); + indexerMotor.setVoltage(magnitude); } private void setAcceleratorVoltage(double magnitude) { @@ -96,12 +86,12 @@ private void setAcceleratorVoltage(double magnitude) { } public Command startIndexerNoPID() { - return run(() -> indexerMotorLeader.set(IndexerPreferences.indexerPercent.getValue())) + return run(() -> indexerMotor.set(IndexerPreferences.indexerPercent.getValue())) .withName("Set Indexer Percent"); } public Command startIndexerReverseNoPID() { - return run(() -> indexerMotorLeader.set(IndexerPreferences.indexerReversePercent.getValue())) + return run(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue())) .withName("Set Indexer Reverse Percent"); } @@ -111,7 +101,7 @@ public Command startAcceleratorNoPID() { } public Command stopIndexerNoPID() { - return runOnce(() -> indexerMotorLeader.set(0)).withName("Stop Indexer Percent"); + return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent"); } public Command stopAcceleratorNoPID() { @@ -120,7 +110,7 @@ public Command stopAcceleratorNoPID() { public Command startFullIndexingNoPID() { return run(() -> { - indexerMotorLeader.set(IndexerPreferences.indexerPercent.getValue()); + indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); }) .withName("Start Full Indexing No PID"); @@ -129,23 +119,36 @@ public Command startFullIndexingNoPID() { public Command stopFullIndexingNoPID() { return runOnce( () -> { - indexerMotorLeader.set(0); + indexerMotor.set(0); acceleratorMotor.set(0); }) .withName("Stop Full Indexing No PID"); } public Command indexCommand() { - return runOnce( + /*return runOnce( () -> indexerVelocityTarget = RotationsPerSecond.of(IndexerPreferences.indexSpeed.getValue())) - .withName("Index Lemons"); + .withName("Index Lemons");*/ // TODO: Removed for pre-pidtuning + return runOnce( + () -> { + indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); + acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); + }).withName("Index Lemons"); } public Command stopCommand() { - return runOnce(() -> indexerVelocityTarget = RotationsPerSecond.of(0)) - .withName("Stop Indexing"); + /*return runOnce( + () -> { + indexerVelocityTarget = RotationsPerSecond.of(0)); + acceleratorVelocityTarget = RotationsPerSecond.of(0); + }).withName("Stop Indexing");*/ // TODO: Removed for pre-pidtuning + return runOnce( + () -> { + indexerMotor.set(0); + acceleratorMotor.set(0); + }).withName("Stop Indexing Lemons"); } public Command pulsingIndexCommand() { @@ -158,11 +161,11 @@ public Command pulsingIndexCommand() { double elapsed = timer.get() % cycleTime; boolean shouldRun = elapsed < IndexerPreferences.indexerRunTime.getValue(); - indexerMotorLeader.set(shouldRun ? IndexerPreferences.indexerPercent.getValue() : 0); + indexerMotor.set(shouldRun ? IndexerPreferences.indexerPercent.getValue() : 0); acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); }, () -> { - indexerMotorLeader.set(0); + indexerMotor.set(0); acceleratorMotor.set(0); }) .beforeStarting(() -> timer.restart()) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 151b4b2..cff5fa4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -17,24 +17,34 @@ public class ShooterConstants { private ShooterConstants() {} - public static final int FLYWHEEL_LEADER_MOTOR_ID = 5; - public static final int FLYWHEEL_FOLLOWER_MOTOR_ID = 6; - public static final int HOOD_MOTOR_ID = 4; - - public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + public static final int FLYWHEEL_LEFT_LEADER_MOTOR_ID = 5; + public static final int FLYWHEEL_RIGHT_MOTOR_ID = 6; + public static final int FLYWHEEL_LEFT_FOLLOWER_MOTOR_ID = 7; + public static final int HOOD_MOTOR_ID = 8; + + public static MotorOutputConfigs createRightFlywheelMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.Inverted = InvertedValue.Clockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Coast; return newConfigs; } + public static MotorOutputConfigs createLeftFlywheelMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } - public static MotorOutputConfigs createFollowerMotorOutputConfigs() { + public static MotorOutputConfigs createHoodMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); newConfigs.Inverted = InvertedValue.Clockwise_Positive; - newConfigs.NeutralMode = NeutralModeValue.Coast; + newConfigs.NeutralMode = NeutralModeValue.Brake; return newConfigs; } + + + /* // TODO: Tune Flywheel and Hood Motor // Flywheel motor @@ -104,6 +114,7 @@ public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { public static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); public static final Current SAFE_STATOR_LIMIT = Amp.of(0.8); + */ // Conversion Constants public static final Angle ROTATIONS_PER_LAUNCH_DEGREE = Rotations.of(0.2); public static final Distance FLYWHEEL_RADIUS = Inch.of(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index bfc9238..b203467 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -23,6 +23,7 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX flywheelMotorFollower; private final TalonFX hoodMotor; + /* @Logged(name = "Velocity Target rads/s", importance = Importance.CRITICAL) private AngularVelocity velocityTarget; // *rads* Per Second is the base unit. @@ -44,6 +45,7 @@ public class ShooterSubsystem extends SubsystemBase { // Log state with SignalLogger class state -> SignalLogger.writeString("SysIdFlywheel_State", state.toString())), new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); + */ public ShooterSubsystem() { flywheelMotorLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEADER_MOTOR_ID); @@ -51,7 +53,7 @@ public ShooterSubsystem() { hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID); flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs()); - flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createLeaderMotorOutputConfigs()); + flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createIndexerMotorOutputConfigs()); flywheelMotorLeader .getConfigurator() .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); From 80a5aaac38b7de94c336172cf6027e1fd404f3bc Mon Sep 17 00:00:00 2001 From: TheGreatPintoJ Date: Mon, 30 Mar 2026 20:42:42 -0400 Subject: [PATCH 02/10] Indexing polishing and spotless --- src/main/java/frc/robot/Robot.java | 7 ++- src/main/java/frc/robot/RobotContainer.java | 30 ++++++------ .../indexer/IndexerPreferences.java | 6 ++- .../subsystems/indexer/IndexerSubsystem.java | 49 ++++++++++--------- .../subsystems/shooter/ShooterConstants.java | 14 ++---- .../subsystems/shooter/ShooterSubsystem.java | 2 - 6 files changed, 52 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7dd70f9..4ebb994 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.statemachines.LaunchState; import frc.robot.statemachines.ShiftState; /** @@ -106,7 +105,7 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - /* + /* m_autonomousCommand = m_robotContainer.getAutonomousCommand(); LaunchState.getInstance().setTargetPose3d(Constants.FieldConstants.getHubTarget()); @@ -142,8 +141,8 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); SignalLogger.stop(); - //m_robotContainer.removeSubsystemDefaultCommands(); - //m_robotContainer.configureTestBindings(); + // m_robotContainer.removeSubsystemDefaultCommands(); + // m_robotContainer.configureTestBindings(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 716c65e..b68032a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; -/* +/* import static edu.wpi.first.units.Units.RadiansPerSecond; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -35,16 +35,17 @@ @Logged public class RobotContainer { - //private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); + // private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); private final CommandXboxController driverJoystick = new CommandXboxController(0); - //private final CommandXboxController operatorJoystick = new CommandXboxController(1); - //@Logged(name = "Drivetrain") - //public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); + // private final CommandXboxController operatorJoystick = new CommandXboxController(1); - //@Logged(name = "Intake") - //public final IntakeSubsystem intake = new IntakeSubsystem(); + // @Logged(name = "Drivetrain") + // public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); + + // @Logged(name = "Intake") + // public final IntakeSubsystem intake = new IntakeSubsystem(); @Logged(name = "Indexer") public final IndexerSubsystem indexer = new IndexerSubsystem(); @@ -52,7 +53,7 @@ public class RobotContainer { @Logged(name = "Shooter") public final ShooterSubsystem shooter = new ShooterSubsystem(); - /* + /* @Logged(name = "Climber") public final ClimberSubsystem climber = new ClimberSubsystem(); @@ -65,7 +66,7 @@ public class RobotContainer { private final DriveState driveState = DriveState.getInstance(); private final LaunchState launchState = LaunchState.getInstance(); - + private final Command driveAndLaunchCommand = drivetrain @@ -90,9 +91,8 @@ public class RobotContainer { private final SendableChooser autoChooser; */ - public RobotContainer() { - /* + /* NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric)); NamedCommands.registerCommand("AutonShoot", autonShootCommand); NamedCommands.registerCommand("StopShot", stopShotCommand); @@ -174,7 +174,7 @@ public void configureSubsystemDefaultCommands() { public void removeSubsystemDefaultCommands() { drivetrain.removeDefaultCommand(); } - + public void configureTestBindings() { // Idle while the robot is disabled. This ensures the configured @@ -215,7 +215,7 @@ public void configureTestBindings() { */ public void configureTeleopBindings() { - /* + /* driverJoystick .rightBumper() // .whileTrue(intake.setExtendNoPID()) @@ -316,12 +316,12 @@ public void configureTeleopBindings() { */ } - /* + /* public Command getAutonomousCommand() { return autoChooser.getSelected(); } - + private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { LaunchRequest launchRequest = launchState.getLaunchRequest(); double rotationalRate = diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java index 90984ca..bb13fc7 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java @@ -7,7 +7,7 @@ final class IndexerPreferences { private IndexerPreferences() {} protected static DoublePreference indexSpeed = - new DoublePreference("Indexer/Index Speed", 0.1); // in rotations per second + new DoublePreference("Indexer/Index Speed (RotationsPS)", 0.1); // in rotations per second protected static DoublePreference indexerPercent = new DoublePreference("Indexer/Index Percent (for without PID)", 0.1); // in percent @@ -15,6 +15,10 @@ private IndexerPreferences() {} protected static DoublePreference indexerReversePercent = new DoublePreference("Indexer/Reverse Index Percent (for without PID)", -0.1); // in percent + protected static DoublePreference accelerateSpeed = + new DoublePreference( + "Indexer/Accelerate Speed (RotationsPS)", 0.1); // in rotations per second + protected static DoublePreference acceleratorPercent = new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 31dcc09..f05bfd8 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -4,10 +4,8 @@ import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.AngularVelocity; @@ -77,6 +75,7 @@ public void periodic() { // indexerControl.withVelocity(indexerVelocityTarget.in(RotationsPerSecond))); } + // SysID Helpers private void setIndexerVoltage(double magnitude) { indexerMotor.setVoltage(magnitude); } @@ -85,6 +84,7 @@ private void setAcceleratorVoltage(double magnitude) { acceleratorMotor.setVoltage(magnitude); } + // Indexer Commands public Command startIndexerNoPID() { return run(() -> indexerMotor.set(IndexerPreferences.indexerPercent.getValue())) .withName("Set Indexer Percent"); @@ -95,27 +95,21 @@ public Command startIndexerReverseNoPID() { .withName("Set Indexer Reverse Percent"); } + public Command stopIndexerNoPID() { + return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent"); + } + + // Accelerator Commands public Command startAcceleratorNoPID() { return run(() -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue())) .withName("Set Acceleration Percent"); } - public Command stopIndexerNoPID() { - return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent"); - } - public Command stopAcceleratorNoPID() { return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent"); } - public Command startFullIndexingNoPID() { - return run(() -> { - indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); - acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); - }) - .withName("Start Full Indexing No PID"); - } - + // Mainly Used Commands public Command stopFullIndexingNoPID() { return runOnce( () -> { @@ -127,28 +121,35 @@ public Command stopFullIndexingNoPID() { public Command indexCommand() { /*return runOnce( - () -> - indexerVelocityTarget = - RotationsPerSecond.of(IndexerPreferences.indexSpeed.getValue())) - .withName("Index Lemons");*/ // TODO: Removed for pre-pidtuning + () -> { + indexerVelocityTarget = + RotationsPerSecond.of(IndexerPreferences.indexSpeed.getValue()); + acceleratorVelocityTarget = + RotationsPerSecond.of(IndexerPreferences.accelerateSpeed.getValue()); + } + .withName("Index Lemons");*/ + // TODO: Removed for pre-pidtuning return runOnce( () -> { indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); - }).withName("Index Lemons"); + }) + .withName("Index Lemons"); } public Command stopCommand() { /*return runOnce( - () -> { - indexerVelocityTarget = RotationsPerSecond.of(0)); - acceleratorVelocityTarget = RotationsPerSecond.of(0); - }).withName("Stop Indexing");*/ // TODO: Removed for pre-pidtuning + () -> { PUT IT BACK! + indexerVelocityTarget = RotationsPerSecond.of(0)); + acceleratorVelocityTarget = RotationsPerSecond.of(0); + }).withName("Stop Indexing");*/ + // TODO: Removed for pre-pidtuning return runOnce( () -> { indexerMotor.set(0); acceleratorMotor.set(0); - }).withName("Stop Indexing Lemons"); + }) + .withName("Stop Indexing Lemons"); } public Command pulsingIndexCommand() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index cff5fa4..a32e3ff 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,15 +2,10 @@ import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; public class ShooterConstants { @@ -21,17 +16,18 @@ private ShooterConstants() {} public static final int FLYWHEEL_RIGHT_MOTOR_ID = 6; public static final int FLYWHEEL_LEFT_FOLLOWER_MOTOR_ID = 7; public static final int HOOD_MOTOR_ID = 8; - + public static MotorOutputConfigs createRightFlywheelMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); newConfigs.Inverted = InvertedValue.Clockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Coast; return newConfigs; } + public static MotorOutputConfigs createLeftFlywheelMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; - newConfigs.NeutralMode = NeutralModeValue.Coast; + newConfigs.NeutralMode = NeutralModeValue.Coast; return newConfigs; } @@ -42,9 +38,7 @@ public static MotorOutputConfigs createHoodMotorOutputConfigs() { return newConfigs; } - - - /* + /* // TODO: Tune Flywheel and Hood Motor // Flywheel motor diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b203467..be15946 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -15,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.preferences.DoublePreference; -import frc.robot.statemachines.LaunchState; @Logged public class ShooterSubsystem extends SubsystemBase { From f87ccfec0746e77c219abf1ffe41feaca9e9d86a Mon Sep 17 00:00:00 2001 From: William Crouch Date: Tue, 31 Mar 2026 18:52:01 -0400 Subject: [PATCH 03/10] Finished code (simulates and builds) --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../indexer/IndexerPreferences.java | 13 +-- .../subsystems/indexer/IndexerSubsystem.java | 53 ++++++----- .../subsystems/shooter/ShooterConstants.java | 71 +++++++++------ .../shooter/ShooterPreferences.java | 42 ++++----- .../subsystems/shooter/ShooterSubsystem.java | 87 ++++++++++++------- 6 files changed, 166 insertions(+), 108 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b68032a..01f944b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; @@ -133,7 +134,12 @@ public RobotContainer() { configureSubsystemDefaultCommands(); */ - configureTeleopBindings(); + SmartDashboard.putData(shooter.setFlywheelOutputCommand()); + SmartDashboard.putData(shooter.setHoodTargetCommand()); + SmartDashboard.putData(indexer.startIndexerNoPID()); + SmartDashboard.putData(indexer.startAcceleratorNoPID()); + SmartDashboard.putData(indexer.stopFullIndexingNoPID()); + // configureTeleopBindings(); } /* diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java index bb13fc7..67920cd 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java @@ -6,12 +6,16 @@ final class IndexerPreferences { private IndexerPreferences() {} - protected static DoublePreference indexSpeed = - new DoublePreference("Indexer/Index Speed (RotationsPS)", 0.1); // in rotations per second - protected static DoublePreference indexerPercent = new DoublePreference("Indexer/Index Percent (for without PID)", 0.1); // in percent + protected static DoublePreference acceleratorPercent = + new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent + + /* + protected static DoublePreference indexSpeed = + new DoublePreference("Indexer/Index Speed (RotationsPS)", 0.1); // in rotations per second + protected static DoublePreference indexerReversePercent = new DoublePreference("Indexer/Reverse Index Percent (for without PID)", -0.1); // in percent @@ -19,12 +23,11 @@ private IndexerPreferences() {} new DoublePreference( "Indexer/Accelerate Speed (RotationsPS)", 0.1); // in rotations per second - protected static DoublePreference acceleratorPercent = - new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent protected static DoublePreference indexerRunTime = new DoublePreference("Indexer/Pulsing Run Time", 2.0); // in seconds protected static DoublePreference indexerPauseTime = new DoublePreference("Indexer/Pulsing Pause Time", 0.1); // in seconds + */ } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index f05bfd8..cd40cac 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.indexer; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; @@ -9,7 +8,6 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -54,18 +52,22 @@ public IndexerSubsystem() { acceleratorMotor = new TalonFX(IndexerConstants.ACCELERATOR_MOTOR_ID); indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerMotorOutputConfigs()); - indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); - - acceleratorMotor.getConfigurator().apply(IndexerConstants.createAcceleratorMotorSlot0Configs()); acceleratorMotor .getConfigurator() .apply(IndexerConstants.createAcceleratorMotorOutputsConfigs()); + indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerCurrentLimitsConfigs()); + acceleratorMotor + .getConfigurator() + .apply(IndexerConstants.createAcceleratorCurrentLimitsConfigs()); + + /* indexerVelocityTarget = RotationsPerSecond.of(0); acceleratorVelocityTarget = RotationsPerSecond.of(0); indexerControl = new VelocityVoltage(0); acceleratorControl = new VelocityVoltage(0); + */ } @Override @@ -90,26 +92,12 @@ public Command startIndexerNoPID() { .withName("Set Indexer Percent"); } - public Command startIndexerReverseNoPID() { - return run(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue())) - .withName("Set Indexer Reverse Percent"); - } - - public Command stopIndexerNoPID() { - return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent"); - } - // Accelerator Commands public Command startAcceleratorNoPID() { return run(() -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue())) .withName("Set Acceleration Percent"); } - public Command stopAcceleratorNoPID() { - return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent"); - } - - // Mainly Used Commands public Command stopFullIndexingNoPID() { return runOnce( () -> { @@ -119,17 +107,33 @@ public Command stopFullIndexingNoPID() { .withName("Stop Full Indexing No PID"); } + /* + public Command startIndexerReverseNoPID() { + return run(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue())) + .withName("Set Indexer Reverse Percent"); + } + + public Command stopIndexerNoPID() { + return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent"); + } + + public Command stopAcceleratorNoPID() { + return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent"); + } + + // Mainly Used Commands + public Command indexCommand() { - /*return runOnce( + return runOnce( () -> { indexerVelocityTarget = RotationsPerSecond.of(IndexerPreferences.indexSpeed.getValue()); acceleratorVelocityTarget = RotationsPerSecond.of(IndexerPreferences.accelerateSpeed.getValue()); } - .withName("Index Lemons");*/ + .withName("Index Lemons"); // TODO: Removed for pre-pidtuning - return runOnce( + return run( () -> { indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); @@ -138,11 +142,11 @@ public Command indexCommand() { } public Command stopCommand() { - /*return runOnce( + return runOnce( () -> { PUT IT BACK! indexerVelocityTarget = RotationsPerSecond.of(0)); acceleratorVelocityTarget = RotationsPerSecond.of(0); - }).withName("Stop Indexing");*/ + }).withName("Stop Indexing"); // TODO: Removed for pre-pidtuning return runOnce( () -> { @@ -172,4 +176,5 @@ public Command pulsingIndexCommand() { .beforeStarting(() -> timer.restart()) .withName("Pulsing Index"); } + */ } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index a32e3ff..8e86f61 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,7 +2,10 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; @@ -38,22 +41,16 @@ public static MotorOutputConfigs createHoodMotorOutputConfigs() { return newConfigs; } - /* - // TODO: Tune Flywheel and Hood Motor - - // Flywheel motor - public static final double FLYWHEEL_KS = 0.11239; - public static final double FLYWHEEL_KV = 0.11589; - public static final double FLYWHEEL_KA = 0.0034356; - public static final double FLYWHEEL_KP = 0.066945 * 2; + public static final double HOOD_FORWARD_LIMIT = 6.2; + public static final double HOOD_REVERSE_LIMIT = 0; - public static Slot0Configs createFlywheelMotorSlot0Configs() { - Slot0Configs slot = new Slot0Configs(); - slot.kS = FLYWHEEL_KS; - slot.kV = FLYWHEEL_KV; - slot.kP = FLYWHEEL_KP; - slot.kA = FLYWHEEL_KA; - return slot; + public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() { + SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); + configs.ForwardSoftLimitEnable = true; + configs.ReverseSoftLimitEnable = true; + configs.ForwardSoftLimitThreshold = HOOD_FORWARD_LIMIT; + configs.ReverseSoftLimitThreshold = HOOD_REVERSE_LIMIT; + return configs; } public static final double FLYWHEEL_CURRENT_LIMIT = 40; @@ -65,9 +62,15 @@ public static CurrentLimitsConfigs createFlywheelCurrentLimitsConfigs() { return configs; } - public static final double ALLOWABLE_HOOD_ERROR = 0.1; - public static final double HOOD_FORWARD_LIMIT = 6.2; - public static final double HOOD_REVERSE_LIMIT = 0; + public static final double HOOD_CURRENT_LIMIT = 40; + + public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { + CurrentLimitsConfigs configs = new CurrentLimitsConfigs(); + configs.StatorCurrentLimit = HOOD_CURRENT_LIMIT; + configs.StatorCurrentLimitEnable = true; + return configs; + } + public static final double HOOD_KS = 0; public static final double HOOD_KP = 2.0; public static final double HOOD_KD = 0; @@ -80,6 +83,29 @@ public static Slot0Configs createHoodMotorSlot0Configs() { return slot; } + /* + // TODO: Tune Flywheel and Hood Motor + + // Flywheel motor + public static final double FLYWHEEL_KS = 0.11239; + public static final double FLYWHEEL_KV = 0.11589; + public static final double FLYWHEEL_KA = 0.0034356; + public static final double FLYWHEEL_KP = 0.066945 * 2; + + public static Slot0Configs createFlywheelMotorSlot0Configs() { + Slot0Configs slot = new Slot0Configs(); + slot.kS = FLYWHEEL_KS; + slot.kV = FLYWHEEL_KV; + slot.kP = FLYWHEEL_KP; + slot.kA = FLYWHEEL_KA; + return slot; + } + + + public static final double ALLOWABLE_HOOD_ERROR = 0.1; + public static final double HOOD_FORWARD_LIMIT = 6.2; + public static final double HOOD_REVERSE_LIMIT = 0; + public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() { SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); configs.ForwardSoftLimitEnable = true; @@ -96,15 +122,6 @@ public static MotorOutputConfigs createHoodMotorOutputConfigs() { return newConfigs; } - public static final double HOOD_CURRENT_LIMIT = 40; - - public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { - CurrentLimitsConfigs configs = new CurrentLimitsConfigs(); - configs.StatorCurrentLimit = HOOD_CURRENT_LIMIT; - configs.StatorCurrentLimitEnable = true; - return configs; - } - public static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); public static final Current SAFE_STATOR_LIMIT = Amp.of(0.8); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java index 1018c9b..9708e39 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java @@ -5,30 +5,32 @@ public class ShooterPreferences { private ShooterPreferences() {} - public static DoublePreference flywheelLaunchSpeed = - new DoublePreference("Shooter/Launch Speed", 0.1); // in rotations per second - public static DoublePreference hoodLaunchAngle = - new DoublePreference("Shooter/Hood Launch Position", 3); // in rotations - - public static DoublePreference flywheelLaunchPercent = - new DoublePreference("Shooter/Launch Percent (for without PID)", 0.1); // in percent - - public static DoublePreference hoodkP = new DoublePreference("Shooter/Hood/kP", 1); - public static DoublePreference hoodkD = new DoublePreference("Shooter/Hood/kD", 0); - public static DoublePreference hoodTargetPreference = new DoublePreference("Shooter/Hood/Target Rotations", 0); - // unit is *radians per second** for velocity. - public static DoublePreference tuningDefaultFlywheelRPS = - new DoublePreference("Shooter/Tuning/FlywheelRPS", 1000.0 / 60.0 / 2 * Math.PI); - public static DoublePreference tuningDefaultFlywheelStepRPS = - new DoublePreference("Shooter/Tuning/FlywheelStepRPS", 100.0 / 60.0 / 2 * Math.PI); - public static DoublePreference tuningDefaultHoodRotations = - new DoublePreference("Shooter/Tuning/HoodRotations", 0.0); - public static DoublePreference tuningDefaultHoodStepRotations = - new DoublePreference("Shooter/Tuning/HoodStepRotations", 0.2); + public static DoublePreference flywheelLaunchPercent = + new DoublePreference("Shooter/Launch Percent (for without PID)", 0.1); // in percent + /* + public static DoublePreference flywheelLaunchSpeed = + new DoublePreference("Shooter/Launch Speed", 0.1); // in rotations per second + public static DoublePreference hoodLaunchAngle = + new DoublePreference("Shooter/Hood Launch Position", 3); // in rotations + + public static DoublePreference hoodkP = new DoublePreference("Shooter/Hood/kP", 1); + public static DoublePreference hoodkD = new DoublePreference("Shooter/Hood/kD", 0); + + // unit is *radians per second** for velocity. + public static DoublePreference tuningDefaultFlywheelRPS = + new DoublePreference("Shooter/Tuning/FlywheelRPS", 1000.0 / 60.0 / 2 * Math.PI); + public static DoublePreference tuningDefaultFlywheelStepRPS = + new DoublePreference("Shooter/Tuning/FlywheelStepRPS", 100.0 / 60.0 / 2 * Math.PI); + public static DoublePreference tuningDefaultHoodRotations = + new DoublePreference("Shooter/Tuning/HoodRotations", 0.0); + public static DoublePreference tuningDefaultHoodStepRotations = + new DoublePreference("Shooter/Tuning/HoodStepRotations", 0.2); + + */ public static DoublePreference passingFlywheelSpeed = new DoublePreference("Shooter/Tuning/Flywheel Pass Speed", 62); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index be15946..02b1e67 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -2,23 +2,21 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.preferences.DoublePreference; @Logged public class ShooterSubsystem extends SubsystemBase { - private final TalonFX flywheelMotorLeader; - private final TalonFX flywheelMotorFollower; + private final TalonFX flywheelMotorLeftLeader; + private final TalonFX flywheelMotorRight; + private final TalonFX flywheelMotorLeftFollower; private final TalonFX hoodMotor; /* @@ -27,13 +25,14 @@ public class ShooterSubsystem extends SubsystemBase { private VelocityVoltage velocityControl; + private final LaunchState launchState = LaunchState.getInstance(); + */ + @Logged(name = "Hood Target (radians)", importance = Importance.CRITICAL) private Angle hoodTarget; // radians is the base unit. private PositionVoltage hoodControl; - private final LaunchState launchState = LaunchState.getInstance(); - final SysIdRoutine m_sysIdRoutineFlywheel = new SysIdRoutine( new SysIdRoutine.Config( @@ -43,50 +42,80 @@ public class ShooterSubsystem extends SubsystemBase { // Log state with SignalLogger class state -> SignalLogger.writeString("SysIdFlywheel_State", state.toString())), new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); - */ public ShooterSubsystem() { - flywheelMotorLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEADER_MOTOR_ID); - flywheelMotorFollower = new TalonFX(ShooterConstants.FLYWHEEL_FOLLOWER_MOTOR_ID); + flywheelMotorLeftLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEFT_LEADER_MOTOR_ID); + flywheelMotorLeftFollower = new TalonFX(ShooterConstants.FLYWHEEL_LEFT_FOLLOWER_MOTOR_ID); + flywheelMotorRight = new TalonFX(ShooterConstants.FLYWHEEL_RIGHT_MOTOR_ID); hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID); - flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs()); - flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createIndexerMotorOutputConfigs()); - flywheelMotorLeader + flywheelMotorLeftLeader .getConfigurator() - .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + .apply(ShooterConstants.createLeftFlywheelMotorOutputConfigs()); + flywheelMotorLeftFollower + .getConfigurator() + .apply(ShooterConstants.createLeftFlywheelMotorOutputConfigs()); + flywheelMotorRight + .getConfigurator() + .apply(ShooterConstants.createRightFlywheelMotorOutputConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorOutputConfigs()); - flywheelMotorFollower + flywheelMotorLeftLeader .getConfigurator() - .apply(ShooterConstants.createFlywheelMotorSlot0Configs()); - flywheelMotorFollower + .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + flywheelMotorLeftFollower .getConfigurator() - .apply(ShooterConstants.createFollowerMotorOutputConfigs()); - flywheelMotorFollower + .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + flywheelMotorRight .getConfigurator() .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodCurrentLimitsConfigs()); - velocityTarget = RotationsPerSecond.of(0); - velocityControl = new VelocityVoltage(0); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorOutputConfigs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodCurrentLimitsConfigs()); + hoodTarget = Rotations.of(0); - hoodControl = new PositionVoltage(0); + hoodControl = new PositionVoltage(hoodTarget); + } + + private void setFlywheelVoltage(double magnitude) { + flywheelMotorLeftLeader.setVoltage(magnitude); + flywheelMotorLeftFollower.setVoltage(magnitude); + flywheelMotorRight.setVoltage(magnitude); } @Override public void periodic() { + /* launchState.refreshRequest(); flywheelMotorLeader.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); flywheelMotorFollower.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); + */ hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); } + public Command setHoodTargetCommand() { + return runOnce( + () -> hoodTarget = Rotations.of(ShooterPreferences.hoodTargetPreference.getValue())) + .withName("Set Hood Target"); + } + + public Command setFlywheelOutputCommand() { + return runOnce( + () -> setFlywheelMotorOutput(ShooterPreferences.flywheelLaunchPercent.getValue())) + .withName("Set Flywheel Output"); + } + + public void setFlywheelMotorOutput(double output) { + flywheelMotorLeftLeader.set(output); + flywheelMotorLeftFollower.set(output); + flywheelMotorRight.set(output); + } + + /* @Logged(name = "Velocity Target RPM", importance = Importance.CRITICAL) public double getFlywheelTargetRPM() { return velocityTarget.in(RotationsPerSecond) * 60; @@ -138,11 +167,6 @@ public Command setHoodToPreference() { () -> hoodTarget = Rotations.of(ShooterPreferences.hoodTargetPreference.getValue())); } - private void setFlywheelVoltage(double magnitude) { - flywheelMotorLeader.setVoltage(magnitude); - flywheelMotorFollower.setVoltage(magnitude); - } - @Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL) public boolean atHoodSetpoint() { return Math.abs(hoodMotor.getPosition().getValueAsDouble() - hoodTarget.in(Rotations)) @@ -261,6 +285,7 @@ public Command stopShooterTuningCommand() { setHoodCommand(Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()))) .withName("Stop Shooter Tuning"); } + */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_sysIdRoutineFlywheel.quasistatic(direction); From 64eacfcd0b99aa23550b6bec67359d513bec3908 Mon Sep 17 00:00:00 2001 From: Brandon Date: Tue, 31 Mar 2026 19:27:54 -0400 Subject: [PATCH 04/10] adjusted shooter variable --- .../subsystems/shooter/ShooterSubsystem.java | 28 ++++++++++++++----- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b203467..7364287 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -23,7 +23,7 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX flywheelMotorFollower; private final TalonFX hoodMotor; - /* + @Logged(name = "Velocity Target rads/s", importance = Importance.CRITICAL) private AngularVelocity velocityTarget; // *rads* Per Second is the base unit. @@ -45,11 +45,12 @@ public class ShooterSubsystem extends SubsystemBase { // Log state with SignalLogger class state -> SignalLogger.writeString("SysIdFlywheel_State", state.toString())), new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); - */ + public ShooterSubsystem() { flywheelMotorLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEADER_MOTOR_ID); - flywheelMotorFollower = new TalonFX(ShooterConstants.FLYWHEEL_FOLLOWER_MOTOR_ID); + flywheelLeftMotorFollower = new TalonFX(ShooterConstants.FLYWHEEL_LEFT_MOTOR_ID); + flywheelRightFollower = new TalonFX(ShooterConstants.FLYWHEEL_RIGHT_MOTOR_ID) hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID); flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs()); @@ -58,15 +59,25 @@ public ShooterSubsystem() { .getConfigurator() .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); - flywheelMotorFollower + flywheelLeftMotorFollower .getConfigurator() .apply(ShooterConstants.createFlywheelMotorSlot0Configs()); - flywheelMotorFollower + flywheelLeftMotorFollower .getConfigurator() .apply(ShooterConstants.createFollowerMotorOutputConfigs()); - flywheelMotorFollower + flywheelLeftMotorFollower .getConfigurator() .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + flywheelRightFollower + .getConfigurator() + .apply(ShooterConstants.createFlywheelMotorSlot0Configs()); + flywheelRightFollower + .getConfigurator() + .apply(ShooterConstants.createFollowerMotorOutputConfigs()); + flywheelRightFollower + .getConfigurator() + .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + velocityTarget = RotationsPerSecond.of(0); velocityControl = new VelocityVoltage(0); @@ -84,7 +95,10 @@ public void periodic() { launchState.refreshRequest(); flywheelMotorLeader.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); - flywheelMotorFollower.setControl( + flywheelRightFollower.setControl( + velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); + hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); + flywheelLeftFollower.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); } From 91299ca946e510b2030c49cd8080b02a5b1e6ff3 Mon Sep 17 00:00:00 2001 From: Brandon Date: Tue, 31 Mar 2026 19:47:03 -0400 Subject: [PATCH 05/10] adjusted shooter variable --- .../frc/robot/subsystems/shooter/ShooterSubsystem.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 02b1e67..5f98bfc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -19,7 +19,7 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX flywheelMotorLeftFollower; private final TalonFX hoodMotor; - /* + /* @Logged(name = "Velocity Target rads/s", importance = Importance.CRITICAL) private AngularVelocity velocityTarget; // *rads* Per Second is the base unit. @@ -69,6 +69,7 @@ public ShooterSubsystem() { flywheelMotorRight .getConfigurator() .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodCurrentLimitsConfigs()); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); @@ -91,7 +92,10 @@ public void periodic() { launchState.refreshRequest(); flywheelMotorLeader.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); - flywheelMotorFollower.setControl( + flywheelRightFollower.setControl( + velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); + hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); + flywheelLeftFollower.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); */ hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); From d9c64b46a7b54d1bd273257fad0571826aedfa5c Mon Sep 17 00:00:00 2001 From: William Crouch Date: Tue, 31 Mar 2026 20:56:48 -0400 Subject: [PATCH 06/10] Current Code --- src/main/java/frc/robot/RobotContainer.java | 6 ++++++ .../robot/subsystems/indexer/IndexerConstants.java | 2 +- .../robot/subsystems/indexer/IndexerSubsystem.java | 8 ++++++-- .../robot/subsystems/shooter/ShooterConstants.java | 2 +- .../robot/subsystems/shooter/ShooterSubsystem.java | 12 +++++++----- 5 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01f944b..e2d994e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -139,6 +139,12 @@ public RobotContainer() { SmartDashboard.putData(indexer.startIndexerNoPID()); SmartDashboard.putData(indexer.startAcceleratorNoPID()); SmartDashboard.putData(indexer.stopFullIndexingNoPID()); + + driverJoystick.leftTrigger().whileTrue(indexer.startIndexerNoPID()); + driverJoystick.rightTrigger().whileTrue(indexer.startAcceleratorNoPID()); + driverJoystick.a().onTrue(shooter.setHoodTargetCommand()); + driverJoystick.leftBumper().onTrue(shooter.setFlywheelOutputCommand()); + driverJoystick.rightBumper().onTrue(shooter.stopFlywheelOutputCommand()); // configureTeleopBindings(); } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 588418a..c78046a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -61,7 +61,7 @@ public static CurrentLimitsConfigs createIndexerCurrentLimitsConfigs() { public static MotorOutputConfigs createAcceleratorMotorOutputsConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Brake; return newConfigs; } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index cd40cac..39938fb 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -88,13 +88,17 @@ private void setAcceleratorVoltage(double magnitude) { // Indexer Commands public Command startIndexerNoPID() { - return run(() -> indexerMotor.set(IndexerPreferences.indexerPercent.getValue())) + return runEnd( + () -> indexerMotor.set(IndexerPreferences.indexerPercent.getValue()), + () -> indexerMotor.set(0)) .withName("Set Indexer Percent"); } // Accelerator Commands public Command startAcceleratorNoPID() { - return run(() -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue())) + return runEnd( + () -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()), + () -> acceleratorMotor.set(0)) .withName("Set Acceleration Percent"); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 8e86f61..0e4cb0b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -36,7 +36,7 @@ public static MotorOutputConfigs createLeftFlywheelMotorOutputConfigs() { public static MotorOutputConfigs createHoodMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Brake; return newConfigs; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 5f98bfc..1c3adf0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -19,7 +19,7 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX flywheelMotorLeftFollower; private final TalonFX hoodMotor; - /* + /* @Logged(name = "Velocity Target rads/s", importance = Importance.CRITICAL) private AngularVelocity velocityTarget; // *rads* Per Second is the base unit. @@ -98,7 +98,7 @@ public void periodic() { flywheelLeftFollower.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); */ - hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); + // hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); } public Command setHoodTargetCommand() { @@ -108,9 +108,11 @@ public Command setHoodTargetCommand() { } public Command setFlywheelOutputCommand() { - return runOnce( - () -> setFlywheelMotorOutput(ShooterPreferences.flywheelLaunchPercent.getValue())) - .withName("Set Flywheel Output"); + return runOnce(() -> setFlywheelMotorOutput(0.5)).withName("Set Flywheel Output"); + } + + public Command stopFlywheelOutputCommand() { + return runOnce(() -> setFlywheelMotorOutput(0)).withName("Set Flywheel Output"); } public void setFlywheelMotorOutput(double output) { From 32d261e9b564629c2107d25c88e9d748f5faa9b0 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Tue, 31 Mar 2026 21:29:53 -0400 Subject: [PATCH 07/10] The leader motor needs to take a class in motoring --- .../frc/robot/subsystems/shooter/ShooterConstants.java | 9 ++++++++- .../frc/robot/subsystems/shooter/ShooterSubsystem.java | 6 +++--- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 0e4cb0b..fea5a6d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -27,7 +27,14 @@ public static MotorOutputConfigs createRightFlywheelMotorOutputConfigs() { return newConfigs; } - public static MotorOutputConfigs createLeftFlywheelMotorOutputConfigs() { + public static MotorOutputConfigs createLeftFlywheelLeaderMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } + + public static MotorOutputConfigs createLeftFlywheelFollowerMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Coast; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 1c3adf0..88445f2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -51,10 +51,10 @@ public ShooterSubsystem() { flywheelMotorLeftLeader .getConfigurator() - .apply(ShooterConstants.createLeftFlywheelMotorOutputConfigs()); + .apply(ShooterConstants.createLeftFlywheelLeaderMotorOutputConfigs()); flywheelMotorLeftFollower .getConfigurator() - .apply(ShooterConstants.createLeftFlywheelMotorOutputConfigs()); + .apply(ShooterConstants.createLeftFlywheelFollowerMotorOutputConfigs()); flywheelMotorRight .getConfigurator() .apply(ShooterConstants.createRightFlywheelMotorOutputConfigs()); @@ -112,7 +112,7 @@ public Command setFlywheelOutputCommand() { } public Command stopFlywheelOutputCommand() { - return runOnce(() -> setFlywheelMotorOutput(0)).withName("Set Flywheel Output"); + return runOnce(() -> setFlywheelMotorOutput(0)).withName("Stop Flywheel Output"); } public void setFlywheelMotorOutput(double output) { From bf67441c834a8f6d7076f27a16c5e17fddf03047 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Wed, 1 Apr 2026 10:01:16 -0400 Subject: [PATCH 08/10] Full indexing command and preferences --- src/main/java/frc/robot/RobotContainer.java | 3 +-- .../robot/subsystems/indexer/IndexerSubsystem.java | 12 ++++++++++++ .../robot/subsystems/shooter/ShooterSubsystem.java | 4 +++- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e2d994e..a9bdb51 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,8 +140,7 @@ public RobotContainer() { SmartDashboard.putData(indexer.startAcceleratorNoPID()); SmartDashboard.putData(indexer.stopFullIndexingNoPID()); - driverJoystick.leftTrigger().whileTrue(indexer.startIndexerNoPID()); - driverJoystick.rightTrigger().whileTrue(indexer.startAcceleratorNoPID()); + driverJoystick.rightTrigger().whileTrue(indexer.startFullIndexingNoPID()); driverJoystick.a().onTrue(shooter.setHoodTargetCommand()); driverJoystick.leftBumper().onTrue(shooter.setFlywheelOutputCommand()); driverJoystick.rightBumper().onTrue(shooter.stopFlywheelOutputCommand()); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 39938fb..b03b18f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -102,6 +102,18 @@ public Command startAcceleratorNoPID() { .withName("Set Acceleration Percent"); } + public Command startFullIndexingNoPID() { + return runEnd( + () -> { + indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); + acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); + }, + () -> { + indexerMotor.set(0); + acceleratorMotor.set(0); + }); + } + public Command stopFullIndexingNoPID() { return runOnce( () -> { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 88445f2..fd0a0bb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -108,7 +108,9 @@ public Command setHoodTargetCommand() { } public Command setFlywheelOutputCommand() { - return runOnce(() -> setFlywheelMotorOutput(0.5)).withName("Set Flywheel Output"); + return runOnce( + () -> setFlywheelMotorOutput(ShooterPreferences.flywheelLaunchPercent.getValue())) + .withName("Set Flywheel Output"); } public Command stopFlywheelOutputCommand() { From f7c40b63ada8c1d02c5a25d258543e3e398faeb8 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Wed, 1 Apr 2026 19:17:15 -0400 Subject: [PATCH 09/10] Premerge cleanup --- src/main/java/frc/robot/RobotContainer.java | 75 +++++++------------ .../subsystems/climber/ClimberConstants.java | 46 ------------ .../climber/ClimberPreferences.java | 9 --- .../subsystems/climber/ClimberSubsystem.java | 68 ----------------- 4 files changed, 28 insertions(+), 170 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberPreferences.java delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9bdb51..ad4807d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,49 +4,46 @@ package frc.robot; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.subsystems.indexer.IndexerSubsystem; -import frc.robot.subsystems.shooter.ShooterSubsystem; - -/* import static edu.wpi.first.units.Units.RadiansPerSecond; + import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.statemachines.DriveState; import frc.robot.statemachines.LaunchState; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DrivePreferences; import frc.robot.subsystems.drive.DrivetrainSubsystem; +import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakePreferences; import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.LaunchRequest; +import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.ui.UISubsystem; import frc.robot.subsystems.vision.VisionSubsystem; -*/ @Logged public class RobotContainer { - // private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); + private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); private final CommandXboxController driverJoystick = new CommandXboxController(0); - // private final CommandXboxController operatorJoystick = new CommandXboxController(1); + private final CommandXboxController operatorJoystick = new CommandXboxController(1); - // @Logged(name = "Drivetrain") - // public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); + @Logged(name = "Drivetrain") + public final DrivetrainSubsystem drivetrain = new DrivetrainSubsystem(); - // @Logged(name = "Intake") - // public final IntakeSubsystem intake = new IntakeSubsystem(); + @Logged(name = "Intake") + public final IntakeSubsystem intake = new IntakeSubsystem(); @Logged(name = "Indexer") public final IndexerSubsystem indexer = new IndexerSubsystem(); @@ -54,21 +51,17 @@ public class RobotContainer { @Logged(name = "Shooter") public final ShooterSubsystem shooter = new ShooterSubsystem(); - /* - @Logged(name = "Climber") - public final ClimberSubsystem climber = new ClimberSubsystem(); - @Logged(name = "Vision") public final VisionSubsystem vision = new VisionSubsystem(); @Logged(name = "UI Feedback") public final UISubsystem uiFeedback = - new UISubsystem(driverJoystick.getHID(), operatorJoystick.getHID()); + new UISubsystem(driverJoystick.getHID(), operatorJoystick.getHID()); private final DriveState driveState = DriveState.getInstance(); private final LaunchState launchState = LaunchState.getInstance(); - + /* private final Command driveAndLaunchCommand = drivetrain .applyRequest(() -> getDriveAndLaunchRequest()) @@ -88,15 +81,14 @@ public class RobotContainer { .stopFullIndexingNoPID() .andThen(shooter.stopFlywheelCommand()) .andThen(shooter.stowHood()); + */ private final SendableChooser autoChooser; - */ public RobotContainer() { - /* NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric)); - NamedCommands.registerCommand("AutonShoot", autonShootCommand); - NamedCommands.registerCommand("StopShot", stopShotCommand); + // NamedCommands.registerCommand("AutonShoot", autonShootCommand); + // NamedCommands.registerCommand("StopShot", stopShotCommand); NamedCommands.registerCommand("StopRoller", intake.stopRollerNoPID()); NamedCommands.registerCommand( "Collect Intake", @@ -111,7 +103,6 @@ public RobotContainer() { NamedCommands.registerCommand( "HP Reload", new WaitCommand(IntakePreferences.outpostReloadWait.getValue())); autoChooser = AutoBuilder.buildAutoChooser("Auto Chooser"); - autoChooser.addOption("Auton Shoot", autonShootCommand); SmartDashboard.putData("Auto Mode", autoChooser); // Idle while the robot is disabled. This ensures the configured @@ -133,21 +124,9 @@ public RobotContainer() { .withName("Rumble & Set Pose")); configureSubsystemDefaultCommands(); - */ - SmartDashboard.putData(shooter.setFlywheelOutputCommand()); - SmartDashboard.putData(shooter.setHoodTargetCommand()); - SmartDashboard.putData(indexer.startIndexerNoPID()); - SmartDashboard.putData(indexer.startAcceleratorNoPID()); - SmartDashboard.putData(indexer.stopFullIndexingNoPID()); - - driverJoystick.rightTrigger().whileTrue(indexer.startFullIndexingNoPID()); - driverJoystick.a().onTrue(shooter.setHoodTargetCommand()); - driverJoystick.leftBumper().onTrue(shooter.setFlywheelOutputCommand()); - driverJoystick.rightBumper().onTrue(shooter.stopFlywheelOutputCommand()); - // configureTeleopBindings(); + configureTeleopBindings(); } - /* public void configureSubsystemDefaultCommands() { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically @@ -186,7 +165,7 @@ public void removeSubsystemDefaultCommands() { drivetrain.removeDefaultCommand(); } - + /* public void configureTestBindings() { // Idle while the robot is disabled. This ensures the configured // neutral mode is applied to the drive motors while disabled. @@ -226,7 +205,6 @@ public void configureTestBindings() { */ public void configureTeleopBindings() { - /* driverJoystick .rightBumper() // .whileTrue(intake.setExtendNoPID()) @@ -240,6 +218,10 @@ public void configureTeleopBindings() { .andThen(intake.setIntakeExtensionCommand(IntakeConstants.INTAKE_REVERSE_LIMIT)) .withName("Stow Intake")); + // stop the roller without retracting. + driverJoystick.x().onTrue(intake.stopRollerNoPID()); + + /* // outtake fuel. don't retract when done. driverJoystick .b() @@ -252,9 +234,6 @@ public void configureTeleopBindings() { .onFalse( intake.stopRollerNoPID().andThen(indexer.stopIndexerNoPID()).withName("Stop Roller")); - // stop the roller without retracting. - driverJoystick.x().onTrue(intake.stopRollerNoPID()); - operatorJoystick .rightTrigger() .whileTrue( @@ -278,6 +257,7 @@ public void configureTeleopBindings() { .rightBumper() .whileTrue(shooter.spinFlywheelPostCommand()) .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood())); + */ // Reset the field-centric heading on start button press. driverJoystick @@ -323,16 +303,18 @@ public void configureTeleopBindings() { .manualRumbleCommand(driverJoystick.getHID()) .withName("Rumble Driver Controller")); + operatorJoystick.rightTrigger().whileTrue(indexer.startFullIndexingNoPID()); + operatorJoystick.a().onTrue(shooter.setHoodTargetCommand()); + operatorJoystick.leftBumper().onTrue(shooter.setFlywheelOutputCommand()); + operatorJoystick.rightBumper().onTrue(shooter.stopFlywheelOutputCommand()); + drivetrain.registerTelemetry(logger::telemeterize); - */ } - /* public Command getAutonomousCommand() { return autoChooser.getSelected(); } - private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { LaunchRequest launchRequest = launchState.getLaunchRequest(); double rotationalRate = @@ -358,5 +340,4 @@ private SwerveRequest.FieldCentric getDriveAndLaunchRequest() { .withRotationalRate(rotationalRate) .withDeadband(DriveConstants.MAX_DRIVE_SPEED * 0.1); } - */ } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java deleted file mode 100644 index eb32b6e..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.subsystems.climber; - -import static edu.wpi.first.units.Units.Amp; -import static edu.wpi.first.units.Units.Rotations; - -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.controls.DutyCycleOut; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Current; - -final class ClimberConstants { - - private ClimberConstants() {} - - // TODO move to real ID when implemented. - protected static final int CLIMB_MOTOR_ID = 19; - - // TODO: Tune motor - protected static final Angle ALLOWABLE_CLIMB_ERROR = Rotations.of(0.1); - protected static final Angle CLIMB_FORWARD_LIMIT = Rotations.of(100); - protected static final Angle CLIMB_REVERSE_LIMIT = Rotations.of(0); - protected static final double CLIMB_KS = 0; - protected static final double CLIMB_KP = 0; - protected static final double CLIMB_KD = 0; - - protected static Slot0Configs createClimbMotorSlot0Configs() { - Slot0Configs slot = new Slot0Configs(); - slot.kS = CLIMB_KS; - slot.kP = CLIMB_KP; - slot.kD = CLIMB_KD; - return slot; - } - - protected static SoftwareLimitSwitchConfigs createClimbSoftwareLimitSwitchConfigs() { - SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); - configs.ForwardSoftLimitEnable = false; - configs.ReverseSoftLimitEnable = false; - configs.ForwardSoftLimitThreshold = CLIMB_FORWARD_LIMIT.in(Rotations); - configs.ReverseSoftLimitThreshold = CLIMB_REVERSE_LIMIT.in(Rotations); - return configs; - } - - protected static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); - protected static final Current SAFE_STATOR_LIMIT = Amp.of(0.8); -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberPreferences.java b/src/main/java/frc/robot/subsystems/climber/ClimberPreferences.java deleted file mode 100644 index b40e1d4..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberPreferences.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.robot.subsystems.climber; - -import frc.robot.preferences.DoublePreference; - -final class ClimberPreferences { - - protected static DoublePreference pullPosition = - new DoublePreference("Climber/Pull Down (Rotations)", 3); // in rotations -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java deleted file mode 100644 index 9f9ceee..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc.robot.subsystems.climber; - -import static edu.wpi.first.units.Units.Amp; -import static edu.wpi.first.units.Units.Rotations; - -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.Logged.Importance; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -@Logged -public class ClimberSubsystem extends SubsystemBase { - private final TalonFX climbMotor; - - @Logged(name = "Climb Target", importance = Importance.CRITICAL) - private Angle climbTarget; // Rotations - - private PositionTorqueCurrentFOC climbControl; - - public ClimberSubsystem() { - climbMotor = new TalonFX(ClimberConstants.CLIMB_MOTOR_ID); - - climbMotor.getConfigurator().apply(ClimberConstants.createClimbMotorSlot0Configs()); - climbMotor.getConfigurator().apply(ClimberConstants.createClimbSoftwareLimitSwitchConfigs()); - climbMotor.setPosition(0); - climbTarget = Rotations.of(0); - climbControl = new PositionTorqueCurrentFOC(0); - } - - @Override - public void periodic() { - climbMotor.setControl(climbControl.withPosition(climbTarget)); - } - - @Logged(name = "Climb Setpoint", importance = Importance.CRITICAL) - public boolean atClimbSetpoint() { - return Math.abs(climbMotor.getPosition().getValueAsDouble() - climbTarget.in(Rotations)) - < ClimberConstants.ALLOWABLE_CLIMB_ERROR.in(Rotations); - } - - public Command climbToPosition(Angle position) { - return runOnce(() -> climbTarget = position) - .andThen(Commands.waitUntil(() -> atClimbSetpoint())); - } - - public Command resetClimb() { - return climbToPosition(Rotations.of(0)); - } - - public Command climbToPreference() { - return climbToPosition(Rotations.of(ClimberPreferences.pullPosition.getValue())); - } - - public Command homeClimber() { - return runEnd( - () -> climbMotor.set(ClimberConstants.SAFE_HOMING_EFFORT.Output), - () -> climbMotor.setPosition(0)) - .until( - () -> { - return climbMotor.getStatorCurrent().getValueAsDouble() - > ClimberConstants.SAFE_STATOR_LIMIT.in(Amp); - }); - } -} From 85f4e121418bdcdc3717da67e888e1248b5e20e6 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Thu, 2 Apr 2026 19:24:47 -0400 Subject: [PATCH 10/10] current hood constants --- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index fea5a6d..11b8682 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -48,7 +48,7 @@ public static MotorOutputConfigs createHoodMotorOutputConfigs() { return newConfigs; } - public static final double HOOD_FORWARD_LIMIT = 6.2; + public static final double HOOD_FORWARD_LIMIT = 4.9; public static final double HOOD_REVERSE_LIMIT = 0; public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() { @@ -78,8 +78,8 @@ public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { return configs; } - public static final double HOOD_KS = 0; - public static final double HOOD_KP = 2.0; + public static final double HOOD_KS = 4.8; + public static final double HOOD_KP = 9; public static final double HOOD_KD = 0; public static Slot0Configs createHoodMotorSlot0Configs() {