From ee0f9bea226b2b957a9baf256687514bb7268a22 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Sun, 11 Jan 2026 02:20:24 -0500 Subject: [PATCH 01/11] Add reset gyro to teleop too --- src/main/java/frc/robot/controls/DriverControls.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index 2b181b9..05d77ff 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -88,6 +88,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur } else if (Robot.isSimulation()) { controller.back().whileTrue(fireAlgae(drivetrain)); } else { + controller.y().onTrue((Commands.runOnce(drivetrain::zeroGyro))); controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); } } From f7149743e0e3fa84c9a4cd1518bbce3782c2fcfd Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Sun, 11 Jan 2026 02:22:02 -0500 Subject: [PATCH 02/11] Make LL conditional --- .../frc/robot/subsystems/SwerveSubsystem.java | 120 ++++++++++-------- 1 file changed, 65 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 01e044b..82ab632 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -76,6 +76,8 @@ public class SwerveSubsystem extends SubsystemBase { private Limelight limelight; private LimelightPoseEstimator poseEstimator; + private final boolean IS_LIMELIGHT_ENABLED = false; + /** * Initialize {@link SwerveDrive} with the directory provided. * @@ -129,50 +131,53 @@ public SwerveSubsystem(File directory) { SwerveIMU gyro = swerveDrive.getGyro(); - limelight = new Limelight("limelight"); - limelight.getSettings() - .withLimelightLEDMode(LEDMode.PipelineControl) - .withCameraOffset(new Pose3d( - Inches.of(14.00).in(Meters), - Inches.of(4.750).in(Meters), - Inches.of(17.25).in(Meters), - Rotation3d.kZero)) - .withImuMode(ImuMode.InternalImuMT1Assist) - .withImuAssistAlpha(0.01) - .save(); - - RobotModeTriggers.disabled().onTrue(Commands.runOnce(() -> { - System.out.println("Setting LL IMU Assist Alpha to 0.001"); - + if (IS_LIMELIGHT_ENABLED) { + limelight = new Limelight("limelight"); limelight.getSettings() - // .withImuMode(ImuMode.InternalImuMT1Assist) - .withImuAssistAlpha(0.001) + .withLimelightLEDMode(LEDMode.PipelineControl) + .withCameraOffset(new Pose3d( + Inches.of(14.00).in(Meters), + Inches.of(4.750).in(Meters), + Inches.of(17.25).in(Meters), + Rotation3d.kZero)) + .withImuMode(ImuMode.InternalImuMT1Assist) + .withImuAssistAlpha(0.01) .save(); - }).ignoringDisable(true)); - Command onEnable = Commands.runOnce(() -> { - System.out.println("Setting LL IMU Assist Alpha to 0.01"); + RobotModeTriggers.disabled().onTrue(Commands.runOnce(() -> { + System.out.println("Setting LL IMU Assist Alpha to 0.001"); + + limelight.getSettings() + // .withImuMode(ImuMode.InternalImuMT1Assist) + .withImuAssistAlpha(0.001) + .save(); + }).ignoringDisable(true)); + + Command onEnable = Commands.runOnce(() -> { + System.out.println("Setting LL IMU Assist Alpha to 0.01"); + + limelight.getSettings() + // .withImuMode(ImuMode.InternalImuMT1Assist) + .withImuAssistAlpha(0.01) + .save(); + }); + + RobotModeTriggers.teleop().onTrue(onEnable); + RobotModeTriggers.autonomous().onTrue(onEnable); + RobotModeTriggers.test().onTrue(onEnable); + // Required for megatag2 in periodic() function before fetching pose. limelight.getSettings() - // .withImuMode(ImuMode.InternalImuMT1Assist) - .withImuAssistAlpha(0.01) + .withRobotOrientation( + new Orientation3d(gyro.getRotation3d(), + new AngularVelocity3d( + DegreesPerSecond.of(0), + DegreesPerSecond.of(0), + DegreesPerSecond.of(0)))) .save(); - }); - RobotModeTriggers.teleop().onTrue(onEnable); - RobotModeTriggers.autonomous().onTrue(onEnable); - RobotModeTriggers.test().onTrue(onEnable); - - // Required for megatag2 in periodic() function before fetching pose. - limelight.getSettings() - .withRobotOrientation( - new Orientation3d(gyro.getRotation3d(), - new AngularVelocity3d( - DegreesPerSecond.of(0), - DegreesPerSecond.of(0), - DegreesPerSecond.of(0)))) - .save(); - - poseEstimator = limelight.createPoseEstimator(EstimationMode.MEGATAG2); + + poseEstimator = limelight.createPoseEstimator(EstimationMode.MEGATAG2); + } setupPathPlanner(); } @@ -181,27 +186,32 @@ public SwerveSubsystem(File directory) { public void periodic() { // swerveDrive.updateOdometry(); // TODO: see if this is needed - // // Get MegaTag2 pose - Optional visionEstimate = poseEstimator.getPoseEstimate(); + if (IS_LIMELIGHT_ENABLED) { + // Get MegaTag2 pose + Optional visionEstimate = poseEstimator.getPoseEstimate(); - // If the pose is present - visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { - if (poseEstimate.tagCount > 0) { - Logger.recordOutput("Limelight/Megatag2Count", poseEstimate.tagCount); + // If the pose is present + visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { + if (poseEstimate.tagCount > 0) { + Logger.recordOutput("Limelight/Megatag2Count", poseEstimate.tagCount); - Logger.recordOutput("FieldSimulation/LLPose", poseEstimate.pose); + Logger.recordOutput("FieldSimulation/LLPose", poseEstimate.pose); - // Add it to the pose estimator. - swerveDrive.addVisionMeasurement( - poseEstimate.pose.toPose2d(), - poseEstimate.timestampSeconds); - // TODO: possibly add stddevs here - // TODO: Instead of providing the limelight's pose as is, replace the rotation - // component with the current pose rotation so the process doesn't take it into - // account? + // Add it to the pose estimator. + swerveDrive.addVisionMeasurement( + poseEstimate.pose.toPose2d(), + poseEstimate.timestampSeconds); - } - }); + // TODO: possibly add stddevs here + // TODO: Instead of providing the limelight's pose as is, replace the + // rotation + // component with the current pose rotation so the process doesn't take it + // into + // account? + + } + }); + } } @Override From 47099d170128d9a5dba7fa3c17b8f68de2c8e088 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Sun, 11 Jan 2026 17:23:48 -0500 Subject: [PATCH 03/11] Basic SparkFlex intake for Sunday night show --- src/main/java/frc/robot/Constants.java | 5 ++ src/main/java/frc/robot/RobotContainer.java | 8 ++- .../frc/robot/controls/OperatorControls.java | 6 ++ .../frc/robot/subsystems/IntakeSubsystem.java | 58 +++++++++++++++++++ .../frc/robot/subsystems/Superstructure.java | 20 ++++++- 5 files changed, 93 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d577f4..2eab002 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -79,4 +79,9 @@ public static class HoodConstants { // 1 Neo, 0-90 degree variability, 50:1 reduction public static final int kMotorId = 19; } + + public static class IntakeConstants { + // SparkFlex controlling the intake flywheel + public static final int kFlywheelMotorId = 30; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f2bff9..b775489 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,11 +17,15 @@ import frc.robot.controls.DriverControls; import frc.robot.controls.OperatorControls; import frc.robot.controls.PoseControls; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; import swervelib.SwerveDrive; public class RobotContainer { private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + private final IntakeSubsystem intake = new IntakeSubsystem(); + private final Superstructure superstructure = new Superstructure(null, null, null, intake); private final SendableChooser autoChooser; @@ -53,8 +57,8 @@ public RobotContainer() { private void configureBindings() { // Set up controllers - DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null); - OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, null); + DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, superstructure); + OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, superstructure); PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase); } diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index ef4c3bd..c762bc1 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -8,5 +8,11 @@ public class OperatorControls { public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { CommandXboxController controller = new CommandXboxController(port); + + // Intake controls - A to intake, B to eject + if (superstructure != null) { + controller.a().whileTrue(superstructure.intakeCommand()); + controller.b().whileTrue(superstructure.ejectCommand()); + } } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..eea0fbf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class IntakeSubsystem extends SubsystemBase { + private final SparkFlex flywheelMotor; + + private final double INTAKE_SPEED = 0.5; + + public IntakeSubsystem() { + flywheelMotor = new SparkFlex(Constants.IntakeConstants.kFlywheelMotorId, MotorType.kBrushless); + } + + /** + * Run the intake to pull game pieces in. + */ + public void intake() { + flywheelMotor.set(INTAKE_SPEED); + } + + /** + * Run the intake in reverse to eject game pieces. + */ + public void eject() { + flywheelMotor.set(-INTAKE_SPEED); + } + + /** + * Stop the intake motor. + */ + public void stop() { + flywheelMotor.set(0); + } + + /** + * Command to run the intake while held. + */ + public Command intakeCommand() { + return this.runEnd(this::intake, this::stop).withName("Intake.Run"); + } + + /** + * Command to eject while held. + */ + public Command ejectCommand() { + return this.runEnd(this::eject, this::stop).withName("Intake.Eject"); + } + + @Override + public void periodic() { + // Add any periodic logging here if needed + } +} diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9e6adce..aaeae65 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * Superstructure coordinates the shooter, turret, and hood subsystems + * Superstructure coordinates the shooter, turret, hood, and intake subsystems * for unified control during shooting operations. */ public class Superstructure extends SubsystemBase { @@ -20,6 +20,7 @@ public class Superstructure extends SubsystemBase { private final ShooterSubsystem shooter; private final TurretSubsystem turret; private final HoodSubsystem hood; + private final IntakeSubsystem intake; // Default values for "ready" state private static final AngularVelocity DEFAULT_SHOOTER_SPEED = RPM.of(4000); @@ -41,10 +42,11 @@ public class Superstructure extends SubsystemBase { private Angle targetTurretAngle = Degrees.of(0); private Angle targetHoodAngle = Degrees.of(0); - public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood) { + public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake) { this.shooter = shooter; this.turret = turret; this.hood = hood; + this.intake = intake; // Create triggers for checking if mechanisms are at their targets this.isShooterAtSpeed = new Trigger( @@ -188,6 +190,20 @@ public Angle getTargetHoodAngle() { return targetHoodAngle; } + /** + * Command to run the intake while held. + */ + public Command intakeCommand() { + return intake.intakeCommand().withName("Superstructure.intake"); + } + + /** + * Command to eject while held. + */ + public Command ejectCommand() { + return intake.ejectCommand().withName("Superstructure.eject"); + } + @Override public void periodic() { // Superstructure doesn't need periodic updates - subsystems handle their own From 41597d91a12e332a294d83d5d678f72bb6538160 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Sun, 11 Jan 2026 18:43:47 -0500 Subject: [PATCH 04/11] YAMS-ify it, baby! --- .../frc/robot/subsystems/IntakeSubsystem.java | 69 ++++++++++++------- 1 file changed, 44 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index eea0fbf..e03f8ff 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -3,56 +3,75 @@ import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.SparkWrapper; public class IntakeSubsystem extends SubsystemBase { - private final SparkFlex flywheelMotor; - private final double INTAKE_SPEED = 0.5; + private static final double INTAKE_SPEED = 0.5; - public IntakeSubsystem() { - flywheelMotor = new SparkFlex(Constants.IntakeConstants.kFlywheelMotorId, MotorType.kBrushless); - } + // SparkFlex controlling the intake flywheel + private SparkFlex flywheelSpark = new SparkFlex(Constants.IntakeConstants.kFlywheelMotorId, MotorType.kBrushless); - /** - * Run the intake to pull game pieces in. - */ - public void intake() { - flywheelMotor.set(INTAKE_SPEED); - } + private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.OPEN_LOOP) + .withTelemetry("IntakeMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) // Direct drive, adjust if geared + .withMotorInverted(false) + .withIdleMode(MotorMode.COAST) + .withStatorCurrentLimit(Amps.of(40)); - /** - * Run the intake in reverse to eject game pieces. - */ - public void eject() { - flywheelMotor.set(-INTAKE_SPEED); - } + private SmartMotorController smc = new SparkWrapper(flywheelSpark, DCMotor.getNeoVortex(1), smcConfig); - /** - * Stop the intake motor. - */ - public void stop() { - flywheelMotor.set(0); + private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(0.5)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) + .withTelemetry("Intake", TelemetryVerbosity.HIGH); + + private FlyWheel intake = new FlyWheel(intakeConfig); + + public IntakeSubsystem() { } /** * Command to run the intake while held. */ public Command intakeCommand() { - return this.runEnd(this::intake, this::stop).withName("Intake.Run"); + return intake.set(INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Run"); } /** * Command to eject while held. */ public Command ejectCommand() { - return this.runEnd(this::eject, this::stop).withName("Intake.Eject"); + return intake.set(-INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Eject"); } @Override public void periodic() { - // Add any periodic logging here if needed + intake.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + intake.simIterate(); } } From 01af3c1ca076de545560fdbd24afae183f8cb484 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Mon, 12 Jan 2026 14:11:17 -0500 Subject: [PATCH 05/11] Add Hopper subsystem --- src/main/java/frc/robot/Constants.java | 14 +++- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/controls/OperatorControls.java | 10 ++- .../frc/robot/subsystems/HopperSubsystem.java | 84 +++++++++++++++++++ .../frc/robot/subsystems/Superstructure.java | 18 +++- .../frc/robot/subsystems/TurretSubsystem.java | 21 +++-- 6 files changed, 130 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/HopperSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2eab002..1013072 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,10 +69,9 @@ public static class ShooterConstants { } public static class TurretConstants { - // 2 Neos, 12in diameter, 25:1 gearbox, 10:1 pivot gearing, non-continuous (270 - // FOV) - public static final int kLeaderMotorId = 17; - public static final int kFollowerMotorId = 18; + // 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous + // 360 deg + public static final int kMotorId = 17; } public static class HoodConstants { @@ -80,8 +79,15 @@ public static class HoodConstants { public static final int kMotorId = 19; } + // Intake subsystem CAN IDs start at 30 public static class IntakeConstants { // SparkFlex controlling the intake flywheel public static final int kFlywheelMotorId = 30; + public static final int kRollerMotorId = 31; + } + + // Hopper subsystem CAN IDs start at 40 + public static class HopperConstants { + public static final int kHopperMotorId = 40; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b775489..232def9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import frc.robot.controls.DriverControls; import frc.robot.controls.OperatorControls; import frc.robot.controls.PoseControls; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; @@ -25,7 +26,8 @@ public class RobotContainer { private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); private final IntakeSubsystem intake = new IntakeSubsystem(); - private final Superstructure superstructure = new Superstructure(null, null, null, intake); + private final HopperSubsystem hopper = new HopperSubsystem(); + private final Superstructure superstructure = new Superstructure(null, null, null, intake, hopper); private final SendableChooser autoChooser; diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index c762bc1..7a37882 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -10,9 +10,11 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur CommandXboxController controller = new CommandXboxController(port); // Intake controls - A to intake, B to eject - if (superstructure != null) { - controller.a().whileTrue(superstructure.intakeCommand()); - controller.b().whileTrue(superstructure.ejectCommand()); - } + controller.a().whileTrue(superstructure.intakeCommand()); + controller.b().whileTrue(superstructure.ejectCommand()); + + // Hopper controls - X to run hopper forward, Y to run backward + controller.x().whileTrue(superstructure.feedCommand()); + controller.y().whileTrue(superstructure.hopperReverseCommand()); } } diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java new file mode 100644 index 0000000..1263faa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.thethriftybot.ThriftyNova; + +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.NovaWrapper; + +public class HopperSubsystem extends SubsystemBase { + + private static final double HOPPER_SPEED = 0.5; + + // Nova motor controller with NEO motor + private ThriftyNova hopperNova = new ThriftyNova(Constants.HopperConstants.kHopperMotorId); + + private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.OPEN_LOOP) + .withTelemetry("HopperMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) // 4:1 gear reduction + .withMotorInverted(false) + .withIdleMode(MotorMode.BRAKE) + .withStatorCurrentLimit(Amps.of(20)); + + private SmartMotorController smc = new NovaWrapper(hopperNova, DCMotor.getNEO(1), smcConfig); + + private final FlyWheelConfig hopperConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(0.5)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) + .withTelemetry("Hopper", TelemetryVerbosity.HIGH); + + private FlyWheel hopper = new FlyWheel(hopperConfig); + + public HopperSubsystem() { + } + + /** + * Command to run the hopper forward while held. + */ + public Command feedCommand() { + return hopper.set(HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Feed"); + } + + /** + * Command to run the hopper in reverse while held. + */ + public Command reverseCommand() { + return hopper.set(-HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Reverse"); + } + + /** + * Command to stop the hopper. + */ + public Command stopCommand() { + return hopper.set(0).withName("Hopper.Stop"); + } + + @Override + public void periodic() { + hopper.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + hopper.simIterate(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index aaeae65..79747ca 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -21,6 +21,7 @@ public class Superstructure extends SubsystemBase { private final TurretSubsystem turret; private final HoodSubsystem hood; private final IntakeSubsystem intake; + private final HopperSubsystem hopper; // Default values for "ready" state private static final AngularVelocity DEFAULT_SHOOTER_SPEED = RPM.of(4000); @@ -42,11 +43,12 @@ public class Superstructure extends SubsystemBase { private Angle targetTurretAngle = Degrees.of(0); private Angle targetHoodAngle = Degrees.of(0); - public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake) { + public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake, HopperSubsystem hopper) { this.shooter = shooter; this.turret = turret; this.hood = hood; this.intake = intake; + this.hopper = hopper; // Create triggers for checking if mechanisms are at their targets this.isShooterAtSpeed = new Trigger( @@ -204,6 +206,20 @@ public Command ejectCommand() { return intake.ejectCommand().withName("Superstructure.eject"); } + /** + * Command to run the hopper forward while held. + */ + public Command feedCommand() { + return hopper.feedCommand().withName("Superstructure.feed"); + } + + /** + * Command to run the hopper in reverse while held. + */ + public Command hopperReverseCommand() { + return hopper.reverseCommand().withName("Superstructure.hopperReverse"); + } + @Override public void periodic() { // Superstructure doesn't need periodic updates - subsystems handle their own diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 4cdf9e1..9e01d9c 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -3,7 +3,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.system.plant.DCMotor; import static edu.wpi.first.units.Units.Amps; @@ -30,30 +29,30 @@ public class TurretSubsystem extends SubsystemBase { - // 2 Neos, 12in diameter, 25:1 gearbox, 10:1 pivot gearing reduction, - // non-continuous (270 FOV) - // Total reduction: 25 * 10 = 250:1 - private SparkMax leaderSpark = new SparkMax(Constants.TurretConstants.kLeaderMotorId, MotorType.kBrushless); - private SparkMax followerSpark = new SparkMax(Constants.TurretConstants.kFollowerMotorId, MotorType.kBrushless); + private final double MAX_ONE_DIR_FOV = 45; // degrees + + // 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous + // 360 deg + // Total reduction: 4 * 10 = 40:1 + private SparkMax spark = new SparkMax(Constants.TurretConstants.kMotorId, MotorType.kBrushless); private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withFollowers(Pair.of(followerSpark, false)) .withControlMode(ControlMode.CLOSED_LOOP) .withClosedLoopController(100, 0, 0, DegreesPerSecond.of(180), DegreesPerSecondPerSecond.of(180)) .withFeedforward(new ArmFeedforward(0, 0, 0.1)) .withTelemetry("TurretMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(25, 10))) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(4, 10))) .withMotorInverted(false) .withIdleMode(MotorMode.BRAKE) - .withSoftLimit(Degrees.of(-135), Degrees.of(135)) // 270 FOV, centered at 0 + .withSoftLimit(Degrees.of(-MAX_ONE_DIR_FOV), Degrees.of(MAX_ONE_DIR_FOV)) .withStatorCurrentLimit(Amps.of(40)) .withClosedLoopRampRate(Seconds.of(0.1)) .withOpenLoopRampRate(Seconds.of(0.1)); - private SmartMotorController smc = new SparkWrapper(leaderSpark, DCMotor.getNEO(2), smcConfig); + private SmartMotorController smc = new SparkWrapper(spark, DCMotor.getNEO(1), smcConfig); private final PivotConfig turretConfig = new PivotConfig(smc) - .withHardLimit(Degrees.of(-140), Degrees.of(140)) + .withHardLimit(Degrees.of(-MAX_ONE_DIR_FOV - 5), Degrees.of(MAX_ONE_DIR_FOV + 5)) .withStartingPosition(Degrees.of(0)) .withMOI(0.05) .withTelemetry("Turret", TelemetryVerbosity.HIGH); From 930c6e0a580c26ad56c7a53f53984c37cd86062c Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Mon, 12 Jan 2026 14:15:20 -0500 Subject: [PATCH 06/11] Update the turret to use a Nova --- src/main/java/frc/robot/subsystems/Superstructure.java | 3 ++- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 9 ++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 79747ca..be72cb1 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -43,7 +43,8 @@ public class Superstructure extends SubsystemBase { private Angle targetTurretAngle = Degrees.of(0); private Angle targetHoodAngle = Degrees.of(0); - public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake, HopperSubsystem hopper) { + public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake, + HopperSubsystem hopper) { this.shooter = shooter; this.turret = turret; this.hood = hood; diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 9e01d9c..c393aec 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; +import com.thethriftybot.ThriftyNova; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.system.plant.DCMotor; @@ -25,7 +24,7 @@ import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.SparkWrapper; +import yams.motorcontrollers.local.NovaWrapper; public class TurretSubsystem extends SubsystemBase { @@ -34,7 +33,7 @@ public class TurretSubsystem extends SubsystemBase { // 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous // 360 deg // Total reduction: 4 * 10 = 40:1 - private SparkMax spark = new SparkMax(Constants.TurretConstants.kMotorId, MotorType.kBrushless); + private ThriftyNova nova = new ThriftyNova(Constants.TurretConstants.kMotorId); private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) .withControlMode(ControlMode.CLOSED_LOOP) @@ -49,7 +48,7 @@ public class TurretSubsystem extends SubsystemBase { .withClosedLoopRampRate(Seconds.of(0.1)) .withOpenLoopRampRate(Seconds.of(0.1)); - private SmartMotorController smc = new SparkWrapper(spark, DCMotor.getNEO(1), smcConfig); + private SmartMotorController smc = new NovaWrapper(nova, DCMotor.getNEO(1), smcConfig); private final PivotConfig turretConfig = new PivotConfig(smc) .withHardLimit(Degrees.of(-MAX_ONE_DIR_FOV - 5), Degrees.of(MAX_ONE_DIR_FOV + 5)) From 1ae1b61d6c3fbd4e771f7516ae76589c8bb87c43 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Mon, 12 Jan 2026 15:30:03 -0500 Subject: [PATCH 07/11] Working hopper with operator controls --- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 17 ++++++++--------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1013072..0bc8594 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -82,7 +82,7 @@ public static class HoodConstants { // Intake subsystem CAN IDs start at 30 public static class IntakeConstants { // SparkFlex controlling the intake flywheel - public static final int kFlywheelMotorId = 30; + public static final int kPivotMotorId = 30; public static final int kRollerMotorId = 31; } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e03f8ff..1720744 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.thethriftybot.ThriftyNova; import edu.wpi.first.math.system.plant.DCMotor; import static edu.wpi.first.units.Units.Amps; @@ -20,14 +19,14 @@ import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.SparkWrapper; +import yams.motorcontrollers.local.NovaWrapper; public class IntakeSubsystem extends SubsystemBase { - private static final double INTAKE_SPEED = 0.5; + private static final double INTAKE_SPEED = 1.0; - // SparkFlex controlling the intake flywheel - private SparkFlex flywheelSpark = new SparkFlex(Constants.IntakeConstants.kFlywheelMotorId, MotorType.kBrushless); + // ThriftyNova controlling the intake roller + private ThriftyNova rollerNova = new ThriftyNova(Constants.IntakeConstants.kRollerMotorId); private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) .withControlMode(ControlMode.OPEN_LOOP) @@ -37,13 +36,13 @@ public class IntakeSubsystem extends SubsystemBase { .withIdleMode(MotorMode.COAST) .withStatorCurrentLimit(Amps.of(40)); - private SmartMotorController smc = new SparkWrapper(flywheelSpark, DCMotor.getNeoVortex(1), smcConfig); + private SmartMotorController smc = new NovaWrapper(rollerNova, DCMotor.getNeoVortex(1), smcConfig); private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc) .withDiameter(Inches.of(4)) .withMass(Pounds.of(0.5)) - .withUpperSoftLimit(RPM.of(6000)) - .withLowerSoftLimit(RPM.of(-6000)) + .withUpperSoftLimit(RPM.of(600000)) + .withLowerSoftLimit(RPM.of(-600000)) .withTelemetry("Intake", TelemetryVerbosity.HIGH); private FlyWheel intake = new FlyWheel(intakeConfig); From c43a62ff961b89c340df3266829418abacdff7d6 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Mon, 12 Jan 2026 15:40:06 -0500 Subject: [PATCH 08/11] Ops --- src/main/java/frc/robot/subsystems/HopperSubsystem.java | 4 ++-- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 1263faa..3a39fcb 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -24,7 +24,7 @@ public class HopperSubsystem extends SubsystemBase { - private static final double HOPPER_SPEED = 0.5; + private static final double HOPPER_SPEED = 1.0; // Nova motor controller with NEO motor private ThriftyNova hopperNova = new ThriftyNova(Constants.HopperConstants.kHopperMotorId); @@ -35,7 +35,7 @@ public class HopperSubsystem extends SubsystemBase { .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) // 4:1 gear reduction .withMotorInverted(false) .withIdleMode(MotorMode.BRAKE) - .withStatorCurrentLimit(Amps.of(20)); + .withStatorCurrentLimit(Amps.of(40)); private SmartMotorController smc = new NovaWrapper(hopperNova, DCMotor.getNEO(1), smcConfig); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1720744..191752d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -23,7 +23,7 @@ public class IntakeSubsystem extends SubsystemBase { - private static final double INTAKE_SPEED = 1.0; + private static final double INTAKE_SPEED = 0.1; // ThriftyNova controlling the intake roller private ThriftyNova rollerNova = new ThriftyNova(Constants.IntakeConstants.kRollerMotorId); From ac5a1bce881aa17d365b589454af1472e277c2a9 Mon Sep 17 00:00:00 2001 From: Patrick Frampton Date: Mon, 12 Jan 2026 17:01:23 -0500 Subject: [PATCH 09/11] Add intake arm mechanism. Note that we still need to test this on real hardware and verify setpoints and limits. --- .../frc/robot/controls/OperatorControls.java | 11 +++++ .../frc/robot/subsystems/IntakeSubsystem.java | 43 +++++++++++++++++++ .../frc/robot/subsystems/Superstructure.java | 7 +++ 3 files changed, 61 insertions(+) diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index 7a37882..c9748ec 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -1,5 +1,7 @@ package frc.robot.controls; +import static edu.wpi.first.units.Units.Degrees; + import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; @@ -16,5 +18,14 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur // Hopper controls - X to run hopper forward, Y to run backward controller.x().whileTrue(superstructure.feedCommand()); controller.y().whileTrue(superstructure.hopperReverseCommand()); + + // Intake pivot controls. Setpoints need to be tested and finalized. + + // 0 for default + // -45 for collection + // +25 just because. We can add more setpoints if necessary. + controller.povUp().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(25))); + controller.povRight().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(0))); + controller.povDown().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(-45))); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 191752d..3eaa57f 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -2,17 +2,28 @@ import com.thethriftybot.ThriftyNova; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; + import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond; +import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Pounds; import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import yams.gearing.GearBox; import yams.gearing.MechanismGearing; +import yams.mechanisms.config.ArmConfig; import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.positional.Arm; import yams.mechanisms.velocity.FlyWheel; import yams.motorcontrollers.SmartMotorController; import yams.motorcontrollers.SmartMotorControllerConfig; @@ -47,6 +58,32 @@ public class IntakeSubsystem extends SubsystemBase { private FlyWheel intake = new FlyWheel(intakeConfig); + // 5:1, 5:1, 60/18 reduction + private SmartMotorControllerConfig intakePivotSmartMotorConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.CLOSED_LOOP) + .withClosedLoopController(200, 0, 0, DegreesPerSecond.of(360), DegreesPerSecondPerSecond.of(360)) + .withFeedforward(new ArmFeedforward(0, 0, 0.1)) + .withTelemetry("IntakePivotMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(5,5, 60.0/18))) + .withMotorInverted(true) + .withIdleMode(MotorMode.BRAKE) + .withStatorCurrentLimit(Amps.of(10)) + .withClosedLoopRampRate(Seconds.of(0.1)); + + private ThriftyNova pivotMotor = new ThriftyNova(Constants.IntakeConstants.kPivotMotorId); + + private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1), intakePivotSmartMotorConfig); + + private final ArmConfig intakePivotConfig = new ArmConfig(intakePivotController) + .withSoftLimits(Degrees.of(-95), Degrees.of(45)) // TODO: Find and set proper limits once setpoints and range is known + .withHardLimit(Degrees.of(-100), Degrees.of(50)) + .withStartingPosition(Degrees.of(-90)) + .withLength(Feet.of(1)) + .withMass(Pounds.of(2)) // Reis says: 2 pounds, not a lot + .withTelemetry("IntakePivot", TelemetryVerbosity.HIGH); + + private Arm intakePivot = new Arm(intakePivotConfig); + public IntakeSubsystem() { } @@ -64,13 +101,19 @@ public Command ejectCommand() { return intake.set(-INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Eject"); } + public Command setPivotAngle(Angle angle) { + return intakePivot.setAngle(angle).withName("IntakePivot.SetAngle"); + } + @Override public void periodic() { intake.updateTelemetry(); + intakePivot.updateTelemetry(); } @Override public void simulationPeriodic() { intake.simIterate(); + intakePivot.simIterate(); } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index be72cb1..758c471 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -221,6 +221,13 @@ public Command hopperReverseCommand() { return hopper.reverseCommand().withName("Superstructure.hopperReverse"); } + /** + * Command to set the intake pivot angle. + */ + public Command setIntakePivotAngle(Angle angle) { + return intake.setPivotAngle(angle).withName("Superstructure.setIntakePivotAngle"); + } + @Override public void periodic() { // Superstructure doesn't need periodic updates - subsystems handle their own From e9ec2661abd6f954b6d20e1b220737896946bd01 Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Mon, 12 Jan 2026 20:25:21 -0500 Subject: [PATCH 10/11] "Big cook" --- src/main/java/frc/robot/Constants.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 8 +- .../frc/robot/controls/OperatorControls.java | 6 +- .../frc/robot/subsystems/IntakeSubsystem.java | 17 +++-- .../frc/robot/subsystems/KickerSubsystem.java | 76 +++++++++++++++++++ .../robot/subsystems/ShooterSubsystem.java | 27 ++++--- .../frc/robot/subsystems/Superstructure.java | 32 +++++++- 7 files changed, 149 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/KickerSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0bc8594..34c5163 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,7 +63,7 @@ public static class CoralConstants { } public static class ShooterConstants { - // 2 Neos, 4in shooter wheels, 4:1 gearbox reduction + // 2 Neos, 4in shooter wheels public static final int kLeaderMotorId = 15; public static final int kFollowerMotorId = 16; } @@ -90,4 +90,9 @@ public static class IntakeConstants { public static class HopperConstants { public static final int kHopperMotorId = 40; } + + // Kicker subsystem CAN IDs start at 50 + public static class KickerConstants { + public static final int kKickerMotorId = 50; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 232def9..1869db3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,15 +19,21 @@ import frc.robot.controls.PoseControls; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.KickerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; import swervelib.SwerveDrive; public class RobotContainer { private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + private final IntakeSubsystem intake = new IntakeSubsystem(); private final HopperSubsystem hopper = new HopperSubsystem(); - private final Superstructure superstructure = new Superstructure(null, null, null, intake, hopper); + private final KickerSubsystem kicker = new KickerSubsystem(); + private final ShooterSubsystem shooter = new ShooterSubsystem(); + + private final Superstructure superstructure = new Superstructure(shooter, null, null, intake, hopper, kicker); private final SendableChooser autoChooser; diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index c9748ec..b99eff7 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -16,9 +16,13 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur controller.b().whileTrue(superstructure.ejectCommand()); // Hopper controls - X to run hopper forward, Y to run backward - controller.x().whileTrue(superstructure.feedCommand()); + controller.x().whileTrue(superstructure.hopperFeedCommand()); controller.y().whileTrue(superstructure.hopperReverseCommand()); + // Shooter controls - Right bumper to shoot + controller.rightBumper().whileTrue(superstructure.shootCommand()); + controller.leftBumper().whileTrue(superstructure.stopShootingCommand()); + // Intake pivot controls. Setpoints need to be tested and finalized. // 0 for default diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 3eaa57f..e2be8e8 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -17,6 +17,7 @@ import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import yams.gearing.GearBox; @@ -52,8 +53,8 @@ public class IntakeSubsystem extends SubsystemBase { private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc) .withDiameter(Inches.of(4)) .withMass(Pounds.of(0.5)) - .withUpperSoftLimit(RPM.of(600000)) - .withLowerSoftLimit(RPM.of(-600000)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) .withTelemetry("Intake", TelemetryVerbosity.HIGH); private FlyWheel intake = new FlyWheel(intakeConfig); @@ -64,7 +65,7 @@ public class IntakeSubsystem extends SubsystemBase { .withClosedLoopController(200, 0, 0, DegreesPerSecond.of(360), DegreesPerSecondPerSecond.of(360)) .withFeedforward(new ArmFeedforward(0, 0, 0.1)) .withTelemetry("IntakePivotMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(5,5, 60.0/18))) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 5, 60.0 / 18))) .withMotorInverted(true) .withIdleMode(MotorMode.BRAKE) .withStatorCurrentLimit(Amps.of(10)) @@ -72,10 +73,12 @@ public class IntakeSubsystem extends SubsystemBase { private ThriftyNova pivotMotor = new ThriftyNova(Constants.IntakeConstants.kPivotMotorId); - private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1), intakePivotSmartMotorConfig); + private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1), + intakePivotSmartMotorConfig); private final ArmConfig intakePivotConfig = new ArmConfig(intakePivotController) - .withSoftLimits(Degrees.of(-95), Degrees.of(45)) // TODO: Find and set proper limits once setpoints and range is known + .withSoftLimits(Degrees.of(-95), Degrees.of(45)) // TODO: Find and set proper limits once setpoints and range is + // known .withHardLimit(Degrees.of(-100), Degrees.of(50)) .withStartingPosition(Degrees.of(-90)) .withLength(Feet.of(1)) @@ -105,6 +108,10 @@ public Command setPivotAngle(Angle angle) { return intakePivot.setAngle(angle).withName("IntakePivot.SetAngle"); } + public Command rezero() { + return Commands.runOnce(() -> pivotMotor.setEncoderPosition(0), this).withName("IntakePivot.Rezero"); + } + @Override public void periodic() { intake.updateTelemetry(); diff --git a/src/main/java/frc/robot/subsystems/KickerSubsystem.java b/src/main/java/frc/robot/subsystems/KickerSubsystem.java new file mode 100644 index 0000000..477a1bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/KickerSubsystem.java @@ -0,0 +1,76 @@ +package frc.robot.subsystems; + +import com.thethriftybot.ThriftyNova; + +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.NovaWrapper; + +public class KickerSubsystem extends SubsystemBase { + + private static final double KICKER_SPEED = 1.0; + + // Nova motor controller with NEO motor + private ThriftyNova kickerNova = new ThriftyNova(Constants.KickerConstants.kKickerMotorId); + + private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.OPEN_LOOP) + .withTelemetry("KickerMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) // 4:1 gear reduction + .withMotorInverted(false) + .withIdleMode(MotorMode.BRAKE) + .withStatorCurrentLimit(Amps.of(20)); + + private SmartMotorController smc = new NovaWrapper(kickerNova, DCMotor.getNEO(1), smcConfig); + + private final FlyWheelConfig kickerConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(0.5)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) + .withTelemetry("Kicker", TelemetryVerbosity.HIGH); + + private FlyWheel kicker = new FlyWheel(kickerConfig); + + public KickerSubsystem() { + } + + /** + * Command to run the kicker forward while held, stops when released. + */ + public Command feedCommand() { + return kicker.set(KICKER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Kicker.Feed"); + } + + /** + * Command to stop the kicker. + */ + public Command stopCommand() { + return kicker.set(0).withName("Kicker.Stop"); + } + + @Override + public void periodic() { + kicker.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + kicker.simIterate(); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index da43d57..8ac75de 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; +import com.thethriftybot.ThriftyNova; import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -27,35 +26,35 @@ import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.SparkWrapper; +import yams.motorcontrollers.local.NovaWrapper; public class ShooterSubsystem extends SubsystemBase { - // 2 Neos, 4in shooter wheels, 4:1 gearbox reduction - private SparkMax leaderSpark = new SparkMax(Constants.ShooterConstants.kLeaderMotorId, MotorType.kBrushless); - private SparkMax followerSpark = new SparkMax(Constants.ShooterConstants.kFollowerMotorId, MotorType.kBrushless); + // 2 Neos, 4in shooter wheels + private final ThriftyNova leaderNova = new ThriftyNova(Constants.ShooterConstants.kLeaderMotorId); + private final ThriftyNova followerNova = new ThriftyNova(Constants.ShooterConstants.kFollowerMotorId); - private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withFollowers(Pair.of(followerSpark, false)) + private final SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withFollowers(Pair.of(followerNova, false)) .withControlMode(ControlMode.CLOSED_LOOP) .withClosedLoopController(0.1, 0, 0) .withFeedforward(new SimpleMotorFeedforward(0, 0.5, 0)) .withTelemetry("ShooterMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) .withMotorInverted(false) .withIdleMode(MotorMode.COAST) .withStatorCurrentLimit(Amps.of(40)); - private SmartMotorController smc = new SparkWrapper(leaderSpark, DCMotor.getNEO(2), smcConfig); + private final SmartMotorController smc = new NovaWrapper(leaderNova, DCMotor.getNEO(2), smcConfig); private final FlyWheelConfig shooterConfig = new FlyWheelConfig(smc) .withDiameter(Inches.of(4)) .withMass(Pounds.of(1)) - .withUpperSoftLimit(RPM.of(6000)) - .withLowerSoftLimit(RPM.of(0)) + .withUpperSoftLimit(RotationsPerSecond.of(6000)) + .withLowerSoftLimit(RotationsPerSecond.of(0)) .withTelemetry("Shooter", TelemetryVerbosity.HIGH); - private FlyWheel shooter = new FlyWheel(shooterConfig); + private final FlyWheel shooter = new FlyWheel(shooterConfig); public ShooterSubsystem() { } @@ -65,7 +64,7 @@ public Command setSpeed(AngularVelocity speed) { } public Command spinUp() { - return shooter.setSpeed(RotationsPerSecond.of(50)); + return shooter.setSpeed(RotationsPerSecond.of(500)); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 758c471..5a38993 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -22,6 +22,7 @@ public class Superstructure extends SubsystemBase { private final HoodSubsystem hood; private final IntakeSubsystem intake; private final HopperSubsystem hopper; + private final KickerSubsystem kicker; // Default values for "ready" state private static final AngularVelocity DEFAULT_SHOOTER_SPEED = RPM.of(4000); @@ -44,12 +45,13 @@ public class Superstructure extends SubsystemBase { private Angle targetHoodAngle = Degrees.of(0); public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake, - HopperSubsystem hopper) { + HopperSubsystem hopper, KickerSubsystem kicker) { this.shooter = shooter; this.turret = turret; this.hood = hood; this.intake = intake; this.hopper = hopper; + this.kicker = kicker; // Create triggers for checking if mechanisms are at their targets this.isShooterAtSpeed = new Trigger( @@ -210,7 +212,7 @@ public Command ejectCommand() { /** * Command to run the hopper forward while held. */ - public Command feedCommand() { + public Command hopperFeedCommand() { return hopper.feedCommand().withName("Superstructure.feed"); } @@ -221,6 +223,13 @@ public Command hopperReverseCommand() { return hopper.reverseCommand().withName("Superstructure.hopperReverse"); } + /** + * Command to run the kicker forward while held, stops when released. + */ + public Command kickerFeedCommand() { + return kicker.feedCommand().withName("Superstructure.kickerFeed"); + } + /** * Command to set the intake pivot angle. */ @@ -228,6 +237,25 @@ public Command setIntakePivotAngle(Angle angle) { return intake.setPivotAngle(angle).withName("Superstructure.setIntakePivotAngle"); } + /** + * Command to shoot - spins up shooter. + */ + public Command shootCommand() { + return shooter.spinUp().withName("Superstructure.shoot"); + } + + /** + * Command to stop shooting - stops shooter. + */ + public Command stopShootingCommand() { + return shooter.stop().withName("Superstructure.stopShooting"); + } + + // Re-zero intake pivot if needed + public Command rezeroIntakePivotCommand() { + return Commands.runOnce(() -> intake.rezero(), intake).withName("Superstructure.rezeroIntakePivot"); + } + @Override public void periodic() { // Superstructure doesn't need periodic updates - subsystems handle their own From 0d1ca30df39b8d094f5cba619cb000232cd0cb4a Mon Sep 17 00:00:00 2001 From: Jordan Miller Date: Mon, 12 Jan 2026 22:41:38 -0500 Subject: [PATCH 11/11] Kicker controls --- src/main/java/frc/robot/controls/OperatorControls.java | 10 +++++++--- src/main/java/frc/robot/subsystems/Superstructure.java | 7 +++++++ 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index b99eff7..bc0dd60 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -23,13 +23,17 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur controller.rightBumper().whileTrue(superstructure.shootCommand()); controller.leftBumper().whileTrue(superstructure.stopShootingCommand()); + // Kicker controls + controller.back().whileTrue(superstructure.kickerFeedCommand()); + controller.start().whileTrue(superstructure.kickerStopCommand()); + // Intake pivot controls. Setpoints need to be tested and finalized. // 0 for default // -45 for collection // +25 just because. We can add more setpoints if necessary. - controller.povUp().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(25))); - controller.povRight().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(0))); - controller.povDown().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(-45))); + // controller.povUp().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(25))); + // controller.povRight().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(0))); + // controller.povDown().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(-45))); } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 5a38993..b7daef5 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -230,6 +230,13 @@ public Command kickerFeedCommand() { return kicker.feedCommand().withName("Superstructure.kickerFeed"); } + /** + * Command to run the kicker stop while held, stops when released. + */ + public Command kickerStopCommand() { + return kicker.stopCommand().withName("Superstructure.kickerStop"); + } + /** * Command to set the intake pivot angle. */