From 7ee8cf445ee0a4c47c721e6f3680ce9f71742b39 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 16:31:36 -0800 Subject: [PATCH 01/24] Linear Slide IO --- .../subsystems/intake/LinearSlideIO.java | 90 +++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/intake/LinearSlideIO.java diff --git a/src/main/java/frc/robot/subsystems/intake/LinearSlideIO.java b/src/main/java/frc/robot/subsystems/intake/LinearSlideIO.java new file mode 100644 index 0000000..68f3e1d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/LinearSlideIO.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.AutoLog; + +public class LinearSlideIO { + + @AutoLog + public static class LinearSlideIOInputs { + public double positionMeters = 0.0; + public double velocityMetersPerSecond = 0.0; + public double voltage = 0.0; + public double statorCurrentAmps = 0.0; + public double supplyCurrentAmps = 0.0; + public double temperatureC = 0.0; + } + + protected final TalonFX motor; + + // Technically, these measure angle, but the conversion from angle to linear movement happens in + // the sensor-to-mechanism ratio + private final StatusSignal positionMeters; + private final StatusSignal velocityMetersPerSecond; + private final StatusSignal voltage; + private final StatusSignal statorCurrent; + private final StatusSignal supplyCurrent; + private final StatusSignal temperature; + + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + // I think we might want to motion profile this so i'm using motion magic + private MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true); + + private double setpointMeters = 0.0; + + public LinearSlideIO(int motorID, CANBus canBus, TalonFXConfiguration config) { + this.motor = new TalonFX(motorID, canBus); + + positionMeters = motor.getPosition(); + velocityMetersPerSecond = motor.getVelocity(); + voltage = motor.getMotorVoltage(); + statorCurrent = motor.getStatorCurrent(); + supplyCurrent = motor.getSupplyCurrent(); + temperature = motor.getDeviceTemp(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + positionMeters, + velocityMetersPerSecond, + voltage, + statorCurrent, + supplyCurrent, + temperature); + motor.optimizeBusUtilization(); + + motor.getConfigurator().apply(config); + } + + public void updateInputs(LinearSlideIOInputs inputs) { + inputs.positionMeters = positionMeters.getValueAsDouble(); + inputs.velocityMetersPerSecond = velocityMetersPerSecond.getValueAsDouble(); + inputs.voltage = voltage.getValueAsDouble(); + inputs.statorCurrentAmps = statorCurrent.getValueAsDouble(); + inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); + inputs.temperatureC = temperature.getValueAsDouble(); + } + + public void setVoltage(double volts) { + motor.setControl(voltageOut.withOutput(volts)); + } + + public void setPositionSetpoint(double setpointMeters) { + this.setpointMeters = setpointMeters; + motor.setControl(motionMagicVoltage.withPosition(setpointMeters)); + } + + public double getSetpointMeters() { + return setpointMeters; + } +} From 90c641a7e2968ff36f5811846751dd7f73684d2b Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 16:51:45 -0800 Subject: [PATCH 02/24] Linear slide io sim --- .../subsystems/intake/LinearSlideIOSim.java | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java diff --git a/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java b/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java new file mode 100644 index 0000000..7564b39 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; + +public class LinearSlideIOSim extends LinearSlideIO { + // TODO: SHOULD THIS BE AN ELEVATOR? + ElevatorSim physicsSim = new ElevatorSim(null, null, getSetpointMeters(), getSetpointMeters(), false, getSetpointMeters()); + + private static final double SIM_LOOP_PERIOD = 0.002; // 2 ms + private Notifier notifier; + private TalonFXSimState talonSim; + private double lastLoopTime = 0.0; + + public LinearSlideIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) { + super(motorId, canBus, config); + + this.talonSim = motor.getSimState(); + // Maybe try to make have these passed in? Maybe not needed tho + this.talonSim.setMotorType(MotorType.KrakenX44); + this.talonSim.Orientation = ChassisReference.Clockwise_Positive; // TODO + + notifier = new Notifier(() -> { + double deltaTime = (Utils.getCurrentTimeSeconds() - lastLoopTime); + lastLoopTime = Utils.getCurrentTimeSeconds(); + + talonSim.setSupplyVoltage(RobotController.getBatteryVoltage()); + + physicsSim.setInputVoltage(talonSim.getMotorVoltage()); + physicsSim.update(deltaTime); + + talonSim.setRawRotorPosition(physicsSim.getPositionMeters() * 1.0); // TODO: GEAR RATIO + talonSim.setRotorVelocity(physicsSim.getVelocityMetersPerSecond() * 1.0); // TODO: GEAR RATIO + }); + + notifier.startPeriodic(SIM_LOOP_PERIOD); + } +} From 13223319c22214c62ebf618a803e595224a74a0c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 16:59:54 -0800 Subject: [PATCH 03/24] Base impl of lintake subsystem --- .../subsystems/intake/LintakeSubsystem.java | 40 +++++++++++++++---- 1 file changed, 32 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index a400db1..dbe4e0a 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -4,34 +4,58 @@ package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.components.rollers.RollerIO; +import frc.robot.components.rollers.RollerIOInputsAutoLogged; /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { + private final LinearSlideIO rackIO; + private LinearSlideIOInputsAutoLogged rackIOInputs = new LinearSlideIOInputsAutoLogged(); + + private final RollerIO rollerIO; + private RollerIOInputsAutoLogged rollerIOInputs = new RollerIOInputsAutoLogged(); + /** Creates a new LintakeSubsystem. */ - public LintakeSubsystem() {} + public LintakeSubsystem(LinearSlideIO rackIO, RollerIO rollerIO) { + this.rackIO = rackIO; + this.rollerIO = rollerIO; + } @Override public void periodic() { - // This method will be called once per scheduler run + rackIO.updateInputs(rackIOInputs); + Logger.processInputs("Intake/Rack", rackIOInputs); + + rollerIO.updateInputs(rollerIOInputs); + Logger.processInputs("Intake/Rollers", rollerIOInputs); } @Override public Command intake() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'intake'"); + return this.run(() -> { + rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rollerIO.setRollerVoltage(10.0); + }); } @Override public Command outtake() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'outtake'"); + return this.run(() -> { + rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rollerIO.setRollerVoltage(10.0); + }); } @Override public Command rest() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'rest'"); + return this.run(() -> { + rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rollerIO.setRollerVoltage(0.0); + }); } } From 28aad470734ec7201b8dc18316b43b2e59951409 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:01:12 -0800 Subject: [PATCH 04/24] Spotless --- .../subsystems/intake/LinearSlideIOSim.java | 69 ++++++++++--------- .../subsystems/intake/LintakeSubsystem.java | 31 +++++---- 2 files changed, 53 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java b/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java index 7564b39..94b0ef6 100644 --- a/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java @@ -6,41 +6,46 @@ import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.ElevatorSim; public class LinearSlideIOSim extends LinearSlideIO { - // TODO: SHOULD THIS BE AN ELEVATOR? - ElevatorSim physicsSim = new ElevatorSim(null, null, getSetpointMeters(), getSetpointMeters(), false, getSetpointMeters()); - - private static final double SIM_LOOP_PERIOD = 0.002; // 2 ms - private Notifier notifier; - private TalonFXSimState talonSim; - private double lastLoopTime = 0.0; - - public LinearSlideIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) { - super(motorId, canBus, config); - - this.talonSim = motor.getSimState(); - // Maybe try to make have these passed in? Maybe not needed tho - this.talonSim.setMotorType(MotorType.KrakenX44); - this.talonSim.Orientation = ChassisReference.Clockwise_Positive; // TODO - - notifier = new Notifier(() -> { - double deltaTime = (Utils.getCurrentTimeSeconds() - lastLoopTime); - lastLoopTime = Utils.getCurrentTimeSeconds(); - - talonSim.setSupplyVoltage(RobotController.getBatteryVoltage()); - - physicsSim.setInputVoltage(talonSim.getMotorVoltage()); - physicsSim.update(deltaTime); - - talonSim.setRawRotorPosition(physicsSim.getPositionMeters() * 1.0); // TODO: GEAR RATIO - talonSim.setRotorVelocity(physicsSim.getVelocityMetersPerSecond() * 1.0); // TODO: GEAR RATIO - }); - - notifier.startPeriodic(SIM_LOOP_PERIOD); - } + // TODO: SHOULD THIS BE AN ELEVATOR? + ElevatorSim physicsSim = + new ElevatorSim( + null, null, getSetpointMeters(), getSetpointMeters(), false, getSetpointMeters()); + + private static final double SIM_LOOP_PERIOD = 0.002; // 2 ms + private Notifier notifier; + private TalonFXSimState talonSim; + private double lastLoopTime = 0.0; + + public LinearSlideIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) { + super(motorId, canBus, config); + + this.talonSim = motor.getSimState(); + // Maybe try to make have these passed in? Maybe not needed tho + this.talonSim.setMotorType(MotorType.KrakenX44); + this.talonSim.Orientation = ChassisReference.Clockwise_Positive; // TODO + + notifier = + new Notifier( + () -> { + double deltaTime = (Utils.getCurrentTimeSeconds() - lastLoopTime); + lastLoopTime = Utils.getCurrentTimeSeconds(); + + talonSim.setSupplyVoltage(RobotController.getBatteryVoltage()); + + physicsSim.setInputVoltage(talonSim.getMotorVoltage()); + physicsSim.update(deltaTime); + + talonSim.setRawRotorPosition( + physicsSim.getPositionMeters() * 1.0); // TODO: GEAR RATIO + talonSim.setRotorVelocity( + physicsSim.getVelocityMetersPerSecond() * 1.0); // TODO: GEAR RATIO + }); + + notifier.startPeriodic(SIM_LOOP_PERIOD); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index dbe4e0a..123eafa 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -4,13 +4,11 @@ package frc.robot.subsystems.intake; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import org.littletonrobotics.junction.Logger; /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { @@ -37,25 +35,28 @@ public void periodic() { @Override public Command intake() { - return this.run(() -> { - rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION - rollerIO.setRollerVoltage(10.0); - }); + return this.run( + () -> { + rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rollerIO.setRollerVoltage(10.0); + }); } @Override public Command outtake() { - return this.run(() -> { - rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION - rollerIO.setRollerVoltage(10.0); - }); + return this.run( + () -> { + rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rollerIO.setRollerVoltage(10.0); + }); } @Override public Command rest() { - return this.run(() -> { - rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION - rollerIO.setRollerVoltage(0.0); - }); + return this.run( + () -> { + rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rollerIO.setRollerVoltage(0.0); + }); } } From fd5840d4db936ee63efc7eaf816c45a9206a26ed Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:29:47 -0800 Subject: [PATCH 05/24] Constants from CAD --- .../robot/subsystems/intake/LintakeSubsystem.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 123eafa..dccc3d4 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -4,6 +4,7 @@ package frc.robot.subsystems.intake; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.components.rollers.RollerIO; @@ -12,6 +13,11 @@ /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { + public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(16.0); + public static final double EXTENDED_POSITION_METERS = MAX_EXTENSION_METERS; + public static final double RACK_GEAR_RATIO = 8.0; + public static final double RACK_PINION_DIAMETER_METERS = Units.inchesToMeters(0.975); + private final LinearSlideIO rackIO; private LinearSlideIOInputsAutoLogged rackIOInputs = new LinearSlideIOInputsAutoLogged(); @@ -37,7 +43,7 @@ public void periodic() { public Command intake() { return this.run( () -> { - rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rackIO.setPositionSetpoint(EXTENDED_POSITION_METERS); rollerIO.setRollerVoltage(10.0); }); } @@ -46,7 +52,7 @@ public Command intake() { public Command outtake() { return this.run( () -> { - rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rackIO.setPositionSetpoint(EXTENDED_POSITION_METERS); rollerIO.setRollerVoltage(10.0); }); } @@ -55,7 +61,7 @@ public Command outtake() { public Command rest() { return this.run( () -> { - rackIO.setPositionSetpoint(0.0); // TODO: EXTENDED POSITION + rackIO.setPositionSetpoint(EXTENDED_POSITION_METERS); rollerIO.setRollerVoltage(0.0); }); } From 9ebf40b0d7d99348584e74cb5f8d00d83eaba206 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:35:38 -0800 Subject: [PATCH 06/24] Rack talon config --- .../subsystems/intake/LintakeSubsystem.java | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index dccc3d4..0fb0363 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -11,6 +11,11 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(16.0); @@ -65,4 +70,33 @@ public Command rest() { rollerIO.setRollerVoltage(0.0); }); } + + public static TalonFXConfiguration getRackMotorConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // TODO + + // Converts rotational motion to linear motion + config.Feedback.SensorToMechanismRatio = RACK_GEAR_RATIO * (Math.PI * RACK_PINION_DIAMETER_METERS); + + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; // Maybe don't need this? + config.Slot0.kG = 0.0; + config.Slot0.kS = 0.0; + config.Slot0.kV = 0.0; + config.Slot0.kP = 0.0; + config.Slot0.kD = 0.0; + + // TODO: TUNE + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + // TODO: TUNE + config.MotionMagic.MotionMagicCruiseVelocity = 10.0; + config.MotionMagic.MotionMagicAcceleration = 30.0; + + return config; + } } From 1cb3361b4958595cb03c1bf6ad087ee495c323ad Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:37:08 -0800 Subject: [PATCH 07/24] Rename slide to rack --- .../{LinearSlideIO.java => LinearRackIO.java} | 8 ++++---- ...arSlideIOSim.java => LinearRackIOSim.java} | 4 ++-- .../subsystems/intake/LintakeSubsystem.java | 20 +++++++++---------- 3 files changed, 16 insertions(+), 16 deletions(-) rename src/main/java/frc/robot/subsystems/intake/{LinearSlideIO.java => LinearRackIO.java} (93%) rename src/main/java/frc/robot/subsystems/intake/{LinearSlideIOSim.java => LinearRackIOSim.java} (92%) diff --git a/src/main/java/frc/robot/subsystems/intake/LinearSlideIO.java b/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java similarity index 93% rename from src/main/java/frc/robot/subsystems/intake/LinearSlideIO.java rename to src/main/java/frc/robot/subsystems/intake/LinearRackIO.java index 68f3e1d..643ffc5 100644 --- a/src/main/java/frc/robot/subsystems/intake/LinearSlideIO.java +++ b/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java @@ -14,10 +14,10 @@ import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; -public class LinearSlideIO { +public class LinearRackIO { @AutoLog - public static class LinearSlideIOInputs { + public static class LinearRackIOInputs { public double positionMeters = 0.0; public double velocityMetersPerSecond = 0.0; public double voltage = 0.0; @@ -43,7 +43,7 @@ public static class LinearSlideIOInputs { private double setpointMeters = 0.0; - public LinearSlideIO(int motorID, CANBus canBus, TalonFXConfiguration config) { + public LinearRackIO(int motorID, CANBus canBus, TalonFXConfiguration config) { this.motor = new TalonFX(motorID, canBus); positionMeters = motor.getPosition(); @@ -66,7 +66,7 @@ public LinearSlideIO(int motorID, CANBus canBus, TalonFXConfiguration config) { motor.getConfigurator().apply(config); } - public void updateInputs(LinearSlideIOInputs inputs) { + public void updateInputs(LinearRackIOInputs inputs) { inputs.positionMeters = positionMeters.getValueAsDouble(); inputs.velocityMetersPerSecond = velocityMetersPerSecond.getValueAsDouble(); inputs.voltage = voltage.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java b/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java similarity index 92% rename from src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java rename to src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java index 94b0ef6..b5d49b6 100644 --- a/src/main/java/frc/robot/subsystems/intake/LinearSlideIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.ElevatorSim; -public class LinearSlideIOSim extends LinearSlideIO { +public class LinearRackIOSim extends LinearRackIO { // TODO: SHOULD THIS BE AN ELEVATOR? ElevatorSim physicsSim = new ElevatorSim( @@ -21,7 +21,7 @@ public class LinearSlideIOSim extends LinearSlideIO { private TalonFXSimState talonSim; private double lastLoopTime = 0.0; - public LinearSlideIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) { + public LinearRackIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) { super(motorId, canBus, config); this.talonSim = motor.getSimState(); diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 0fb0363..610bb88 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -4,6 +4,10 @@ package frc.robot.subsystems.intake; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -11,11 +15,6 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(16.0); @@ -23,14 +22,14 @@ public class LintakeSubsystem extends SubsystemBase implements Intake { public static final double RACK_GEAR_RATIO = 8.0; public static final double RACK_PINION_DIAMETER_METERS = Units.inchesToMeters(0.975); - private final LinearSlideIO rackIO; - private LinearSlideIOInputsAutoLogged rackIOInputs = new LinearSlideIOInputsAutoLogged(); + private final LinearRackIO rackIO; + private LinearRackIOInputsAutoLogged rackIOInputs = new LinearRackIOInputsAutoLogged(); private final RollerIO rollerIO; private RollerIOInputsAutoLogged rollerIOInputs = new RollerIOInputsAutoLogged(); /** Creates a new LintakeSubsystem. */ - public LintakeSubsystem(LinearSlideIO rackIO, RollerIO rollerIO) { + public LintakeSubsystem(LinearRackIO rackIO, RollerIO rollerIO) { this.rackIO = rackIO; this.rollerIO = rollerIO; } @@ -78,7 +77,8 @@ public static TalonFXConfiguration getRackMotorConfig() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // TODO // Converts rotational motion to linear motion - config.Feedback.SensorToMechanismRatio = RACK_GEAR_RATIO * (Math.PI * RACK_PINION_DIAMETER_METERS); + config.Feedback.SensorToMechanismRatio = + RACK_GEAR_RATIO * (Math.PI * RACK_PINION_DIAMETER_METERS); config.Slot0.GravityType = GravityTypeValue.Elevator_Static; // Maybe don't need this? config.Slot0.kG = 0.0; @@ -87,7 +87,7 @@ public static TalonFXConfiguration getRackMotorConfig() { config.Slot0.kP = 0.0; config.Slot0.kD = 0.0; - // TODO: TUNE + // TODO: TUNE config.CurrentLimits.StatorCurrentLimit = 80.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 40.0; From 5b11938f38f707c2c9b5c1137fffd540f56e796f Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:45:39 -0800 Subject: [PATCH 08/24] Linear rack sim constants from CAD --- .../frc/robot/subsystems/intake/LinearRackIOSim.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java b/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java index b5d49b6..9c05d97 100644 --- a/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java @@ -6,6 +6,10 @@ import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.ElevatorSim; @@ -14,7 +18,7 @@ public class LinearRackIOSim extends LinearRackIO { // TODO: SHOULD THIS BE AN ELEVATOR? ElevatorSim physicsSim = new ElevatorSim( - null, null, getSetpointMeters(), getSetpointMeters(), false, getSetpointMeters()); + LinearSystemId.createElevatorSystem(DCMotor.getKrakenX44Foc(1), Units.lbsToKilograms(10.0), LintakeSubsystem.RACK_PINION_DIAMETER_METERS / 2, LintakeSubsystem.RACK_GEAR_RATIO), DCMotor.getKrakenX44Foc(1), 0.0, LintakeSubsystem.MAX_EXTENSION_METERS, false, 0.0); private static final double SIM_LOOP_PERIOD = 0.002; // 2 ms private Notifier notifier; @@ -40,10 +44,11 @@ public LinearRackIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) physicsSim.setInputVoltage(talonSim.getMotorVoltage()); physicsSim.update(deltaTime); + // I think these should be multiplied? talonSim.setRawRotorPosition( - physicsSim.getPositionMeters() * 1.0); // TODO: GEAR RATIO + physicsSim.getPositionMeters() * (LintakeSubsystem.RACK_GEAR_RATIO * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS))); talonSim.setRotorVelocity( - physicsSim.getVelocityMetersPerSecond() * 1.0); // TODO: GEAR RATIO + physicsSim.getVelocityMetersPerSecond() * (LintakeSubsystem.RACK_GEAR_RATIO * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS))); }); notifier.startPeriodic(SIM_LOOP_PERIOD); From 6d653398d2b093a74c6f1135478f1d2650e85653 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:52:52 -0800 Subject: [PATCH 09/24] Roller config --- .../subsystems/intake/LintakeSubsystem.java | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 610bb88..4a8b599 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -21,6 +21,7 @@ public class LintakeSubsystem extends SubsystemBase implements Intake { public static final double EXTENDED_POSITION_METERS = MAX_EXTENSION_METERS; public static final double RACK_GEAR_RATIO = 8.0; public static final double RACK_PINION_DIAMETER_METERS = Units.inchesToMeters(0.975); + public static final double ROLLER_GEAR_RATIO = 34 / 15; private final LinearRackIO rackIO; private LinearRackIOInputsAutoLogged rackIOInputs = new LinearRackIOInputsAutoLogged(); @@ -99,4 +100,27 @@ public static TalonFXConfiguration getRackMotorConfig() { return config; } + + public static TalonFXConfiguration getRollerMotorConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // TODO + + // Converts rotational motion to linear motion + config.Feedback.SensorToMechanismRatio = ROLLER_GEAR_RATIO; + + config.Slot0.kS = 0.0; + config.Slot0.kV = 0.0; + config.Slot0.kP = 0.0; + config.Slot0.kD = 0.0; + + // TODO: TUNE + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + return config; + } } From 35529d85fadf74a93d038c036670d5209a823e39 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:53:35 -0800 Subject: [PATCH 10/24] Spotless --- .../subsystems/intake/LinearRackIOSim.java | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java b/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java index 9c05d97..567b624 100644 --- a/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; @@ -18,7 +17,16 @@ public class LinearRackIOSim extends LinearRackIO { // TODO: SHOULD THIS BE AN ELEVATOR? ElevatorSim physicsSim = new ElevatorSim( - LinearSystemId.createElevatorSystem(DCMotor.getKrakenX44Foc(1), Units.lbsToKilograms(10.0), LintakeSubsystem.RACK_PINION_DIAMETER_METERS / 2, LintakeSubsystem.RACK_GEAR_RATIO), DCMotor.getKrakenX44Foc(1), 0.0, LintakeSubsystem.MAX_EXTENSION_METERS, false, 0.0); + LinearSystemId.createElevatorSystem( + DCMotor.getKrakenX44Foc(1), + Units.lbsToKilograms(10.0), + LintakeSubsystem.RACK_PINION_DIAMETER_METERS / 2, + LintakeSubsystem.RACK_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1), + 0.0, + LintakeSubsystem.MAX_EXTENSION_METERS, + false, + 0.0); private static final double SIM_LOOP_PERIOD = 0.002; // 2 ms private Notifier notifier; @@ -46,9 +54,13 @@ public LinearRackIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) // I think these should be multiplied? talonSim.setRawRotorPosition( - physicsSim.getPositionMeters() * (LintakeSubsystem.RACK_GEAR_RATIO * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS))); + physicsSim.getPositionMeters() + * (LintakeSubsystem.RACK_GEAR_RATIO + * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS))); talonSim.setRotorVelocity( - physicsSim.getVelocityMetersPerSecond() * (LintakeSubsystem.RACK_GEAR_RATIO * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS))); + physicsSim.getVelocityMetersPerSecond() + * (LintakeSubsystem.RACK_GEAR_RATIO + * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS))); }); notifier.startPeriodic(SIM_LOOP_PERIOD); From aca47715faa93a4b4e12d164f402d1507d314615 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 17:57:18 -0800 Subject: [PATCH 11/24] Instantiate new lintake in constructor --- src/main/java/frc/robot/Robot.java | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a94a93f..1c4ee7d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,6 +36,8 @@ import frc.robot.subsystems.indexer.SpindexerSubsystem; import frc.robot.subsystems.intake.FintakeSubsystem; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.LinearRackIO; +import frc.robot.subsystems.intake.LinearRackIOSim; import frc.robot.subsystems.intake.LintakeSubsystem; import frc.robot.subsystems.led.LEDIOReal; import frc.robot.subsystems.led.LEDSubsystem; @@ -276,7 +278,25 @@ public Robot() { break; case COMP: indexer = new SpindexerSubsystem(); - intake = new LintakeSubsystem(); + // TODO: MOTOR IDS + intake = + (ROBOT_MODE == RobotMode.REAL) + ? new LintakeSubsystem( + new LinearRackIO(50, canivore, LintakeSubsystem.getRackMotorConfig()), + new RollerIO(51, LintakeSubsystem.getRollerMotorConfig(), canivore)) + : new LintakeSubsystem( + new LinearRackIOSim(50, canivore, LintakeSubsystem.getRackMotorConfig()), + new RollerIOSim( + 51, + LintakeSubsystem.getRollerMotorConfig(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), + 0.001, + LintakeSubsystem.ROLLER_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore)); shooter = new TurretSubsystem(); climber = new ClimberSubsystem(); // TODO climber break; From 5a00e24b7466c73c8ecea0730708a6743b42682f Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 18:02:32 -0800 Subject: [PATCH 12/24] CAN range --- src/main/java/frc/robot/Robot.java | 9 ++++++--- .../robot/subsystems/intake/LintakeSubsystem.java | 15 ++++++++++++++- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1c4ee7d..b98c856 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.components.canrange.CANrangeIOReal; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; import frc.robot.subsystems.climber.ClimberSubsystem; @@ -278,12 +279,13 @@ public Robot() { break; case COMP: indexer = new SpindexerSubsystem(); - // TODO: MOTOR IDS + // TODO: CAN IDS AND FOVs intake = (ROBOT_MODE == RobotMode.REAL) ? new LintakeSubsystem( new LinearRackIO(50, canivore, LintakeSubsystem.getRackMotorConfig()), - new RollerIO(51, LintakeSubsystem.getRollerMotorConfig(), canivore)) + new RollerIO(51, LintakeSubsystem.getRollerMotorConfig(), canivore), + new CANrangeIOReal(10, canivore, 10)) : new LintakeSubsystem( new LinearRackIOSim(50, canivore, LintakeSubsystem.getRackMotorConfig()), new RollerIOSim( @@ -296,7 +298,8 @@ public Robot() { LintakeSubsystem.ROLLER_GEAR_RATIO), DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44, - canivore)); + canivore), + new CANrangeIOReal(10, canivore, 10)); shooter = new TurretSubsystem(); climber = new ClimberSubsystem(); // TODO climber break; diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 4a8b599..976de20 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -11,6 +11,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.components.canrange.CANrangeIO; +import frc.robot.components.canrange.CANrangeIOInputsAutoLogged; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; @@ -29,10 +31,14 @@ public class LintakeSubsystem extends SubsystemBase implements Intake { private final RollerIO rollerIO; private RollerIOInputsAutoLogged rollerIOInputs = new RollerIOInputsAutoLogged(); + private final CANrangeIO canRangeIO; + private CANrangeIOInputsAutoLogged canRangeIOInputs = new CANrangeIOInputsAutoLogged(); + /** Creates a new LintakeSubsystem. */ - public LintakeSubsystem(LinearRackIO rackIO, RollerIO rollerIO) { + public LintakeSubsystem(LinearRackIO rackIO, RollerIO rollerIO, CANrangeIO canRangeIO) { this.rackIO = rackIO; this.rollerIO = rollerIO; + this.canRangeIO = canRangeIO; } @Override @@ -42,6 +48,9 @@ public void periodic() { rollerIO.updateInputs(rollerIOInputs); Logger.processInputs("Intake/Rollers", rollerIOInputs); + + canRangeIO.updateInputs(canRangeIOInputs); + Logger.processInputs("Intake/CANRange", canRangeIOInputs); } @Override @@ -71,6 +80,10 @@ public Command rest() { }); } + public boolean getCanRangeIsDetected() { + return canRangeIOInputs.isDetected; + } + public static TalonFXConfiguration getRackMotorConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); From 65759a39641b518e7729f249ce55982b4d3f83a7 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 13:31:11 -0800 Subject: [PATCH 13/24] Can ids --- src/main/java/frc/robot/Robot.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7ca8b08..969c521 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -315,17 +315,17 @@ public Robot() { DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44, canivore)); - // TODO: CAN IDS AND FOVs + // TODO: FOVs intake = (ROBOT_MODE == RobotMode.REAL) ? new LintakeSubsystem( - new LinearRackIO(50, canivore, LintakeSubsystem.getRackMotorConfig()), - new RollerIO(51, LintakeSubsystem.getRollerMotorConfig(), canivore), - new CANrangeIOReal(10, canivore, 10)) + new LinearRackIO(14, canivore, LintakeSubsystem.getRackMotorConfig()), + new RollerIO(8, LintakeSubsystem.getRollerMotorConfig(), canivore), + new CANrangeIOReal(0, canivore, 10)) : new LintakeSubsystem( - new LinearRackIOSim(50, canivore, LintakeSubsystem.getRackMotorConfig()), + new LinearRackIOSim(14, canivore, LintakeSubsystem.getRackMotorConfig()), new RollerIOSim( - 51, + 8, LintakeSubsystem.getRollerMotorConfig(), new DCMotorSim( LinearSystemId.createDCMotorSystem( @@ -335,7 +335,7 @@ public Robot() { DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44, canivore), - new CANrangeIOReal(10, canivore, 10)); + new CANrangeIOReal(0, canivore, 10)); shooter = new TurretSubsystem( ROBOT_MODE == RobotMode.REAL From 6652844d71ca33f3cabc3f0c4ea99867449ce788 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 13:32:06 -0800 Subject: [PATCH 14/24] Impl beambreak method --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 546f510..227ada9 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -139,7 +139,6 @@ public static TalonFXConfiguration getRollerMotorConfig() { @Override public boolean beambreak() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'beambreak'"); + return canRangeIOInputs.isDetected; } } From c3bacc88add9a8ea56fb61b6cb808978b96bbe22 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 13:33:52 -0800 Subject: [PATCH 15/24] Remove unused method in intake subsystem --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 227ada9..c25a529 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -80,10 +80,6 @@ public Command rest() { }); } - public boolean getCanRangeIsDetected() { - return canRangeIOInputs.isDetected; - } - public static TalonFXConfiguration getRackMotorConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); From 3921e2a4dfb962db022d5f49d6368f8a95f3a808 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 22:45:52 -0800 Subject: [PATCH 16/24] Osciliate when outtaking --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index c25a529..1e1fe14 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -9,8 +9,10 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.components.canrange.CANrangeIO; import frc.robot.components.canrange.CANrangeIOInputsAutoLogged; import frc.robot.components.rollers.RollerIO; @@ -66,7 +68,7 @@ public Command intake() { public Command outtake() { return this.run( () -> { - rackIO.setPositionSetpoint(EXTENDED_POSITION_METERS); + rackIO.setPositionSetpoint(Math.sin(Timer.getFPGATimestamp()) * EXTENDED_POSITION_METERS); rollerIO.setRollerVoltage(10.0); }); } From f4a5e6753c6366e176ae8aeb1f24e9811bebde29 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 22:49:16 -0800 Subject: [PATCH 17/24] Scale oscilation to be reasonable --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 1e1fe14..fd44948 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -68,7 +68,8 @@ public Command intake() { public Command outtake() { return this.run( () -> { - rackIO.setPositionSetpoint(Math.sin(Timer.getFPGATimestamp()) * EXTENDED_POSITION_METERS); + // Oscillate between 0.5x extension pos and 1x extension pos + rackIO.setPositionSetpoint((0.25 * Math.sin(Timer.getFPGATimestamp()) + 0.75) * EXTENDED_POSITION_METERS); rollerIO.setRollerVoltage(10.0); }); } From a7998029ece37346ce33d44d3f758bf2833ca568 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 22:53:35 -0800 Subject: [PATCH 18/24] Spotless --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index fd44948..8ef60f2 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; import frc.robot.components.canrange.CANrangeIO; import frc.robot.components.canrange.CANrangeIOInputsAutoLogged; import frc.robot.components.rollers.RollerIO; @@ -69,7 +68,8 @@ public Command outtake() { return this.run( () -> { // Oscillate between 0.5x extension pos and 1x extension pos - rackIO.setPositionSetpoint((0.25 * Math.sin(Timer.getFPGATimestamp()) + 0.75) * EXTENDED_POSITION_METERS); + rackIO.setPositionSetpoint( + (0.25 * Math.sin(Timer.getFPGATimestamp()) + 0.75) * EXTENDED_POSITION_METERS); rollerIO.setRollerVoltage(10.0); }); } From 08bfa82cb088536815e08963a53da2f5a1709f71 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 10 Feb 2026 10:52:37 -0800 Subject: [PATCH 19/24] Add current zeroing --- .../robot/subsystems/intake/LinearRackIO.java | 4 ++++ .../subsystems/intake/LintakeSubsystem.java | 19 +++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java b/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java index 643ffc5..6b40c6a 100644 --- a/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java +++ b/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java @@ -87,4 +87,8 @@ public void setPositionSetpoint(double setpointMeters) { public double getSetpointMeters() { return setpointMeters; } + + public void resetEncoder(double positionMeters) { + motor.setPosition(positionMeters); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 8ef60f2..815c794 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -8,9 +8,12 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.components.canrange.CANrangeIO; import frc.robot.components.canrange.CANrangeIOInputsAutoLogged; @@ -25,6 +28,7 @@ public class LintakeSubsystem extends SubsystemBase implements Intake { public static final double RACK_GEAR_RATIO = 8.0; public static final double RACK_PINION_DIAMETER_METERS = Units.inchesToMeters(0.975); public static final double ROLLER_GEAR_RATIO = 34 / 15; + public static final double CURRENT_ZEROING_THRESHOLD = 30; // TODO: TUNE private final LinearRackIO rackIO; private LinearRackIOInputsAutoLogged rackIOInputs = new LinearRackIOInputsAutoLogged(); @@ -35,6 +39,9 @@ public class LintakeSubsystem extends SubsystemBase implements Intake { private final CANrangeIO canRangeIO; private CANrangeIOInputsAutoLogged canRangeIOInputs = new CANrangeIOInputsAutoLogged(); + private LinearFilter rackCurrentFilter = LinearFilter.movingAverage(10); + private double rackCurrentFilterValue = 0.0; + /** Creates a new LintakeSubsystem. */ public LintakeSubsystem(LinearRackIO rackIO, RollerIO rollerIO, CANrangeIO canRangeIO) { this.rackIO = rackIO; @@ -52,6 +59,8 @@ public void periodic() { canRangeIO.updateInputs(canRangeIOInputs); Logger.processInputs("Intake/CANRange", canRangeIOInputs); + + rackCurrentFilterValue = rackCurrentFilter.calculate(rackIOInputs.statorCurrentAmps); } @Override @@ -83,6 +92,16 @@ public Command rest() { }); } + public Command runCurrentZeroing() { + return this.run(() -> rackIO.setVoltage(-3)) + .until(() -> rackCurrentFilterValue > CURRENT_ZEROING_THRESHOLD) + .andThen(Commands.parallel(Commands.print("Intake Zeroed"), zeroRack())); + } + + public Command zeroRack() { + return this.run(() -> rackIO.resetEncoder(0.0)); + } + public static TalonFXConfiguration getRackMotorConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); From c818d0fe7466a3050c59988423fe5e79f3e15bf8 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 10 Feb 2026 10:54:41 -0800 Subject: [PATCH 20/24] Actually zero rack by extending --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 815c794..c7eb205 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -23,7 +23,8 @@ /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { - public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(16.0); + // I'm calling zero fully retracted and 1 fully extended (so that kG works if its needed) + public static final double MAX_EXTENSION_METERS = -Units.inchesToMeters(16.0); public static final double EXTENDED_POSITION_METERS = MAX_EXTENSION_METERS; public static final double RACK_GEAR_RATIO = 8.0; public static final double RACK_PINION_DIAMETER_METERS = Units.inchesToMeters(0.975); @@ -93,13 +94,13 @@ public Command rest() { } public Command runCurrentZeroing() { - return this.run(() -> rackIO.setVoltage(-3)) + return this.run(() -> rackIO.setVoltage(3)) .until(() -> rackCurrentFilterValue > CURRENT_ZEROING_THRESHOLD) .andThen(Commands.parallel(Commands.print("Intake Zeroed"), zeroRack())); } public Command zeroRack() { - return this.run(() -> rackIO.resetEncoder(0.0)); + return this.run(() -> rackIO.resetEncoder(MAX_EXTENSION_METERS)); } public static TalonFXConfiguration getRackMotorConfig() { From f73c8d9126521d6e8283e5fe76ac4bc69b690f3a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 10 Feb 2026 10:55:08 -0800 Subject: [PATCH 21/24] Spotless --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index c7eb205..e3ecfe1 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; @@ -95,8 +94,8 @@ public Command rest() { public Command runCurrentZeroing() { return this.run(() -> rackIO.setVoltage(3)) - .until(() -> rackCurrentFilterValue > CURRENT_ZEROING_THRESHOLD) - .andThen(Commands.parallel(Commands.print("Intake Zeroed"), zeroRack())); + .until(() -> rackCurrentFilterValue > CURRENT_ZEROING_THRESHOLD) + .andThen(Commands.parallel(Commands.print("Intake Zeroed"), zeroRack())); } public Command zeroRack() { From 413024a796ac244364c1e96639878b938aebdf8b Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 11 Feb 2026 07:59:23 -0800 Subject: [PATCH 22/24] Make extension not negative lol --- src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index e3ecfe1..ab30748 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -23,7 +23,7 @@ /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { // I'm calling zero fully retracted and 1 fully extended (so that kG works if its needed) - public static final double MAX_EXTENSION_METERS = -Units.inchesToMeters(16.0); + public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(16.0); public static final double EXTENDED_POSITION_METERS = MAX_EXTENSION_METERS; public static final double RACK_GEAR_RATIO = 8.0; public static final double RACK_PINION_DIAMETER_METERS = Units.inchesToMeters(0.975); From b3247dd27d37dad13aaed342fc67f3c35b096dc4 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 11 Feb 2026 07:59:43 -0800 Subject: [PATCH 23/24] Zeroing is a run once --- src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index ab30748..f91d4b1 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -99,7 +99,7 @@ public Command runCurrentZeroing() { } public Command zeroRack() { - return this.run(() -> rackIO.resetEncoder(MAX_EXTENSION_METERS)); + return this.runOnce(() -> rackIO.resetEncoder(MAX_EXTENSION_METERS)); } public static TalonFXConfiguration getRackMotorConfig() { From 07aba7e05f16bdae17c33deb6b0e939e9fad16db Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 11 Feb 2026 08:00:24 -0800 Subject: [PATCH 24/24] Lower current limit to make it more springy hopefully (needs tuning irl) --- src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index f91d4b1..0eb9a61 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -120,7 +120,7 @@ public static TalonFXConfiguration getRackMotorConfig() { config.Slot0.kD = 0.0; // TODO: TUNE - config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimit = 30.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLimitEnable = true;