From 7ee8cf445ee0a4c47c721e6f3680ce9f71742b39 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 29 Jan 2026 16:31:36 -0800 Subject: [PATCH 01/35] 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/35] 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/35] 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/35] 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/35] 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/35] 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/35] 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/35] 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/35] 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/35] 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/35] 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/35] 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 0d29155555fdfe473fafcd9e16bb9cc66a077359 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 15:38:06 -0800 Subject: [PATCH 13/35] Turret works hood sort of --- src/main/java/frc/robot/Robot.java | 38 +++++++++++++++++++ .../subsystems/shooter/TurretSubsystem.java | 10 +++++ 2 files changed, 48 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dbedcf8..464e539 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,7 +8,12 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -49,6 +54,7 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.LoggedTunableNumber; import java.util.Optional; import java.util.Set; import org.ironmaple.simulation.SimulatedArena; @@ -558,6 +564,9 @@ private void addAutos() { // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + private LoggedTunableNumber turretAngle = new LoggedTunableNumber("Turret Angle Rads", 0); + private LoggedTunableNumber hoodAngle = new LoggedTunableNumber("Hood angle rads", 0); + @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); @@ -565,6 +574,35 @@ public void robotPeriodic() { superstructure.periodic(); // TODO Log mechanism poses + Pose3d turretPose = + new Pose3d( + new Translation3d(-0.177413, -0.111702, 0.350341), + new Rotation3d(0, 0, turretAngle.getAsDouble())); + Logger.recordOutput( + "Hood pivot point", + new Pose3d(new Translation3d(-0.095638, 0, 0.095123), new Rotation3d())); + Logger.recordOutput( + "Robot/Mechanism Poses", + new Pose3d[] { + // Turret + turretPose, + // Hood + new Pose3d(turretPose.getTranslation(), Rotation3d.kZero) + .rotateAround( + turretPose.getTranslation().plus(new Translation3d(-0.095638, 0, 0.095123)), + new Rotation3d(0, hoodAngle.getAsDouble(), 0)) + .transformBy(new Transform3d(Translation3d.kZero, turretPose.getRotation())) + }); + Logger.recordOutput( + "Robot/Zeroed Mechanism Poses", + new Pose3d[] { + // Turret + new Pose3d(), + // Hood + new Pose3d() + }); + + Logger.recordOutput("Robot/Zero Position", new Pose2d()); updateAlerts(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index eedea8c..2cd6c1f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -173,4 +173,14 @@ public boolean isFacingTarget() { // return MathUtil.isNear( // target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees // } + + // @Override + // public double getHoodAngleRads() { + // return hoodInputs.hoodPositionRotations; + // } + + // @Override + // public double getTurretAngleRads() { + // return 0; + // } } From d85fcc685ab469933b1c14394eb34a02d62f8939 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 16:04:25 -0800 Subject: [PATCH 14/35] Comments --- src/main/java/frc/robot/Robot.java | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 464e539..625771e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -578,20 +578,23 @@ public void robotPeriodic() { new Pose3d( new Translation3d(-0.177413, -0.111702, 0.350341), new Rotation3d(0, 0, turretAngle.getAsDouble())); - Logger.recordOutput( - "Hood pivot point", - new Pose3d(new Translation3d(-0.095638, 0, 0.095123), new Rotation3d())); + Logger.recordOutput( "Robot/Mechanism Poses", new Pose3d[] { // Turret turretPose, // Hood - new Pose3d(turretPose.getTranslation(), Rotation3d.kZero) - .rotateAround( - turretPose.getTranslation().plus(new Translation3d(-0.095638, 0, 0.095123)), - new Rotation3d(0, hoodAngle.getAsDouble(), 0)) - .transformBy(new Transform3d(Translation3d.kZero, turretPose.getRotation())) + turretPose + // First transform the hood out to the hood pivot, and rotate by the amount needed + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123), + new Rotation3d(0, hoodAngle.getAsDouble(), 0))) + // Then, transform the hood back to the correct location relative to the turret + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)) }); Logger.recordOutput( "Robot/Zeroed Mechanism Poses", From 9580ae840e9d344c698696f751c0a4814ffe2b6b Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 16:14:39 -0800 Subject: [PATCH 15/35] Intake viz --- src/main/java/frc/robot/Robot.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 625771e..2a61849 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -566,6 +566,7 @@ private void addAutos() { private LoggedTunableNumber turretAngle = new LoggedTunableNumber("Turret Angle Rads", 0); private LoggedTunableNumber hoodAngle = new LoggedTunableNumber("Hood angle rads", 0); + private LoggedTunableNumber intakeExtension = new LoggedTunableNumber("Intake extension", 0); @Override public void robotPeriodic() { @@ -578,7 +579,9 @@ public void robotPeriodic() { new Pose3d( new Translation3d(-0.177413, -0.111702, 0.350341), new Rotation3d(0, 0, turretAngle.getAsDouble())); - + + Rotation2d intakeRotation = Rotation2d.fromDegrees(17.329856); + Logger.recordOutput( "Robot/Mechanism Poses", new Pose3d[] { @@ -594,7 +597,13 @@ public void robotPeriodic() { // Then, transform the hood back to the correct location relative to the turret .transformBy( new Transform3d( - new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)) + new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), + // Intake + new Pose3d( + intakeExtension.get() * intakeRotation.getCos(), + 0, + -(intakeExtension.get() * intakeRotation.getSin()), + Rotation3d.kZero) }); Logger.recordOutput( "Robot/Zeroed Mechanism Poses", @@ -602,6 +611,7 @@ public void robotPeriodic() { // Turret new Pose3d(), // Hood + new Pose3d(), new Pose3d() }); From 5fc55a53adf3588eb4f21b08f68dfc927a040bb0 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 16:22:22 -0800 Subject: [PATCH 16/35] Climber viz --- src/main/java/frc/robot/Robot.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2a61849..7c226f2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -567,6 +567,7 @@ private void addAutos() { private LoggedTunableNumber turretAngle = new LoggedTunableNumber("Turret Angle Rads", 0); private LoggedTunableNumber hoodAngle = new LoggedTunableNumber("Hood angle rads", 0); private LoggedTunableNumber intakeExtension = new LoggedTunableNumber("Intake extension", 0); + private LoggedTunableNumber climberExtension = new LoggedTunableNumber("Climber extension", 0); @Override public void robotPeriodic() { @@ -603,7 +604,9 @@ public void robotPeriodic() { intakeExtension.get() * intakeRotation.getCos(), 0, -(intakeExtension.get() * intakeRotation.getSin()), - Rotation3d.kZero) + Rotation3d.kZero), + // Climber + new Pose3d(0, 0, climberExtension.getAsDouble(), Rotation3d.kZero) }); Logger.recordOutput( "Robot/Zeroed Mechanism Poses", @@ -612,6 +615,7 @@ public void robotPeriodic() { new Pose3d(), // Hood new Pose3d(), + new Pose3d(), new Pose3d() }); From ed03d74730ac83e83bea2617c39178463c16e547 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 16:23:45 -0800 Subject: [PATCH 17/35] Cleanup and comments --- src/main/java/frc/robot/Robot.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7c226f2..440cf99 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -575,14 +575,16 @@ public void robotPeriodic() { superstructure.periodic(); - // TODO Log mechanism poses + // TODO: YAW VALUE FROM HARDWARE Pose3d turretPose = new Pose3d( new Translation3d(-0.177413, -0.111702, 0.350341), new Rotation3d(0, 0, turretAngle.getAsDouble())); + // From CAD Rotation2d intakeRotation = Rotation2d.fromDegrees(17.329856); + // TODO: USE MEASURED EXTENSIONS AND ANGLES Logger.recordOutput( "Robot/Mechanism Poses", new Pose3d[] { From 9c29ea49efb467418acc2f1ff142273d187cef45 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 17:02:17 -0800 Subject: [PATCH 18/35] Make constants constants --- src/main/java/frc/robot/Robot.java | 7 ++----- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 4 ++++ 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 440cf99..1b92fbd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -581,9 +581,6 @@ public void robotPeriodic() { new Translation3d(-0.177413, -0.111702, 0.350341), new Rotation3d(0, 0, turretAngle.getAsDouble())); - // From CAD - Rotation2d intakeRotation = Rotation2d.fromDegrees(17.329856); - // TODO: USE MEASURED EXTENSIONS AND ANGLES Logger.recordOutput( "Robot/Mechanism Poses", @@ -603,9 +600,9 @@ public void robotPeriodic() { new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), // Intake new Pose3d( - intakeExtension.get() * intakeRotation.getCos(), + intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getCos(), 0, - -(intakeExtension.get() * intakeRotation.getSin()), + -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), Rotation3d.kZero), // Climber new Pose3d(0, 0, climberExtension.getAsDouble(), Rotation3d.kZero) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 425f6b0..4f1a664 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -4,11 +4,15 @@ package frc.robot.subsystems.intake; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** Lintake = Linear Intake. !! COMP !! */ public class LintakeSubsystem extends SubsystemBase implements Intake { + // From CAD + public static final Rotation2d INTAKE_ROTATION = Rotation2d.fromDegrees(17.329856); + /** Creates a new LintakeSubsystem. */ public LintakeSubsystem() {} From 6d1e3f13f097aacebefc756a0a3c64020e605575 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 7 Feb 2026 17:07:57 -0800 Subject: [PATCH 19/35] Make zeroed mech and robot poses sim only --- src/main/java/frc/robot/Robot.java | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1b92fbd..aeaba29 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -607,18 +607,6 @@ public void robotPeriodic() { // Climber new Pose3d(0, 0, climberExtension.getAsDouble(), Rotation3d.kZero) }); - Logger.recordOutput( - "Robot/Zeroed Mechanism Poses", - new Pose3d[] { - // Turret - new Pose3d(), - // Hood - new Pose3d(), - new Pose3d(), - new Pose3d() - }); - - Logger.recordOutput("Robot/Zero Position", new Pose2d()); updateAlerts(); } @@ -682,7 +670,21 @@ public void simulationInit() { } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + // Log zeroed poses for mechs and robot for debugging in sim + Logger.recordOutput( + "Robot/Zeroed Mechanism Poses", + new Pose3d[] { + // Turret + new Pose3d(), + // Hood + new Pose3d(), + new Pose3d(), + new Pose3d() + }); + + Logger.recordOutput("Robot/Zero Position", new Pose2d()); + } @Override public void disabledInit() {} From 65759a39641b518e7729f249ce55982b4d3f83a7 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 13:31:11 -0800 Subject: [PATCH 20/35] 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 21/35] 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 22/35] 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 6b3861d0fd0c7a0dddcba3d7ff93fba0c4d54ce6 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 14:00:54 -0800 Subject: [PATCH 23/35] Climber position from hardware --- src/main/java/frc/robot/Robot.java | 5 ++--- .../frc/robot/subsystems/climber/ClimberSubsystem.java | 7 +++++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 502da1c..e1d6bc8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -573,7 +573,6 @@ private void addAutos() { private LoggedTunableNumber turretAngle = new LoggedTunableNumber("Turret Angle Rads", 0); private LoggedTunableNumber hoodAngle = new LoggedTunableNumber("Hood angle rads", 0); private LoggedTunableNumber intakeExtension = new LoggedTunableNumber("Intake extension", 0); - private LoggedTunableNumber climberExtension = new LoggedTunableNumber("Climber extension", 0); @Override public void robotPeriodic() { @@ -599,7 +598,7 @@ public void robotPeriodic() { .transformBy( new Transform3d( new Translation3d(-0.095638, 0, 0.095123), - new Rotation3d(0, hoodAngle.getAsDouble(), 0))) + new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0))) // Then, transform the hood back to the correct location relative to the turret .transformBy( new Transform3d( @@ -611,7 +610,7 @@ public void robotPeriodic() { -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), Rotation3d.kZero), // Climber - new Pose3d(0, 0, climberExtension.getAsDouble(), Rotation3d.kZero) + new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) }); updateAlerts(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 57c1c46..6247d68 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -8,6 +9,7 @@ public class ClimberSubsystem extends SubsystemBase { // todo: find actual constants public static double GEAR_RATIO = (45.0 / 1.0); public static double MAX_EXTENSION_METERS = 0.2413; + public static double SPOOL_DIAMETER_METERS = Units.inchesToMeters(0.668898); public static double MAX_ACCELERATION = 10.0; public static double MAX_VELOCITY = 2.0; @@ -39,4 +41,9 @@ public Command retractClimber() { climberIO.setClimberPosition(0.0); }); } + + public double getClimberExtensionMeters() { + // Convert rotations into linear motion + return climberInputs.motorPositionRotations.getRotations() * (Math.PI * SPOOL_DIAMETER_METERS); + } } From c5a9a4b8deb9c7fe2b61aede64907fdf874226ac Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 15:27:03 -0800 Subject: [PATCH 24/35] Make sure all climber stuff uses correct units --- src/main/java/frc/robot/Robot.java | 31 ++++++++++++++++++ .../robot/subsystems/climber/ClimberIO.java | 32 +++++++++++-------- .../subsystems/climber/ClimberSubsystem.java | 6 +++- 3 files changed, 55 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e1d6bc8..20ba9c5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -613,6 +613,37 @@ public void robotPeriodic() { new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) }); + // TODO: ACTUAL SETPOINT + Pose3d turretSetpoint = new Pose3d( + new Translation3d(-0.177413, -0.111702, 0.350341), + new Rotation3d(0, 0, turretAngle.getAsDouble())); + // TODO: ACTUAL SETPOINTS + Logger.recordOutput( + "Robot/Mechanism Setpoints", + new Pose3d[] { + // Turret + turretSetpoint, + // Hood + turretSetpoint + // First transform the hood out to the hood pivot, and rotate by the amount needed + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123), + new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0))) + // Then, transform the hood back to the correct location relative to the turret + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), + // Intake + new Pose3d( + intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + 0, + -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + Rotation3d.kZero), + // Climber + new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) + }); + updateAlerts(); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 88b1b93..bc85932 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -17,12 +17,13 @@ import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; public class ClimberIO { @AutoLog public static class ClimberIOInputs { - public Rotation2d motorPositionRotations = new Rotation2d(); + public double motorPositionMeters = 0.0; public double motorVelocityMetersPerSec = 0.0; public double motorStatorCurrentAmps = 0.0; public double motorSupplyCurrentAmps = 0.0; @@ -32,8 +33,9 @@ public static class ClimberIOInputs { protected final TalonFX climberMotor; - private final StatusSignal motorPositionRotations; - private final StatusSignal angularVelocityRotsPerSec; + // Rotation -> linear conversion happens in sensor to mech ratio + private final StatusSignal motorPositionMeters; + private final StatusSignal velocityMetersPerSec; private final StatusSignal statorCurrentAmps; private final StatusSignal supplyCurrentAmps; private final StatusSignal motorVoltage; @@ -50,21 +52,21 @@ public ClimberIO(CANBus canBus) { climberMotor = new TalonFX(30, canBus); climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration()); - angularVelocityRotsPerSec = climberMotor.getVelocity(); + velocityMetersPerSec = climberMotor.getVelocity(); supplyCurrentAmps = climberMotor.getSupplyCurrent(); motorVoltage = climberMotor.getMotorVoltage(); statorCurrentAmps = climberMotor.getStatorCurrent(); motorTemp = climberMotor.getDeviceTemp(); - motorPositionRotations = climberMotor.getPosition(); + motorPositionMeters = climberMotor.getPosition(); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, - angularVelocityRotsPerSec, + velocityMetersPerSec, supplyCurrentAmps, motorVoltage, statorCurrentAmps, motorTemp, - motorPositionRotations); + motorPositionMeters); climberMotor.optimizeBusUtilization(); } @@ -76,7 +78,7 @@ public static TalonFXConfiguration getClimberConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // todo: find and make climber gear ratio variable - config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS); // todo: tune config.Slot0.kS = 0.0; @@ -108,19 +110,23 @@ public void setClimberVelocity(double climberVelocity) { public void updateInputs(ClimberIOInputs inputs) { BaseStatusSignal.refreshAll( - motorPositionRotations, - angularVelocityRotsPerSec, + motorPositionMeters, + velocityMetersPerSec, statorCurrentAmps, supplyCurrentAmps, motorVoltage, motorTemp); - inputs.motorPositionRotations = - Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorPositionMeters = motorPositionMeters.getValueAsDouble(); inputs.motorVoltage = motorVoltage.getValueAsDouble(); inputs.motorTempC = motorTemp.getValueAsDouble(); inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble(); - inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble(); + inputs.motorVelocityMetersPerSec = velocityMetersPerSec.getValueAsDouble(); + } + + @AutoLogOutput(key = "Climber/Setpoint Meters") + public double getClimberSetpointMeters() { + return climberSetpoint; } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 6247d68..43910a2 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -44,6 +44,10 @@ public Command retractClimber() { public double getClimberExtensionMeters() { // Convert rotations into linear motion - return climberInputs.motorPositionRotations.getRotations() * (Math.PI * SPOOL_DIAMETER_METERS); + return climberInputs.motorPositionMeters; + } + + public double getClimberSetpointMeters() { + return climberIO.getClimberSetpointMeters(); } } From 761a7ea343c9f786958eddaa278a18e92b0b71be Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 15:27:28 -0800 Subject: [PATCH 25/35] Log climber setpoint --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 20ba9c5..685e85d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -641,7 +641,7 @@ public void robotPeriodic() { -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), Rotation3d.kZero), // Climber - new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) + new Pose3d(0, 0, climber.getClimberSetpointMeters(), Rotation3d.kZero) }); updateAlerts(); From 3921e2a4dfb962db022d5f49d6368f8a95f3a808 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 22:45:52 -0800 Subject: [PATCH 26/35] 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 27/35] 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 28/35] 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 29/35] 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 30/35] 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 31/35] 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 7e2485d9d768da90f91df2c46d2b325cbd163d85 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 10 Feb 2026 11:00:49 -0800 Subject: [PATCH 32/35] Add back intake rotation lol --- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index e3ecfe1..b9971b2 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -28,6 +29,8 @@ 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; + // From CAD + public static final Rotation2d INTAKE_ROTATION = Rotation2d.fromDegrees(17.329856); public static final double CURRENT_ZEROING_THRESHOLD = 30; // TODO: TUNE private final LinearRackIO rackIO; From 26557ecc9a14cb4cb2de36836bac1bd82b31609b Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 10 Feb 2026 17:31:48 -0800 Subject: [PATCH 33/35] Fix direction --- .../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 b9971b2..c888f72 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -23,8 +23,8 @@ /** 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); + // I'm calling zero fully retracted and + 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); From e7dca4b2483d46292ab1aa8ad5d21eb69f156c7a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 10 Feb 2026 17:36:45 -0800 Subject: [PATCH 34/35] Add lintake viz --- src/main/java/frc/robot/Robot.java | 11 ++++++----- .../frc/robot/subsystems/intake/FintakeSubsystem.java | 10 ++++++++++ src/main/java/frc/robot/subsystems/intake/Intake.java | 4 ++++ .../frc/robot/subsystems/intake/LintakeSubsystem.java | 10 ++++++++++ 4 files changed, 30 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 90fe215..957e4ab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -180,6 +180,8 @@ public enum RobotEdition { private final LEDSubsystem leds; private final ClimberSubsystem climber; + private Intake intake = null; + // climber only exists for the comp bot - this is accounted for later private final Superstructure superstructure; @@ -225,7 +227,6 @@ public Robot() { // break // granted this would never actually happen but Indexer indexer = null; - Intake intake = null; Shooter shooter = null; // this looks at the ROBOT_EDITION variable and decides which version of each subsystem to @@ -628,9 +629,9 @@ public void robotPeriodic() { new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), // Intake new Pose3d( - intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + intake.getExtensionMeters() * LintakeSubsystem.INTAKE_ROTATION.getCos(), 0, - -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + -(intake.getExtensionMeters() * LintakeSubsystem.INTAKE_ROTATION.getSin()), Rotation3d.kZero), // Climber new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) @@ -659,9 +660,9 @@ public void robotPeriodic() { new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), // Intake new Pose3d( - intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + intake.getExtensionSetpointMeters() * LintakeSubsystem.INTAKE_ROTATION.getCos(), 0, - -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + -(intake.getExtensionSetpointMeters() * LintakeSubsystem.INTAKE_ROTATION.getSin()), Rotation3d.kZero), // Climber new Pose3d(0, 0, climber.getClimberSetpointMeters(), Rotation3d.kZero) diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index 3d3256c..626a734 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -98,4 +98,14 @@ public static TalonFXConfiguration getIntakeConfig() { public boolean beambreak() { return canrangeInputs.isDetected; } + + @Override + public double getExtensionMeters() { + return 0; + } + + @Override + public double getExtensionSetpointMeters() { + return 0; + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e193211..a65a19f 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -19,4 +19,8 @@ public interface Intake { /** for controller rumble */ public boolean beambreak(); + + public double getExtensionMeters(); + + public double getExtensionSetpointMeters(); } diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index c888f72..bbbfe6b 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -162,4 +162,14 @@ public static TalonFXConfiguration getRollerMotorConfig() { public boolean beambreak() { return canRangeIOInputs.isDetected; } + + @Override + public double getExtensionMeters() { + return rackIOInputs.positionMeters; + } + + @Override + public double getExtensionSetpointMeters() { + return rackIO.getSetpointMeters(); + } } From f93f1181b6aa1e43744cc26eb64b439241a912b6 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 10 Feb 2026 17:37:01 -0800 Subject: [PATCH 35/35] Spotless --- src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/robot/subsystems/climber/ClimberIO.java | 4 ++-- .../java/frc/robot/subsystems/intake/FintakeSubsystem.java | 4 ++-- .../java/frc/robot/subsystems/intake/LintakeSubsystem.java | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 957e4ab..2ff5e31 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -638,7 +638,8 @@ public void robotPeriodic() { }); // TODO: ACTUAL SETPOINT - Pose3d turretSetpoint = new Pose3d( + Pose3d turretSetpoint = + new Pose3d( new Translation3d(-0.177413, -0.111702, 0.350341), new Rotation3d(0, 0, turretAngle.getAsDouble())); // TODO: ACTUAL SETPOINTS diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index bc85932..9e579b1 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -10,7 +10,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -78,7 +77,8 @@ public static TalonFXConfiguration getClimberConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // todo: find and make climber gear ratio variable - config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS); + config.Feedback.SensorToMechanismRatio = + ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS); // todo: tune config.Slot0.kS = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index 626a734..2f7195f 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -101,11 +101,11 @@ public boolean beambreak() { @Override public double getExtensionMeters() { - return 0; + return 0; } @Override public double getExtensionSetpointMeters() { - return 0; + return 0; } } diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index bbbfe6b..2490af8 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -170,6 +170,6 @@ public double getExtensionMeters() { @Override public double getExtensionSetpointMeters() { - return rackIO.getSetpointMeters(); + return rackIO.getSetpointMeters(); } }