From 8bf49324369f1412089ef922734e3ab355888d74 Mon Sep 17 00:00:00 2001 From: novA2026 Date: Mon, 2 Mar 2026 20:34:27 -0500 Subject: [PATCH] added kicker subsystem and removed feeder from indexer --- src/main/java/org/team2342/frc/Constants.java | 20 +++++--- .../java/org/team2342/frc/RobotContainer.java | 11 ++-- .../frc/subsystems/indexer/Indexer.java | 22 ++++---- .../frc/subsystems/kicker/Kicker.java | 51 +++++++++++++++++++ 4 files changed, 78 insertions(+), 26 deletions(-) create mode 100644 src/main/java/org/team2342/frc/subsystems/kicker/Kicker.java diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 3c7532e..9b14a47 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -115,6 +115,17 @@ public static final class DriveConstants { public static final double ODOMETRY_FREQUENCY = IS_CANFD ? 250.0 : 100.0; } + public static final class KickerConstants { + public static final double RUN_VOLTAGE = 7.0; + + public static final MotorConfig KICKER_CONFIG = + new MotorConfig() + .withMotorInverted(true) + .withSupplyCurrentLimit(30.0) + .withStatorCurrentLimit(40.0) + .withIdleMode(MotorConfig.IdleMode.BRAKE); + } + public static final class IndexerConstants { public static final double RUN_VOLTAGE = 7.0; public static final double FEEDER_VOLTAGE = 7.0; @@ -135,12 +146,6 @@ public static final class IndexerConstants { .withSupplyCurrentLimit(30.0) .withStatorCurrentLimit(40.0) .withIdleMode(MotorConfig.IdleMode.BRAKE); - public static final MotorConfig INDEXER_FEEDER_CONFIG = - new MotorConfig() - .withMotorInverted(true) - .withSupplyCurrentLimit(30.0) - .withStatorCurrentLimit(40.0) - .withIdleMode(MotorConfig.IdleMode.BRAKE); } public static final class IntakeConstants { @@ -225,7 +230,8 @@ public static final class CANConstants { public static final int INDEXER_WHEEL_ID = 20; public static final int INDEXER_BELT_ID = 21; - public static final int INDEXER_FEEDER_ID = 22; + + public static final int KICKER_ID = 22; public static final int PDH_ID = 62; } diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 78e2927..4b2f050 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -89,10 +89,9 @@ public RobotContainer() { new Indexer( new DumbMotorIOTalonFXFOC( CANConstants.INDEXER_WHEEL_ID, IndexerConstants.INDEXER_WHEEL_CONFIG), - new DumbMotorIOTalonFXFOC( - CANConstants.INDEXER_BELT_ID, IndexerConstants.INDEXER_BELT_CONFIG), - new DumbMotorIOTalonFXFOC( - CANConstants.INDEXER_FEEDER_ID, IndexerConstants.INDEXER_WHEEL_CONFIG)); + new DumbMotorIO() { + + }); wheels = new Wheels( @@ -132,7 +131,7 @@ public RobotContainer() { PoseStrategy.CONSTRAINED_SOLVEPNP, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, drive::getRawOdometryPose)); - indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}, new DumbMotorIO() {}); + indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}); wheels = new Wheels( @@ -168,7 +167,7 @@ public RobotContainer() { drive::getTimestampedHeading, new VisionIO() {}, new VisionIO() {}); - indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}, new DumbMotorIO() {}); + indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}); wheels = new Wheels(new DumbMotorIO() {}); flywheel = new Flywheel(new SmartMotorIO() {}); hood = new Hood(new SmartMotorIO() {}); diff --git a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java index 5c6e80c..be11cd1 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java @@ -19,22 +19,18 @@ public class Indexer extends SubsystemBase { private final DumbMotorIO wheelMotor; private final DumbMotorIO beltMotor; - private final DumbMotorIO feederMotor; private final DumbMotorIOInputsAutoLogged wheelMotorInputs = new DumbMotorIOInputsAutoLogged(); private final DumbMotorIOInputsAutoLogged beltMotorInputs = new DumbMotorIOInputsAutoLogged(); - private final DumbMotorIOInputsAutoLogged feederMotorInputs = new DumbMotorIOInputsAutoLogged(); - + private final Alert wheelMotorAlert = new Alert("Indexer Wheel Motor is diconnected", AlertType.kError); private final Alert beltMotorAlert = new Alert("Indexer Belt Motor is diconnected", AlertType.kError); - private final Alert feederMotorAlert = - new Alert("Indexer Feeder Motor is diconnected", AlertType.kError); - public Indexer(DumbMotorIO wheelMotor, DumbMotorIO beltMotor, DumbMotorIO feederMotor) { + + public Indexer(DumbMotorIO wheelMotor, DumbMotorIO beltMotor) { this.wheelMotor = wheelMotor; this.beltMotor = beltMotor; - this.feederMotor = feederMotor; setName("Indexer"); setDefaultCommand( @@ -42,7 +38,7 @@ public Indexer(DumbMotorIO wheelMotor, DumbMotorIO beltMotor, DumbMotorIO feeder () -> { wheelMotor.runVoltage(0.0); beltMotor.runVoltage(0.0); - feederMotor.runVoltage(0.0); + })); } @@ -50,15 +46,15 @@ public Indexer(DumbMotorIO wheelMotor, DumbMotorIO beltMotor, DumbMotorIO feeder public void periodic() { wheelMotor.updateInputs(wheelMotorInputs); beltMotor.updateInputs(beltMotorInputs); - feederMotor.updateInputs(feederMotorInputs); + Logger.processInputs("Indexer/WheelMotor", wheelMotorInputs); Logger.processInputs("Indexer/BeltMotor", beltMotorInputs); - Logger.processInputs("Indexer/FeederMotor", feederMotorInputs); + wheelMotorAlert.set(!wheelMotorInputs.connected); beltMotorAlert.set(!beltMotorInputs.connected); - feederMotorAlert.set(!feederMotorInputs.connected); + ExecutionLogger.log("Indexer"); } @@ -74,7 +70,7 @@ public Command feed() { return run(() -> { wheelMotor.runTorqueCurrent(IndexerConstants.RUN_CURRENT); beltMotor.runTorqueCurrent(IndexerConstants.RUN_CURRENT); - feederMotor.runTorqueCurrent(IndexerConstants.RUN_CURRENT); + }) .withName("Indexer Feed"); } @@ -84,7 +80,7 @@ public Command stop() { () -> { wheelMotor.runVoltage(0.0); beltMotor.runVoltage(0.0); - feederMotor.runVoltage(0.0); + }) .withName("Indexer Stop"); } diff --git a/src/main/java/org/team2342/frc/subsystems/kicker/Kicker.java b/src/main/java/org/team2342/frc/subsystems/kicker/Kicker.java new file mode 100644 index 0000000..a7cbf4d --- /dev/null +++ b/src/main/java/org/team2342/frc/subsystems/kicker/Kicker.java @@ -0,0 +1,51 @@ +package org.team2342.frc.subsystems.kicker; + +import org.littletonrobotics.junction.Logger; +import org.team2342.frc.Constants.KickerConstants; +import org.team2342.lib.logging.ExecutionLogger; +import org.team2342.lib.motors.dumb.DumbMotorIO; +import org.team2342.lib.motors.dumb.DumbMotorIOInputsAutoLogged; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Kicker extends SubsystemBase{ + private final DumbMotorIO kickerMotor; + + private final DumbMotorIOInputsAutoLogged kickerMotorInputs = new DumbMotorIOInputsAutoLogged(); + private final Alert kickerMotorAlert = + new Alert("Indexer Feeder Motor is diconnected", AlertType.kError); + + public Kicker(DumbMotorIO kickerMotor) { + this.kickerMotor = kickerMotor; + setName("Shooter/Kicker"); + setDefaultCommand(run(() -> kickerMotor.runVoltage(0.0))); + } + + @Override + public void periodic() { + kickerMotor.updateInputs(kickerMotorInputs); + + Logger.processInputs("Shooter/Kicker", kickerMotorInputs); + + kickerMotorAlert.set(!kickerMotorInputs.connected); + + ExecutionLogger.log("Shooter/Kicker"); + } + + public Command in() { + return run(() -> kickerMotor.runVoltage(KickerConstants.RUN_VOLTAGE)).withName("Kicker Motor Run"); + } + public Command out() { + return run(() -> kickerMotor.runVoltage(-KickerConstants.RUN_VOLTAGE)).withName("Kicker Motor Run"); + } + public Command stop() { + return runOnce( + () -> { + kickerMotor.runVoltage(0.0); + }) + .withName("Kicker Stop"); + } +}