From fb235d43295ad99de098b89a2017f899e93b811a Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Thu, 22 Jan 2026 18:08:26 -0800 Subject: [PATCH 1/7] Created bumpAlign method --- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 220fa62..af916be 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -593,6 +593,18 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { yVel); } + public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { + return driveWithHeadingSnap( + () -> { + Translation2d robotHubVec = + FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation()); + // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk) + return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())); + }, + xVel, + yVel); + } + public boolean isInAutoAimTolerance(Pose2d target) { return isInTolerance( target, AutoAlign.TRANSLATION_TOLERANCE_METERS, AutoAlign.ROTATION_TOLERANCE_RADIANS); From 1e9c81216fd8afa58580cce8185a1ec26b1f1b52 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 25 Jan 2026 13:38:11 -0800 Subject: [PATCH 2/7] updating spindexer subsystem --- .../indexer/SpindexerSubsystem.java | 171 +++++++++++++++--- 1 file changed, 144 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 4346925..cb06d3c 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -1,61 +1,178 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems.indexer; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; 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 edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.*; +import frc.robot.components.canrange.CANrangeIOInputsAutoLogged; +import frc.robot.components.canrange.CANrangeIOReal; +import frc.robot.components.rollers.RollerIO; +import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import org.littletonrobotics.junction.Logger; -/** Spindexer = SPINning Indexer. !! COMP !! */ +/** Lindexer = Linear Indexer. !! ALPHA !! */ public class SpindexerSubsystem extends SubsystemBase implements Indexer { - /** Creates a new SpindexerSubsystem. */ - public SpindexerSubsystem() {} - @Override - public void periodic() { - // This method will be called once per scheduler run + public static final double GEAR_RATIO = 2.0; + private CANrangeIOReal firstCANRangeIO; + private CANrangeIOReal secondCANRangeIO; + + private RollerIO indexRollerIO; + + CANrangeIOInputsAutoLogged firstCANRangeInputs = new CANrangeIOInputsAutoLogged(); + CANrangeIOInputsAutoLogged secondCANRangeInputs = new CANrangeIOInputsAutoLogged(); + + RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); + + RollerIO kickerIO; + RollerIOInputsAutoLogged kickerInputs = new RollerIOInputsAutoLogged(); + + private SysIdRoutine indexRollerSysid = + new SysIdRoutine( + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state.toString())), + new Mechanism((volts) -> indexRollerIO.setRollerVoltage(volts.in(Volts)), null, this)); + + public static final double MAX_ACCELERATION = 10.0; + public static final double MAX_VELOCITY = 10.0; + public static final double KICKER_GEAR_RATIO = 2.0; + + public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { + this.kickerIO = kickerIO; + firstCANRangeIO = new CANrangeIOReal(0, canbus, 10); + secondCANRangeIO = new CANrangeIOReal(1, canbus, 10); + this.indexRollerIO = indexRollerIO; } @Override public boolean isFull() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isFull'"); + return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } @Override public boolean isEmpty() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isEmpty'"); + return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; } @Override public boolean isPartiallyFull() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isPartiallyFull'"); + return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } @Override public Command index() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'index'"); + return this.run( + () -> { + indexRollerIO.setRollerVoltage(7); + kickerIO.setRollerVoltage(7); + }); } @Override - public Command spit() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'spit'"); + public Command kick() { + return this.run( + () -> { + indexRollerIO.setRollerVoltage(12); + kickerIO.setRollerVoltage(-7); + }); } @Override - public Command kick() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'kick'"); + public Command spit() { + return this.run( + () -> { + indexRollerIO.setRollerVoltage(-5); + kickerIO.setRollerVoltage(-5); + }); } @Override public Command rest() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'rest'"); + return this.run( + () -> { + indexRollerIO.setRollerVoltage(0.0); + kickerIO.setRollerVoltage(0.0); + }); + } + + public static TalonFXConfiguration getIndexerConfigs() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.25; + + return config; + } + + public static TalonFXConfiguration getKickerConfigs() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + // Converts angular motion to linear motion + config.Feedback.SensorToMechanismRatio = KICKER_GEAR_RATIO; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.25; + + return config; + } + + @Override + public void periodic() { + firstCANRangeIO.updateInputs(firstCANRangeInputs); + Logger.processInputs("Indexer/First Beambreak", firstCANRangeInputs); + secondCANRangeIO.updateInputs(secondCANRangeInputs); + Logger.processInputs("Indexer/Second Beambreak", secondCANRangeInputs); + indexRollerIO.updateInputs(rollerInputs); + Logger.processInputs("Indexer/Roller", rollerInputs); + kickerIO.updateInputs(kickerInputs); + Logger.processInputs("Indexer/Kicker", kickerInputs); + } + + public Command runRollerSysId() { + return Commands.sequence( + indexRollerSysid.quasistatic(Direction.kForward), + indexRollerSysid.quasistatic(Direction.kReverse), + indexRollerSysid.dynamic(Direction.kForward), + indexRollerSysid.dynamic(Direction.kReverse)); } -} +} \ No newline at end of file From e1c593b396f1e005114bdc9241922839f90f7611 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 25 Jan 2026 13:45:45 -0800 Subject: [PATCH 3/7] Creating spindexer subsystem in robot.java --- src/main/java/frc/robot/Robot.java | 29 +++++++++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..f6fe996 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -219,7 +219,7 @@ public Robot() { MotorType.KrakenX44, canivore), (ROBOT_MODE == RobotMode.REAL) - ? new RollerIO(10, LindexerSubsystem.getIndexerConfigs(), canivore) + ? new RollerIO(10, LindexerSubsystem.getKickerConfigs(), canivore) : new RollerIOSim( 10, LindexerSubsystem.getKickerConfigs(), @@ -257,7 +257,32 @@ public Robot() { // note that the climber is not instantiated here break; case COMP: - indexer = new SpindexerSubsystem(); + indexer = new SpindexerSubsystem( + canivore, + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(9, SpindexerSubsystem.getIndexerConfigs(), canivore) + : new RollerIOSim( + 9, + SpindexerSubsystem.getIndexerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.003, SpindexerSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore), + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(10, SpindexerSubsystem.getKickerConfigs(), canivore) + : new RollerIOSim( + 10, + SpindexerSubsystem.getKickerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), + 0.00001, + SpindexerSubsystem.KICKER_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore)); intake = new LintakeSubsystem(); shooter = new TurretSubsystem(); climber = new ClimberSubsystem(); // TODO climber From 1184c165ebc5cff29bd1c8b231f9b823efea3cda Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 25 Jan 2026 15:04:48 -0800 Subject: [PATCH 4/7] Removing isFull method and updating canrange methods for spindexer. --- .../java/frc/robot/subsystems/indexer/Indexer.java | 4 +--- .../robot/subsystems/indexer/LindexerSubsystem.java | 9 ++------- .../robot/subsystems/indexer/SpindexerSubsystem.java | 10 +++------- 3 files changed, 6 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 6bd3118..0b35d8a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -9,11 +9,9 @@ /** Add your docs here. */ public interface Indexer { - public boolean isFull(); - public boolean isEmpty(); - public boolean isPartiallyFull(); + public boolean isNotEmpty(); /** Run indexer towards shooter and kicker away from shooter */ public Command index(); diff --git a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index 44cceff..876e9c0 100644 --- a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -54,19 +54,14 @@ public LindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerI this.indexRollerIO = indexRollerIO; } - @Override - public boolean isFull() { - return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; - } - @Override public boolean isEmpty() { return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; } @Override - public boolean isPartiallyFull() { - return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; + public boolean isNotEmpty() { + return secondCANRangeInputs.isDetected; } @Override diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index cb06d3c..387524c 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -54,19 +54,15 @@ public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kicker this.indexRollerIO = indexRollerIO; } - @Override - public boolean isFull() { - return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; - } @Override public boolean isEmpty() { - return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; + return !secondCANRangeInputs.isDetected; } @Override - public boolean isPartiallyFull() { - return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; + public boolean isNotEmpty() { + return secondCANRangeInputs.isDetected; } @Override From 35b1397029eeccf6af4d4310bc234f2f6eea1f42 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Thu, 29 Jan 2026 17:00:23 -0800 Subject: [PATCH 5/7] Removing second CANRange from Spindexer Subsystem --- .../indexer/SpindexerSubsystem.java | 21 +++++++------------ 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 387524c..bb2f6b7 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -17,17 +17,15 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; -/** Lindexer = Linear Indexer. !! ALPHA !! */ +/** Spindexer = Spinning Indexer. !! COMP !! */ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double GEAR_RATIO = 2.0; - private CANrangeIOReal firstCANRangeIO; - private CANrangeIOReal secondCANRangeIO; + private CANrangeIOReal CANRangeIO; private RollerIO indexRollerIO; - CANrangeIOInputsAutoLogged firstCANRangeInputs = new CANrangeIOInputsAutoLogged(); - CANrangeIOInputsAutoLogged secondCANRangeInputs = new CANrangeIOInputsAutoLogged(); + CANrangeIOInputsAutoLogged CANRangeInputs = new CANrangeIOInputsAutoLogged(); RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); @@ -49,20 +47,19 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { this.kickerIO = kickerIO; - firstCANRangeIO = new CANrangeIOReal(0, canbus, 10); - secondCANRangeIO = new CANrangeIOReal(1, canbus, 10); + CANRangeIO = new CANrangeIOReal(0, canbus, 10); this.indexRollerIO = indexRollerIO; } @Override public boolean isEmpty() { - return !secondCANRangeInputs.isDetected; + return !CANRangeInputs.isDetected; } @Override public boolean isNotEmpty() { - return secondCANRangeInputs.isDetected; + return CANRangeInputs.isDetected; } @Override @@ -154,10 +151,8 @@ public static TalonFXConfiguration getKickerConfigs() { @Override public void periodic() { - firstCANRangeIO.updateInputs(firstCANRangeInputs); - Logger.processInputs("Indexer/First Beambreak", firstCANRangeInputs); - secondCANRangeIO.updateInputs(secondCANRangeInputs); - Logger.processInputs("Indexer/Second Beambreak", secondCANRangeInputs); + CANRangeIO.updateInputs(CANRangeInputs); + Logger.processInputs("Indexer/First Beambreak", CANRangeInputs); indexRollerIO.updateInputs(rollerInputs); Logger.processInputs("Indexer/Roller", rollerInputs); kickerIO.updateInputs(kickerInputs); From d9162a394cbbd3ae260532f8bb1ec6f039a96b71 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Thu, 29 Jan 2026 17:24:35 -0800 Subject: [PATCH 6/7] Removing unused isFull method --- src/main/java/frc/robot/Superstructure.java | 5 ----- .../frc/robot/subsystems/indexer/SpindexerSubsystem.java | 2 +- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 0ce8617..a4fcf50 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -80,9 +80,6 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/Anti Jam Req") private Trigger antiJamReq; - @AutoLogOutput(key = "Superstructure/Is Full") - private Trigger isFull; - @AutoLogOutput(key = "Superstructure/Is Empty") private Trigger isEmpty; @@ -133,8 +130,6 @@ private void addTriggers() { antiJamReq = driver.a().or(operator.a()); - isFull = new Trigger(indexer::isFull).debounce(0.5); // TODO tune - isEmpty = new Trigger(indexer::isEmpty); } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index bb2f6b7..efe5c32 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -47,7 +47,7 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { this.kickerIO = kickerIO; - CANRangeIO = new CANrangeIOReal(0, canbus, 10); + CANRangeIO = new CANrangeIOReal(1, canbus, 10); this.indexRollerIO = indexRollerIO; } From 406a145c9b9b7122d3d4255b16d47a6d8ea3c617 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 30 Jan 2026 12:46:05 -0800 Subject: [PATCH 7/7] fmt --- src/main/java/frc/robot/Robot.java | 3 ++- .../java/frc/robot/subsystems/indexer/SpindexerSubsystem.java | 3 +-- src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f6fe996..ba5e31e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -257,7 +257,8 @@ public Robot() { // note that the climber is not instantiated here break; case COMP: - indexer = new SpindexerSubsystem( + indexer = + new SpindexerSubsystem( canivore, (ROBOT_MODE == RobotMode.REAL) ? new RollerIO(9, SpindexerSubsystem.getIndexerConfigs(), canivore) diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index efe5c32..3ae7192 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -51,7 +51,6 @@ public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kicker this.indexRollerIO = indexRollerIO; } - @Override public boolean isEmpty() { return !CANRangeInputs.isDetected; @@ -166,4 +165,4 @@ public Command runRollerSysId() { indexRollerSysid.dynamic(Direction.kForward), indexRollerSysid.dynamic(Direction.kReverse)); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index aa7230b..e546a3c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -605,7 +605,7 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { yVel); } - public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { + public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { return driveWithHeadingSnap( () -> { Translation2d robotHubVec =