From faff80fb9a77eaef94c41280e882b69f2fa8d310 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Fri, 6 Feb 2026 21:07:38 -0500 Subject: [PATCH] Adding a few subsystems, not sure I like the use of periodic for the shooter and intake pivot subsystems --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../robot/constants/IntakePivotConstants.java | 69 ++++++++++++++++ .../constants/IntakeRollerConstants.java | 34 ++++++++ .../robot/constants/SpindexerConstants.java | 32 ++++++++ .../frc/robot/subsystems/IntakePivot.java | 82 +++++++++++++++++++ .../frc/robot/subsystems/IntakeRoller.java | 51 ++++++++++++ .../java/frc/robot/subsystems/Spindexer.java | 42 ++++++++++ .../java/frc/robot/utilities/Utilities.java | 24 ++---- 8 files changed, 323 insertions(+), 19 deletions(-) create mode 100644 src/main/java/frc/robot/constants/IntakePivotConstants.java create mode 100644 src/main/java/frc/robot/constants/IntakeRollerConstants.java create mode 100644 src/main/java/frc/robot/constants/SpindexerConstants.java create mode 100644 src/main/java/frc/robot/subsystems/IntakePivot.java create mode 100644 src/main/java/frc/robot/subsystems/IntakeRoller.java create mode 100644 src/main/java/frc/robot/subsystems/Spindexer.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 94aa618..cc7c988 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,28 +94,28 @@ public class RobotContainer { new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> { SmartDashboard.putStringArray( OIConstants.kCurrentActiveHub, - Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay + Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay ); })); new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> { SmartDashboard.putStringArray( OIConstants.kCurrentActiveHub, - Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay + Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay ); })); new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> { SmartDashboard.putStringArray( OIConstants.kCurrentActiveHub, - Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay + Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay ); })); new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> { SmartDashboard.putStringArray( OIConstants.kCurrentActiveHub, - Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay + Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay ); })); diff --git a/src/main/java/frc/robot/constants/IntakePivotConstants.java b/src/main/java/frc/robot/constants/IntakePivotConstants.java new file mode 100644 index 0000000..dd32568 --- /dev/null +++ b/src/main/java/frc/robot/constants/IntakePivotConstants.java @@ -0,0 +1,69 @@ +package frc.robot.constants; + +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +public class IntakePivotConstants { + // TODO Real values + public enum IntakePivotPosition { + kUp(0), + kDown(0); + private double positionRadians; + + private IntakePivotPosition(double positionRadians) { + this.positionRadians = positionRadians; + } + + public double getPositionRadians() { + return positionRadians; + } + } + + public static final int kLeftMotorCANID = 0; + public static final int kRightMotorCANID = 1; + + public static final double kConversionFactor = 0; + + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0; + public static final double kV = 0; + public static final double kA = 0; + + public static final boolean kInvertMotors = false; + + public static final int kCurrentLimit = 30; + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig(); + + static { + leftMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kInvertMotors); + leftMotorConfig.absoluteEncoder + .positionConversionFactor(kConversionFactor) + .velocityConversionFactor(kConversionFactor / 60); + leftMotorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .pid(kP, kI, kD) + .outputRange(-1, 1) + .positionWrappingEnabled(true) + .positionWrappingInputRange(0, 2 * Math.PI) + .feedForward + .sva(kS, kV, kA); + + rightMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kInvertMotors) + .follow(kLeftMotorCANID); + } +} diff --git a/src/main/java/frc/robot/constants/IntakeRollerConstants.java b/src/main/java/frc/robot/constants/IntakeRollerConstants.java new file mode 100644 index 0000000..f92de77 --- /dev/null +++ b/src/main/java/frc/robot/constants/IntakeRollerConstants.java @@ -0,0 +1,34 @@ +package frc.robot.constants; + +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +public class IntakeRollerConstants { + // TODO Real values + public static final int kLeftMotorCANID = 0; + public static final int kRightMotorCANID = 0; + + public static final int kCurrentLimit = 30; + + public static final boolean kInvertMotors = false; + + public static final IdleMode kIdleMode = IdleMode.kCoast; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig(); + + static { + leftMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kInvertMotors); + + rightMotorConfig + .idleMode(kIdleMode) + .smartCurrentLimit(kCurrentLimit) + .inverted(kInvertMotors) + .follow(kLeftMotorCANID); + } +} diff --git a/src/main/java/frc/robot/constants/SpindexerConstants.java b/src/main/java/frc/robot/constants/SpindexerConstants.java new file mode 100644 index 0000000..22b1341 --- /dev/null +++ b/src/main/java/frc/robot/constants/SpindexerConstants.java @@ -0,0 +1,32 @@ +package frc.robot.constants; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class SpindexerConstants { + // TODO Real values + public static final int kMotorCANID = 0; + + public static final int kStatorCurrentLimit = 80; + public static final int kSupplyCurrentLimit = 30; + + public static final InvertedValue kInversionState = InvertedValue.Clockwise_Positive; + public static final NeutralModeValue kIdleMode = NeutralModeValue.Brake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final CurrentLimitsConfigs kCurrentLimitConfig = new CurrentLimitsConfigs(); + public static final MotorOutputConfigs kMotorConfig = new MotorOutputConfigs(); + + static { + kCurrentLimitConfig.StatorCurrentLimitEnable = true; + kCurrentLimitConfig.SupplyCurrentLimitEnable = true; + kCurrentLimitConfig.StatorCurrentLimit = kStatorCurrentLimit; + kCurrentLimitConfig.SupplyCurrentLimit = kSupplyCurrentLimit; + + kMotorConfig.Inverted = kInversionState; + kMotorConfig.NeutralMode = kIdleMode; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java new file mode 100644 index 0000000..5cd27ab --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -0,0 +1,82 @@ +package frc.robot.subsystems; + +import java.util.Optional; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.IntakePivotConstants; +import frc.robot.constants.IntakePivotConstants.IntakePivotPosition; + +public class IntakePivot extends SubsystemBase { + private SparkMax leftMotor; + private SparkMax rightMotor; + + private AbsoluteEncoder encoder; + + private SparkClosedLoopController controller; + + private IntakePivotPosition currentTargetPosition; + + public IntakePivot() { + leftMotor = new SparkMax(IntakePivotConstants.kLeftMotorCANID, MotorType.kBrushless); + rightMotor = new SparkMax(IntakePivotConstants.kRightMotorCANID, MotorType.kBrushless); + + leftMotor.configure( + IntakePivotConstants.leftMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + rightMotor.configure( + IntakePivotConstants.rightMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + controller = leftMotor.getClosedLoopController(); + + encoder = leftMotor.getAbsoluteEncoder(); + } + + @Override + public void periodic() { + if(currentTargetPosition == null) { + leftMotor.disable(); + rightMotor.disable(); + } else { + controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition); + } + + Logger.recordOutput( + "IntakePivot/TargetPosition", + currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians()); + Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition()); + Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint()); + } + + public Command setTargetPosition(IntakePivotPosition position) { + return runOnce(() -> { + currentTargetPosition = position; + }); + } + + public Command stop() { + return runOnce(() -> { + currentTargetPosition = null; + }); + } + + public Optional getCurrentTargetPosition() { + return Optional.of(currentTargetPosition); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeRoller.java b/src/main/java/frc/robot/subsystems/IntakeRoller.java new file mode 100644 index 0000000..287f5b0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeRoller.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.IntakeRollerConstants; + +public class IntakeRoller extends SubsystemBase { + private SparkMax leftMotor; + private SparkMax rightMotor; + + public IntakeRoller() { + leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless); + rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless); + + leftMotor.configure( + IntakeRollerConstants.leftMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + rightMotor.configure( + IntakeRollerConstants.rightMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + } + + public Command runIn() { + return run(() -> { + leftMotor.set(1); + }); + } + + public Command runOut() { + return run(() -> { + leftMotor.set(-1); + }); + } + + public Command stop() { + return run(() -> { + leftMotor.set(0); + }); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java new file mode 100644 index 0000000..71c9af3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.SpindexerConstants; + +public class Spindexer extends SubsystemBase { + private TalonFX motor; + + private DutyCycleOut motorOutput; + + public Spindexer() { + motor = new TalonFX(SpindexerConstants.kMotorCANID); + + motor.getConfigurator().apply(SpindexerConstants.kCurrentLimitConfig); + motor.getConfigurator().apply(SpindexerConstants.kMotorConfig); + + motorOutput = new DutyCycleOut(0); + } + + public Command spinToShooter() { + return run(() -> { + motor.setControl(motorOutput.withOutput(1)); + }); + } + + public Command spinToIntake() { + return run(() -> { + motor.setControl(motorOutput.withOutput(-1)); + }); + } + + public Command stop() { + return run(() -> { + motor.setControl(motorOutput.withOutput(0)); + }); + } + +} diff --git a/src/main/java/frc/robot/utilities/Utilities.java b/src/main/java/frc/robot/utilities/Utilities.java index ecbc8fd..fbaeca5 100644 --- a/src/main/java/frc/robot/utilities/Utilities.java +++ b/src/main/java/frc/robot/utilities/Utilities.java @@ -4,28 +4,22 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; public class Utilities { - static String gameData; - - static Boolean Blue = false; - - static Boolean Red = false; - public static final double kG = -9.81; - public static Alliance ShiftFirst() { - gameData = DriverStation.getGameSpecificMessage(); - + public static Alliance whoHasFirstShift() { + String gameData = DriverStation.getGameSpecificMessage(); if(gameData.length() > 0) { switch (gameData.charAt(0)) { - case 'B' : - return Alliance.Red; - case 'R' : - return Alliance.Blue; - default : - return null; + case 'B': + return Alliance.Red; + case 'R': + return Alliance.Blue; + default: + return null; } } + return null; }