From 5a747f328d2fa5a060dc8971cd83035c5f16d47e Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 2 Apr 2026 19:18:20 -0400 Subject: [PATCH 01/29] Started building out the new intake deployer. Finished initial build of IO and IOSim --- src/main/java/frc/robot/Constants.java | 2 + .../robot/subsystems/intake/IntakeArmIO.java | 18 ++- .../subsystems/intake/IntakeArmIOSim.java | 123 ++++++++++++++++-- .../subsystems/intake/IntakeConstants.java | 16 +++ 4 files changed, 142 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7eb374fa..d2c7abec 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,6 +111,8 @@ public static final class CAN2 { // Intake public static final int intakeRollerLower = 22; public static final int intakeRollerUpper = 23; + public static final int intakeArmRight = 24; + public static final int intakeArmLeft = 25; } public static final class CANHD { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index 4171e42c..746d526d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -1,17 +1,27 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; public interface IntakeArmIO { @AutoLog public static class IntakeArmIOInputs { - public DoubleSolenoid.Value isDeployed = DoubleSolenoid.Value.kReverse; + public boolean connected = false; + public double position = 0.0; + public double velocityMetersPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; } public default void updateInputs(IntakeArmIOInputs inputs) {} - public default void deploy() {} + public default void setOpenLoop(Voltage volts) {} - public default void retract() {} + public default void setPosition(double position) {} + + public default void setVelocity(double velocity) {} + + public default void configureSoftLimits(boolean enable) {} + + public default void resetEncoder() {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java index 278ef7f2..c1659376 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java @@ -1,32 +1,129 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; +import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; +import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.motorReduction; +import static edu.wpi.first.units.Units.*; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.simulation.DoubleSolenoidSim; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.Robot; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.NEOConstants; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; public class IntakeArmIOSim implements IntakeArmIO { - public final DoubleSolenoidSim intakeArmPneumatic; + + private final DCMotorSim armSim; + + private final SparkMax maxRight; + private final SparkMax maxLeft; + private final SparkClosedLoopController controller; + private final SparkMaxSim maxSim; + + private final SparkMaxConfig armConfig; + private final SparkMaxConfig followerConfig; public IntakeArmIOSim() { - intakeArmPneumatic = - new DoubleSolenoidSim(PneumaticsModuleType.REVPH, intakeArmForward, intakeArmReverse); - intakeArmPneumatic.set(kReverse); + maxRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); + maxLeft = new SparkMax(CAN2.intakeArmLeft, MotorType.kBrushless); + + controller = maxRight.getClosedLoopController(); + + armConfig = new SparkMaxConfig(); + + armConfig + .inverted(false) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOConstants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + armConfig + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + armConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kPSim, 0.0, kDSim); + + armConfig + .softLimit + .forwardSoftLimit(maxPos) + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(minPos) + .reverseSoftLimitEnabled(true); + + followerConfig = new SparkMaxConfig(); + + followerConfig + .apply(armConfig) + .follow(CAN2.intakeArmRight); + + maxRight.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + maxSim = new SparkMaxSim(maxRight, gearbox); + + armSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + + armSim.setState(0.0, 0.0); + maxSim.setPosition(0.0); + + maxLeft.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @Override public void updateInputs(IntakeArmIOInputs inputs) { - inputs.isDeployed = intakeArmPneumatic.get(); + // Update simulation state + double busVoltage = RoboRioSim.getVInVoltage(); + armSim.setInput(maxSim.getAppliedOutput() * busVoltage); + armSim.update(Robot.defaultPeriodSecs); + + if (maxSim.getPosition() > maxPos) { + armSim.setState(maxPos, 0.0); + maxSim.setPosition(maxPos); + } + + maxSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); + + // Update inputs + inputs.connected = true; + inputs.position = maxSim.getPosition(); + inputs.velocityMetersPerSec = maxSim.getVelocity(); + inputs.appliedVolts = maxSim.getAppliedOutput() * maxSim.getBusVoltage(); + inputs.currentAmps = Math.abs(maxSim.getMotorCurrent()); + } + + @Override + public void setOpenLoop(Voltage volts) { + maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + } + + @Override + public void setPosition(double position) { + controller.setSetpoint(position, ControlType.kPosition); } @Override - public void deploy() { - intakeArmPneumatic.set(kForward); + public void configureSoftLimits(boolean enable) { + armConfig.softLimit.forwardSoftLimitEnabled(enable); + armConfig.softLimit.reverseSoftLimitEnabled(enable); } @Override - public void retract() { - intakeArmPneumatic.set(kReverse); + public void resetEncoder() { + maxSim.setPosition(maxPos); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f8e53da6..7b1e1780 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.KrakenX60Constants; @@ -51,4 +52,19 @@ public RollerConfig(int port, CANBus bus, boolean inverted) { this.inverted = inverted; } } + + public static class ArmConstants { + public static final double encoderPositionFactor = 1.0; + public static final double encoderVelocityFactor = 1.0; + + public static final double kPSim = 1.0; + public static final double kDSim = 1.0; + + public static final double maxPos = 1.0; + public static final double minPos = 1.0; + + public static final double motorReduction = 1.0; + + public static final DCMotor gearbox = DCMotor.getNEO(2); + } } From 442cc9d6484e1fb7a23329e1f1c57470746903a5 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 16 Apr 2026 18:01:17 -0400 Subject: [PATCH 02/29] finished building out the intake arm system, still need to build out commands and fix units as needed --- src/main/java/frc/robot/Robot.java | 3 - .../frc/robot/subsystems/intake/Intake.java | 14 +- .../subsystems/intake/IntakeArmIOReal.java | 138 ++++++++++++++++-- .../subsystems/intake/IntakeArmIOSim.java | 58 ++++---- .../subsystems/intake/IntakeConstants.java | 4 +- 5 files changed, 160 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a1801a16..96cd2228 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.REVPHSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -248,8 +247,6 @@ public Robot() { new RollerIOSimTalonFX(RollerConstants.upperRollerConfig), new RollerIOSimTalonFX(RollerConstants.lowerRollerConfig), intakeArmIOSim); - pneumaticsSimulator = - new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1)); break; case REPLAY: // Replaying a log diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e0393bee..df4a2792 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -77,23 +76,18 @@ public void periodic() { public void stop() { upperRollerIO.setOpenLoop(Volts.of(0.0)); lowerRollerIO.setOpenLoop(Volts.of(0.0)); - intakeArmIO.retract(); } - public void deployArm() { - intakeArmIO.deploy(); - } + public void deployArm() {} - public void retractArm() { - intakeArmIO.retract(); - } + public void retractArm() {} public Boolean isDeployed() { - return intakeArmInputs.isDeployed == DoubleSolenoid.Value.kForward; + return false; } public boolean isStowed() { - return intakeArmInputs.isDeployed == DoubleSolenoid.Value.kReverse; + return false; } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java index 4aeb79c9..2c2f122a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java @@ -1,31 +1,145 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; +import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; +import static frc.robot.util.SparkUtil.*; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.NEOConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.util.SparkOdometryThread; +import frc.robot.util.SparkOdometryThread.SparkInputs; public class IntakeArmIOReal implements IntakeArmIO { - public final DoubleSolenoid intakeArmPneumatic; + private final SparkMax intakeArmRight; + private final SparkMax intakeArmLeft; + private final RelativeEncoder encoderSpark; + private final SparkClosedLoopController intakeArmController; + private final SparkInputs sparkInputs; + + private final SparkMaxConfig rightArmConfig; + private final SparkMaxConfig leftArmConfig; + + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); public IntakeArmIOReal() { - intakeArmPneumatic = - new DoubleSolenoid(PneumaticsModuleType.REVPH, intakeArmForward, intakeArmReverse); + intakeArmRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); + intakeArmLeft = new SparkMax(CAN2.intakeArmLeft, MotorType.kBrushless); + encoderSpark = intakeArmRight.getEncoder(); + intakeArmController = intakeArmRight.getClosedLoopController(); + + rightArmConfig = new SparkMaxConfig(); + + rightArmConfig + .inverted(false) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOConstants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + rightArmConfig + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + rightArmConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kPRealPos, 0.0, 0.0, ClosedLoopSlot.kSlot0) + .pid(kPRealVel, 0.0, 0.0, ClosedLoopSlot.kSlot1); + + rightArmConfig + .softLimit + .forwardSoftLimit(maxPos) + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(minPos) + .reverseSoftLimitEnabled(true); + + leftArmConfig = new SparkMaxConfig(); + + leftArmConfig.apply(rightArmConfig).follow(CAN2.intakeArmRight); + + rightArmConfig + .signals + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + tryUntilOk( + intakeArmRight, + 5, + () -> + intakeArmRight.configure( + rightArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + tryUntilOk( + intakeArmLeft, + 5, + () -> + intakeArmLeft.configure( + leftArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + sparkInputs = SparkOdometryThread.getInstance().registerSpark(intakeArmRight, encoderSpark); } @Override public void updateInputs(IntakeArmIOInputs inputs) { - inputs.isDeployed = intakeArmPneumatic.get(); + inputs.position = sparkInputs.getPosition(); + inputs.velocityMetersPerSec = sparkInputs.getVelocity(); + inputs.appliedVolts = sparkInputs.getAppliedVolts(); + inputs.currentAmps = sparkInputs.getOutputCurrent(); + inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); + } + + @Override + public void setOpenLoop(Voltage volts) { + intakeArmRight.setVoltage(volts); + } + + @Override + public void setPosition(double position) { + double setpoint = MathUtil.clamp(position, minPos, maxPos); + intakeArmController.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0); + } + + @Override + public void setVelocity(double velocity) { + intakeArmController.setSetpoint(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot1); } @Override - public void deploy() { - intakeArmPneumatic.set(kForward); + public void configureSoftLimits(boolean enable) { + rightArmConfig.softLimit.forwardSoftLimitEnabled(enable); + rightArmConfig.softLimit.reverseSoftLimitEnabled(enable); + tryUntilOk( + intakeArmRight, + 5, + () -> + intakeArmRight.configure( + rightArmConfig, + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters)); + tryUntilOk( + intakeArmLeft, + 5, + () -> + intakeArmLeft.configure( + leftArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); } @Override - public void retract() { - intakeArmPneumatic.set(kReverse); + public void resetEncoder() { + encoderSpark.setPosition(maxPos); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java index c1659376..36040395 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java @@ -1,31 +1,30 @@ package frc.robot.subsystems.intake; +import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.motorReduction; -import static edu.wpi.first.units.Units.*; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.sim.SparkMaxSim; import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import frc.robot.Robot; -import frc.robot.Constants.RobotConstants; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEOConstants; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.NEOConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; public class IntakeArmIOSim implements IntakeArmIO { - + private final DCMotorSim armSim; private final SparkMax maxRight; @@ -45,43 +44,40 @@ public IntakeArmIOSim() { armConfig = new SparkMaxConfig(); armConfig - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .inverted(false) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOConstants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); armConfig - .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); - armConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kPSim, 0.0, kDSim); + armConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, kDSim); armConfig - .softLimit - .forwardSoftLimit(maxPos) - .forwardSoftLimitEnabled(true) - .reverseSoftLimit(minPos) - .reverseSoftLimitEnabled(true); + .softLimit + .forwardSoftLimit(maxPos) + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(minPos) + .reverseSoftLimitEnabled(true); followerConfig = new SparkMaxConfig(); - followerConfig - .apply(armConfig) - .follow(CAN2.intakeArmRight); + followerConfig.apply(armConfig).follow(CAN2.intakeArmRight); maxRight.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); maxSim = new SparkMaxSim(maxRight, gearbox); - armSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + armSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); armSim.setState(0.0, 0.0); maxSim.setPosition(0.0); - maxLeft.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + maxLeft.configure( + followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 7b1e1780..8a9393af 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.KrakenX60Constants; @@ -60,6 +59,9 @@ public static class ArmConstants { public static final double kPSim = 1.0; public static final double kDSim = 1.0; + public static final double kPRealPos = 1.0; + public static final double kPRealVel = 1.0; + public static final double maxPos = 1.0; public static final double minPos = 1.0; From 60ae31cdc7316fab9be8375d084d040145bb39e9 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 21 Apr 2026 18:55:41 -0400 Subject: [PATCH 03/29] fixed units, changed constants, hopefully put everything together right --- src/main/java/frc/robot/Constants.java | 4 +-- src/main/java/frc/robot/Robot.java | 3 +- .../frc/robot/subsystems/intake/Intake.java | 33 +++++++++++++++++-- .../robot/subsystems/intake/IntakeArmIO.java | 6 ++-- .../subsystems/intake/IntakeArmIOReal.java | 18 ++++++---- .../subsystems/intake/IntakeArmIOSim.java | 17 +++++----- .../subsystems/intake/IntakeConstants.java | 15 +++++---- 7 files changed, 68 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d2c7abec..57062fc7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,8 +111,8 @@ public static final class CAN2 { // Intake public static final int intakeRollerLower = 22; public static final int intakeRollerUpper = 23; - public static final int intakeArmRight = 24; - public static final int intakeArmLeft = 25; + public static final int intakeArmRight = 27; + public static final int intakeArmLeft = 26; } public static final class CANHD { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 96cd2228..91138cdf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -325,7 +325,8 @@ public Robot() { Field.plotRegions(); feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop")); - intake.setDefaultCommand(intake.getDefaultCommand()); + intake.setDefaultCommand( + intake.initializeIntakeArmCommand().andThen(intake.getDefaultCommand())); launcher.setDefaultCommand( launcher .initializeHoodCommand() diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index df4a2792..bac8a83c 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,11 +2,13 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import java.util.function.BooleanSupplier; @@ -24,6 +26,7 @@ public class Intake extends SubsystemBase { private final Alert upperRollerDisconnectedAlert; private final Alert lowerRollerDisconnectedAlert; + private final Alert intakeArmDisconnectedAlert; // Injected after both subsystems are created to avoid a circular dependency. // When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed. @@ -37,6 +40,7 @@ public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intake upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError); lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError); + intakeArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); } @Override @@ -54,8 +58,10 @@ public void periodic() { upperRollerDisconnectedAlert.set(!upperRollerInputs.connected); lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected); + intakeArmDisconnectedAlert.set(!intakeArmInputs.connected); Logger.recordOutput("Faults/Intake/UpperRollerDisconnected", !upperRollerInputs.connected); Logger.recordOutput("Faults/Intake/LowerRollerDisconnected", !lowerRollerInputs.connected); + Logger.recordOutput("Faults/Intake/IntakeArmDisconnected", !intakeArmInputs.connected); // Profiling output if (Constants.FeatureFlags.PROFILING_ENABLED) { @@ -76,11 +82,16 @@ public void periodic() { public void stop() { upperRollerIO.setOpenLoop(Volts.of(0.0)); lowerRollerIO.setOpenLoop(Volts.of(0.0)); + intakeArmIO.setPosition(minPos); } - public void deployArm() {} + public void deployArm() { + intakeArmIO.setPosition(maxPos); + } - public void retractArm() {} + public void retractArm() { + intakeArmIO.setPosition(minPos); + } public Boolean isDeployed() { return false; @@ -163,4 +174,22 @@ public Command getShakeIntakeCommand() { Commands.runOnce(this::deployArm, this)) .repeatedly(); } + + public Command initializeIntakeArmCommand() { + return new StartEndCommand( + // initialize + () -> { + intakeArmIO.configureSoftLimits(false); + intakeArmIO.setOpenLoop(Volts.of(1.0)); + }, + // end + () -> { + intakeArmIO.configureSoftLimits(true); + intakeArmIO.resetEncoder(); + }, + // requirements + this) + .withTimeout(1.0) + .withName("Initialize intake arm"); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index 746d526d..2ec4efb4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.intake; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; @@ -17,9 +19,9 @@ public default void updateInputs(IntakeArmIOInputs inputs) {} public default void setOpenLoop(Voltage volts) {} - public default void setPosition(double position) {} + public default void setPosition(Angle rotation) {} - public default void setVelocity(double velocity) {} + public default void setVelocity(AngularVelocity velocity) {} public default void configureSoftLimits(boolean enable) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java index 2c2f122a..dd535430 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.intake; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; import static frc.robot.util.SparkUtil.*; @@ -16,6 +17,8 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.NEOConstants; @@ -62,9 +65,9 @@ public IntakeArmIOReal() { rightArmConfig .softLimit - .forwardSoftLimit(maxPos) + .forwardSoftLimit(maxPosRad) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(minPos) + .reverseSoftLimit(minPosRad) .reverseSoftLimitEnabled(true); leftArmConfig = new SparkMaxConfig(); @@ -108,14 +111,15 @@ public void setOpenLoop(Voltage volts) { } @Override - public void setPosition(double position) { - double setpoint = MathUtil.clamp(position, minPos, maxPos); + public void setPosition(Angle rotation) { + double setpoint = MathUtil.clamp(rotation.magnitude(), minPosRad, maxPosRad); intakeArmController.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0); } @Override - public void setVelocity(double velocity) { - intakeArmController.setSetpoint(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot1); + public void setVelocity(AngularVelocity velocity) { + intakeArmController.setSetpoint( + velocity.in(RadiansPerSecond), ControlType.kVelocity, ClosedLoopSlot.kSlot1); } @Override @@ -140,6 +144,6 @@ public void configureSoftLimits(boolean enable) { @Override public void resetEncoder() { - encoderSpark.setPosition(maxPos); + encoderSpark.setPosition(minPosRad); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java index 36040395..d1138c4b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java @@ -15,6 +15,7 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -58,9 +59,9 @@ public IntakeArmIOSim() { armConfig .softLimit - .forwardSoftLimit(maxPos) + .forwardSoftLimit(maxPosRad) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(minPos) + .reverseSoftLimit(minPosRad) .reverseSoftLimitEnabled(true); followerConfig = new SparkMaxConfig(); @@ -87,9 +88,9 @@ public void updateInputs(IntakeArmIOInputs inputs) { armSim.setInput(maxSim.getAppliedOutput() * busVoltage); armSim.update(Robot.defaultPeriodSecs); - if (maxSim.getPosition() > maxPos) { - armSim.setState(maxPos, 0.0); - maxSim.setPosition(maxPos); + if (maxSim.getPosition() > maxPosRad) { + armSim.setState(maxPosRad, 0.0); + maxSim.setPosition(maxPosRad); } maxSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); @@ -108,8 +109,8 @@ public void setOpenLoop(Voltage volts) { } @Override - public void setPosition(double position) { - controller.setSetpoint(position, ControlType.kPosition); + public void setPosition(Angle rotation) { + controller.setSetpoint(rotation.magnitude(), ControlType.kPosition); } @Override @@ -120,6 +121,6 @@ public void configureSoftLimits(boolean enable) { @Override public void resetEncoder() { - maxSim.setPosition(maxPos); + maxSim.setPosition(maxPosRad); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 8a9393af..8b34352d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import frc.robot.Constants.CANBusPorts.CAN2; @@ -53,8 +54,10 @@ public RollerConfig(int port, CANBus bus, boolean inverted) { } public static class ArmConstants { - public static final double encoderPositionFactor = 1.0; - public static final double encoderVelocityFactor = 1.0; + public static final double motorReduction = 15.0; + + public static final double encoderPositionFactor = 2 * Math.PI / motorReduction; + public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * motorReduction); public static final double kPSim = 1.0; public static final double kDSim = 1.0; @@ -62,10 +65,10 @@ public static class ArmConstants { public static final double kPRealPos = 1.0; public static final double kPRealVel = 1.0; - public static final double maxPos = 1.0; - public static final double minPos = 1.0; - - public static final double motorReduction = 1.0; + public static final Angle maxPos = Degrees.of(90.0); + public static final Angle minPos = Degrees.of(0.0); + public static final double maxPosRad = maxPos.in(Radians); + public static final double minPosRad = minPos.in(Radians); public static final DCMotor gearbox = DCMotor.getNEO(2); } From 2dfdfb520211f9ff55b2c79be4548c415bf5de35 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 21 Apr 2026 19:14:00 -0400 Subject: [PATCH 04/29] made extra adjustments and renamed some classes --- src/main/java/frc/robot/Robot.java | 8 ++++---- .../frc/robot/subsystems/intake/Intake.java | 7 ++++--- .../robot/subsystems/intake/IntakeArmIO.java | 2 +- ...eArmIOSim.java => IntakeArmIOSimSpark.java} | 18 +++++++++++++----- ...akeArmIOReal.java => IntakeArmIOSpark.java} | 16 ++++++++++++---- .../subsystems/intake/IntakeConstants.java | 8 +++----- 6 files changed, 37 insertions(+), 22 deletions(-) rename src/main/java/frc/robot/subsystems/intake/{IntakeArmIOSim.java => IntakeArmIOSimSpark.java} (86%) rename src/main/java/frc/robot/subsystems/intake/{IntakeArmIOReal.java => IntakeArmIOSpark.java} (90%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 91138cdf..ee0b549c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -66,8 +66,8 @@ import frc.robot.subsystems.hopper.HopperIOSim; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeArmIO; -import frc.robot.subsystems.intake.IntakeArmIOReal; -import frc.robot.subsystems.intake.IntakeArmIOSim; +import frc.robot.subsystems.intake.IntakeArmIOSpark; +import frc.robot.subsystems.intake.IntakeArmIOSimSpark; import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; import frc.robot.subsystems.intake.RollerIO; @@ -199,7 +199,7 @@ public Robot() { new Intake( new RollerIOTalonFX(RollerConstants.upperRollerConfig), new RollerIOTalonFX(RollerConstants.lowerRollerConfig), - new IntakeArmIOReal()); + new IntakeArmIOSpark()); feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor"); @@ -241,7 +241,7 @@ public Robot() { new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOSim()); - var intakeArmIOSim = new IntakeArmIOSim(); + var intakeArmIOSim = new IntakeArmIOSimSpark(); intake = new Intake( new RollerIOSimTalonFX(RollerConstants.upperRollerConfig), diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index bac8a83c..1ffdb388 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.intake; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; @@ -82,15 +83,15 @@ public void periodic() { public void stop() { upperRollerIO.setOpenLoop(Volts.of(0.0)); lowerRollerIO.setOpenLoop(Volts.of(0.0)); - intakeArmIO.setPosition(minPos); + intakeArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); } public void deployArm() { - intakeArmIO.setPosition(maxPos); + intakeArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0)); } public void retractArm() { - intakeArmIO.setPosition(minPos); + intakeArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); } public Boolean isDeployed() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index 2ec4efb4..354b872a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -19,7 +19,7 @@ public default void updateInputs(IntakeArmIOInputs inputs) {} public default void setOpenLoop(Voltage volts) {} - public default void setPosition(Angle rotation) {} + public default void setPosition(Angle rotation, AngularVelocity velocity) {} public default void setVelocity(AngularVelocity velocity) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java similarity index 86% rename from src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java rename to src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index d1138c4b..fa45c2b7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -2,11 +2,11 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; -import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.motorReduction; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; @@ -16,6 +16,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -24,7 +25,9 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class IntakeArmIOSim implements IntakeArmIO { +public class IntakeArmIOSimSpark implements IntakeArmIO { + private final double kPSim = 1.0; + private final double kDSim = 1.0; private final DCMotorSim armSim; @@ -36,7 +39,7 @@ public class IntakeArmIOSim implements IntakeArmIO { private final SparkMaxConfig armConfig; private final SparkMaxConfig followerConfig; - public IntakeArmIOSim() { + public IntakeArmIOSimSpark() { maxRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); maxLeft = new SparkMax(CAN2.intakeArmLeft, MotorType.kBrushless); @@ -109,8 +112,13 @@ public void setOpenLoop(Voltage volts) { } @Override - public void setPosition(Angle rotation) { - controller.setSetpoint(rotation.magnitude(), ControlType.kPosition); + public void setPosition(Angle rotation, AngularVelocity velocity) { + double feedforward = + RobotConstants.kNominalVoltage + * velocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond); + controller.setSetpoint( + rotation.magnitude(), ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java similarity index 90% rename from src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java rename to src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index dd535430..3b54278f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -26,7 +26,10 @@ import frc.robot.util.SparkOdometryThread; import frc.robot.util.SparkOdometryThread.SparkInputs; -public class IntakeArmIOReal implements IntakeArmIO { +public class IntakeArmIOSpark implements IntakeArmIO { + private final double kPRealPos = 1.0; + private final double kPRealVel = 1.0; + private final SparkMax intakeArmRight; private final SparkMax intakeArmLeft; private final RelativeEncoder encoderSpark; @@ -38,7 +41,7 @@ public class IntakeArmIOReal implements IntakeArmIO { private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - public IntakeArmIOReal() { + public IntakeArmIOSpark() { intakeArmRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); intakeArmLeft = new SparkMax(CAN2.intakeArmLeft, MotorType.kBrushless); encoderSpark = intakeArmRight.getEncoder(); @@ -111,9 +114,14 @@ public void setOpenLoop(Voltage volts) { } @Override - public void setPosition(Angle rotation) { + public void setPosition(Angle rotation, AngularVelocity velocity) { + double feedforward = + RobotConstants.kNominalVoltage + * velocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond); double setpoint = MathUtil.clamp(rotation.magnitude(), minPosRad, maxPosRad); - intakeArmController.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0); + intakeArmController.setSetpoint( + setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 8b34352d..1c2d9dca 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -11,6 +11,7 @@ import edu.wpi.first.units.measure.Distance; import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.KrakenX60Constants; +import frc.robot.Constants.MotorConstants.NEOConstants; public class IntakeConstants { /** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */ @@ -59,11 +60,8 @@ public static class ArmConstants { public static final double encoderPositionFactor = 2 * Math.PI / motorReduction; public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * motorReduction); - public static final double kPSim = 1.0; - public static final double kDSim = 1.0; - - public static final double kPRealPos = 1.0; - public static final double kPRealVel = 1.0; + public static final AngularVelocity maxAngularVelocity = + NEOConstants.kFreeSpeed.div(motorReduction); public static final Angle maxPos = Degrees.of(90.0); public static final Angle minPos = Degrees.of(0.0); From e81944f58ecae8da07dae3f3768aa68757bdfab7 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 21 Apr 2026 19:15:46 -0400 Subject: [PATCH 05/29] formatting --- 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 d21e8d22..2b934c8c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -66,8 +66,8 @@ import frc.robot.subsystems.hopper.HopperIOSim; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeArmIO; -import frc.robot.subsystems.intake.IntakeArmIOSpark; import frc.robot.subsystems.intake.IntakeArmIOSimSpark; +import frc.robot.subsystems.intake.IntakeArmIOSpark; import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; import frc.robot.subsystems.intake.RollerIO; From cbf2f1d6ce8e65a378a034cbe042afadd3cd1996 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 21 Apr 2026 19:20:46 -0400 Subject: [PATCH 06/29] renamed variables --- src/main/java/frc/robot/Robot.java | 3 +-- .../frc/robot/subsystems/intake/IntakeArmIOSimSpark.java | 6 +++--- .../frc/robot/subsystems/intake/IntakeArmIOSpark.java | 8 ++++---- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2b934c8c..b4d8db17 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -241,12 +241,11 @@ public Robot() { new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOSim()); - var intakeArmIOSim = new IntakeArmIOSimSpark(); intake = new Intake( new RollerIOSimSpark(RollerConstants.upperRollerConfig), new RollerIOSimSpark(RollerConstants.lowerRollerConfig), - intakeArmIOSim); + new IntakeArmIOSimSpark()); break; case REPLAY: // Replaying a log diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index fa45c2b7..b0e4748e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -26,8 +26,8 @@ import frc.robot.Robot; public class IntakeArmIOSimSpark implements IntakeArmIO { - private final double kPSim = 1.0; - private final double kDSim = 1.0; + private static final double kP = 1.0; + private static final double kD = 1.0; private final DCMotorSim armSim; @@ -58,7 +58,7 @@ public IntakeArmIOSimSpark() { .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - armConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, kDSim); + armConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); armConfig .softLimit diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 3b54278f..3dd0a18c 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -27,8 +27,8 @@ import frc.robot.util.SparkOdometryThread.SparkInputs; public class IntakeArmIOSpark implements IntakeArmIO { - private final double kPRealPos = 1.0; - private final double kPRealVel = 1.0; + private static final double kPPos = 1.0; + private static final double kPVel = 1.0; private final SparkMax intakeArmRight; private final SparkMax intakeArmLeft; @@ -63,8 +63,8 @@ public IntakeArmIOSpark() { rightArmConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kPRealPos, 0.0, 0.0, ClosedLoopSlot.kSlot0) - .pid(kPRealVel, 0.0, 0.0, ClosedLoopSlot.kSlot1); + .pid(kPPos, 0.0, 0.0, ClosedLoopSlot.kSlot0) + .pid(kPVel, 0.0, 0.0, ClosedLoopSlot.kSlot1); rightArmConfig .softLimit From e4ff5d72ce7cfb6b9e89d415446832fbecf6a4c2 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 21 Apr 2026 20:02:22 -0400 Subject: [PATCH 07/29] removed compressor --- src/main/java/frc/robot/Robot.java | 13 ------------- .../java/frc/robot/subsystems/intake/Intake.java | 2 +- 2 files changed, 1 insertion(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b4d8db17..8757d7e5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,7 +7,6 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.BatterySim; @@ -32,7 +31,6 @@ import frc.lib.ControllerSelector.DriverConfig; import frc.lib.ControllerSelector.DriverController; import frc.lib.ControllerSelector.OperatorConfig; -import frc.lib.LoggedCompressor; import frc.lib.LoggedPowerDistribution; import frc.lib.ZorroController.Axis; import frc.robot.Constants.DIOPorts; @@ -137,8 +135,6 @@ public class Robot extends LoggedRobot { private Intake intake; private Hopper hopper; private LEDController leds = LEDController.getInstance(); - private LoggedCompressor compressor; - private PneumaticsSimulator pneumaticsSimulator; // Battery simulation constants private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc @@ -201,7 +197,6 @@ public Robot() { new RollerIOSpark(RollerConstants.lowerRollerConfig), new IntakeArmIOSpark()); feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); - compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor"); // Start kernel log monitoring (singleton, starts automatically on first call) KernelLogMonitor.getInstance(); @@ -352,7 +347,6 @@ public void robotPeriodic() { logCANBus("CAN2", Constants.CANBusPorts.CAN2.bus); logCANBus("CANHD", Constants.CANBusPorts.CANHD.bus); powerDistribution.log(); - if (compressor != null) compressor.log(); logHIDs(); logScheduler(); @@ -441,9 +435,6 @@ public void teleopInit() { public void teleopPeriodic() { leds.displayHubCountdown(); leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning()); - if (!DriverStation.isFMSAttached()) { - leds.displayCompressorState(compressor != null && compressor.isEnabled()); - } } /** This function is called once when test mode is enabled. */ @@ -470,11 +461,8 @@ public void simulationInit() { /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { - // Skip battery simulation during replay (pneumaticsSimulator is only initialized in SIM mode) - if (pneumaticsSimulator == null) return; // Update battery voltage based on total current draw this cycle - pneumaticsSimulator.update(Robot.defaultPeriodSecs); RoboRioSim.setVInVoltage( vBusFilter.calculate( Math.max( @@ -484,7 +472,6 @@ public void simulationPeriodic() { launcher.getSimCurrentDrawAmps(), feeder.getSimCurrentDrawAmps(), intake.getSimCurrentDrawAmps(), - pneumaticsSimulator.getCompressorCurrentAmps(), ELECTRONICS_OVERHEAD_AMPS)))); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 1ffdb388..f21add19 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -181,7 +181,7 @@ public Command initializeIntakeArmCommand() { // initialize () -> { intakeArmIO.configureSoftLimits(false); - intakeArmIO.setOpenLoop(Volts.of(1.0)); + intakeArmIO.setOpenLoop(Volts.of(-1.0)); }, // end () -> { From 1928d96baaffd4b9a6e6c9117be4fdb885718547 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Tue, 21 Apr 2026 20:37:48 -0400 Subject: [PATCH 08/29] some tuning --- .../java/frc/robot/subsystems/intake/IntakeArmIOSpark.java | 4 ++-- .../java/frc/robot/subsystems/intake/IntakeConstants.java | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 3dd0a18c..dc8a59df 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -75,7 +75,7 @@ public IntakeArmIOSpark() { leftArmConfig = new SparkMaxConfig(); - leftArmConfig.apply(rightArmConfig).follow(CAN2.intakeArmRight); + leftArmConfig.apply(rightArmConfig).follow(CAN2.intakeArmRight, true); rightArmConfig .signals @@ -152,6 +152,6 @@ public void configureSoftLimits(boolean enable) { @Override public void resetEncoder() { - encoderSpark.setPosition(minPosRad); + encoderSpark.setPosition(0.0); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index e9ea1925..68905b12 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -55,8 +55,9 @@ public static class ArmConstants { public static final AngularVelocity maxAngularVelocity = NEOConstants.kFreeSpeed.div(motorReduction); + public static final Angle backlash = Degrees.of(25); public static final Angle maxPos = Degrees.of(90.0); - public static final Angle minPos = Degrees.of(0.0); + public static final Angle minPos = Degrees.of(0.0).minus(backlash); public static final double maxPosRad = maxPos.in(Radians); public static final double minPosRad = minPos.in(Radians); From 8fae0b601902e49a6a6bb7fa1ab5e58fe92c80b2 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 26 May 2026 18:47:32 -0400 Subject: [PATCH 09/29] added absolute encoder to the intake arm --- .../frc/robot/subsystems/intake/Intake.java | 2 +- .../robot/subsystems/intake/IntakeArmIO.java | 3 ++ .../subsystems/intake/IntakeArmIOSpark.java | 40 ++++++++++++------- .../subsystems/intake/IntakeConstants.java | 2 +- 4 files changed, 31 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index f21add19..1ffdb388 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -181,7 +181,7 @@ public Command initializeIntakeArmCommand() { // initialize () -> { intakeArmIO.configureSoftLimits(false); - intakeArmIO.setOpenLoop(Volts.of(-1.0)); + intakeArmIO.setOpenLoop(Volts.of(1.0)); }, // end () -> { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index 354b872a..3c2a17b0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.intake; +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.Voltage; @@ -13,6 +14,8 @@ public static class IntakeArmIOInputs { public double velocityMetersPerSec = 0.0; public double appliedVolts = 0.0; public double currentAmps = 0.0; + + public Rotation2d absolutePosition = Rotation2d.kZero; } public default void updateInputs(IntakeArmIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index dc8a59df..cd359016 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -4,6 +4,7 @@ import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; import static frc.robot.util.SparkUtil.*; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; @@ -17,6 +18,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; +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.Voltage; @@ -30,8 +32,9 @@ public class IntakeArmIOSpark implements IntakeArmIO { private static final double kPPos = 1.0; private static final double kPVel = 1.0; - private final SparkMax intakeArmRight; private final SparkMax intakeArmLeft; + private final SparkMax intakeArmRight; + private final AbsoluteEncoder absoluteEncoder; private final RelativeEncoder encoderSpark; private final SparkClosedLoopController intakeArmController; private final SparkInputs sparkInputs; @@ -41,11 +44,14 @@ public class IntakeArmIOSpark implements IntakeArmIO { private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private boolean relativeEncoderSeeded = false; + public IntakeArmIOSpark() { - intakeArmRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); intakeArmLeft = new SparkMax(CAN2.intakeArmLeft, MotorType.kBrushless); - encoderSpark = intakeArmRight.getEncoder(); - intakeArmController = intakeArmRight.getClosedLoopController(); + intakeArmRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); + absoluteEncoder = intakeArmLeft.getAbsoluteEncoder(); + encoderSpark = intakeArmLeft.getEncoder(); + intakeArmController = intakeArmLeft.getClosedLoopController(); rightArmConfig = new SparkMaxConfig(); @@ -84,33 +90,39 @@ public IntakeArmIOSpark() { .outputCurrentPeriodMs(20); tryUntilOk( - intakeArmRight, + intakeArmLeft, 5, () -> - intakeArmRight.configure( + intakeArmLeft.configure( rightArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); tryUntilOk( - intakeArmLeft, + intakeArmRight, 5, () -> - intakeArmLeft.configure( + intakeArmRight.configure( leftArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - sparkInputs = SparkOdometryThread.getInstance().registerSpark(intakeArmRight, encoderSpark); + sparkInputs = SparkOdometryThread.getInstance().registerSpark(intakeArmLeft, encoderSpark); } @Override public void updateInputs(IntakeArmIOInputs inputs) { + if (!relativeEncoderSeeded) { + encoderSpark.setPosition(absoluteEncoder.getPosition()); + } + inputs.position = sparkInputs.getPosition(); inputs.velocityMetersPerSec = sparkInputs.getVelocity(); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); + + inputs.absolutePosition = new Rotation2d(absoluteEncoder.getPosition()); } @Override public void setOpenLoop(Voltage volts) { - intakeArmRight.setVoltage(volts); + intakeArmLeft.setVoltage(volts); } @Override @@ -135,18 +147,18 @@ public void configureSoftLimits(boolean enable) { rightArmConfig.softLimit.forwardSoftLimitEnabled(enable); rightArmConfig.softLimit.reverseSoftLimitEnabled(enable); tryUntilOk( - intakeArmRight, + intakeArmLeft, 5, () -> - intakeArmRight.configure( + intakeArmLeft.configure( rightArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); tryUntilOk( - intakeArmLeft, + intakeArmRight, 5, () -> - intakeArmLeft.configure( + intakeArmRight.configure( leftArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 68905b12..17c06d85 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -56,7 +56,7 @@ public static class ArmConstants { NEOConstants.kFreeSpeed.div(motorReduction); public static final Angle backlash = Degrees.of(25); - public static final Angle maxPos = Degrees.of(90.0); + public static final Angle maxPos = Degrees.of(110.0); public static final Angle minPos = Degrees.of(0.0).minus(backlash); public static final double maxPosRad = maxPos.in(Radians); public static final double minPosRad = minPos.in(Radians); From f1bae5558b693cb0ab3d8f3c49cfb7d63ace0e75 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 26 May 2026 19:21:35 -0400 Subject: [PATCH 10/29] added absolute encoder config --- .../subsystems/intake/IntakeArmIOSpark.java | 45 ++++++++++++------- .../subsystems/intake/IntakeConstants.java | 5 +++ 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index cd359016..258517e0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.AbsoluteEncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; @@ -39,8 +40,9 @@ public class IntakeArmIOSpark implements IntakeArmIO { private final SparkClosedLoopController intakeArmController; private final SparkInputs sparkInputs; - private final SparkMaxConfig rightArmConfig; private final SparkMaxConfig leftArmConfig; + private final SparkMaxConfig rightArmConfig; + private final AbsoluteEncoderConfig absEncoderConfig; private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); @@ -53,37 +55,46 @@ public IntakeArmIOSpark() { encoderSpark = intakeArmLeft.getEncoder(); intakeArmController = intakeArmLeft.getClosedLoopController(); - rightArmConfig = new SparkMaxConfig(); + absEncoderConfig = new AbsoluteEncoderConfig(); + + absEncoderConfig + .zeroOffset(absEncoderOffset) + .positionConversionFactor(absEncoderPositionFactor) + .velocityConversionFactor(absEncoderVelocityFactor); + + leftArmConfig = new SparkMaxConfig(); - rightArmConfig + leftArmConfig .inverted(false) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOConstants.kDefaultSupplyCurrentLimit) .voltageCompensation(RobotConstants.kNominalVoltage); - rightArmConfig + leftArmConfig .encoder .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - rightArmConfig + leftArmConfig.absoluteEncoder.apply(absEncoderConfig); + + leftArmConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pid(kPPos, 0.0, 0.0, ClosedLoopSlot.kSlot0) .pid(kPVel, 0.0, 0.0, ClosedLoopSlot.kSlot1); - rightArmConfig + leftArmConfig .softLimit .forwardSoftLimit(maxPosRad) .forwardSoftLimitEnabled(true) .reverseSoftLimit(minPosRad) .reverseSoftLimitEnabled(true); - leftArmConfig = new SparkMaxConfig(); + rightArmConfig = new SparkMaxConfig(); - leftArmConfig.apply(rightArmConfig).follow(CAN2.intakeArmRight, true); + rightArmConfig.apply(leftArmConfig).follow(CAN2.intakeArmRight, true); - rightArmConfig + leftArmConfig .signals .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) @@ -94,13 +105,13 @@ public IntakeArmIOSpark() { 5, () -> intakeArmLeft.configure( - rightArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + leftArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); tryUntilOk( intakeArmRight, 5, () -> intakeArmRight.configure( - leftArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + rightArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); sparkInputs = SparkOdometryThread.getInstance().registerSpark(intakeArmLeft, encoderSpark); } @@ -144,22 +155,22 @@ public void setVelocity(AngularVelocity velocity) { @Override public void configureSoftLimits(boolean enable) { - rightArmConfig.softLimit.forwardSoftLimitEnabled(enable); - rightArmConfig.softLimit.reverseSoftLimitEnabled(enable); + leftArmConfig.softLimit.forwardSoftLimitEnabled(enable); + leftArmConfig.softLimit.reverseSoftLimitEnabled(enable); tryUntilOk( intakeArmLeft, 5, () -> intakeArmLeft.configure( - rightArmConfig, - ResetMode.kNoResetSafeParameters, - PersistMode.kNoPersistParameters)); + leftArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); tryUntilOk( intakeArmRight, 5, () -> intakeArmRight.configure( - leftArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); + rightArmConfig, + ResetMode.kNoResetSafeParameters, + PersistMode.kNoPersistParameters)); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 17c06d85..89763cda 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -52,6 +52,11 @@ public static class ArmConstants { public static final double encoderPositionFactor = 2 * Math.PI / motorReduction; public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * motorReduction); + public static final double absEncoderPositionFactor = 2 * Math.PI; + public static final double absEncoderVelocityFactor = (2 * Math.PI) / 60.0; + + public static final double absEncoderOffset = 0; + public static final AngularVelocity maxAngularVelocity = NEOConstants.kFreeSpeed.div(motorReduction); From 5e882ce726856b7524090fd3d8b42ee504ceb296 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 28 May 2026 18:41:50 -0400 Subject: [PATCH 11/29] attempted to tune absolute encoder, it is not working right now --- src/main/java/frc/robot/Robot.java | 4 +-- .../frc/robot/subsystems/intake/Intake.java | 35 +++++++++---------- .../subsystems/intake/IntakeArmIOSpark.java | 1 + .../subsystems/intake/IntakeConstants.java | 2 +- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8757d7e5..6d09b3d0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -319,8 +319,8 @@ public Robot() { Field.plotRegions(); feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop")); - intake.setDefaultCommand( - intake.initializeIntakeArmCommand().andThen(intake.getDefaultCommand())); + intake.setDefaultCommand(intake.getDefaultCommand()); + // intake.initializeIntakeArmCommand().andThen(intake.getDefaultCommand())); launcher.setDefaultCommand( launcher .initializeHoodCommand() diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 1ffdb388..77ca9b81 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import java.util.function.BooleanSupplier; @@ -176,21 +175,21 @@ public Command getShakeIntakeCommand() { .repeatedly(); } - public Command initializeIntakeArmCommand() { - return new StartEndCommand( - // initialize - () -> { - intakeArmIO.configureSoftLimits(false); - intakeArmIO.setOpenLoop(Volts.of(1.0)); - }, - // end - () -> { - intakeArmIO.configureSoftLimits(true); - intakeArmIO.resetEncoder(); - }, - // requirements - this) - .withTimeout(1.0) - .withName("Initialize intake arm"); - } + // public Command initializeIntakeArmCommand() { + // return new StartEndCommand( + // // initialize + // () -> { + // intakeArmIO.configureSoftLimits(false); + // intakeArmIO.setOpenLoop(Volts.of(1.0)); + // }, + // // end + // () -> { + // intakeArmIO.configureSoftLimits(true); + // intakeArmIO.resetEncoder(); + // }, + // // requirements + // this) + // .withTimeout(1.0) + // .withName("Initialize intake arm"); + // } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 258517e0..71e6370e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -120,6 +120,7 @@ public IntakeArmIOSpark() { public void updateInputs(IntakeArmIOInputs inputs) { if (!relativeEncoderSeeded) { encoderSpark.setPosition(absoluteEncoder.getPosition()); + relativeEncoderSeeded = true; } inputs.position = sparkInputs.getPosition(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 89763cda..7b4f6527 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -60,7 +60,7 @@ public static class ArmConstants { public static final AngularVelocity maxAngularVelocity = NEOConstants.kFreeSpeed.div(motorReduction); - public static final Angle backlash = Degrees.of(25); + public static final Angle backlash = Degrees.of(0); // original was 25 public static final Angle maxPos = Degrees.of(110.0); public static final Angle minPos = Degrees.of(0.0).minus(backlash); public static final double maxPosRad = maxPos.in(Radians); From bdfbdc412934b1b1ee3e4c5da327f6e85b49d15b Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 09:53:59 -0400 Subject: [PATCH 12/29] Fix stale constant references left over from merge Constants.java's UPPER_SNAKE_CASE rename (PR #192, merged from main) landed after the motorized intake arm's CAN ports and current-limit/voltage constants were written against the old names, so they never got renamed. Update the two Spark IO files to match. --- .../subsystems/intake/IntakeArmIOSimSpark.java | 14 +++++++------- .../robot/subsystems/intake/IntakeArmIOSpark.java | 12 ++++++------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index b0e4748e..1fe214af 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -40,8 +40,8 @@ public class IntakeArmIOSimSpark implements IntakeArmIO { private final SparkMaxConfig followerConfig; public IntakeArmIOSimSpark() { - maxRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); - maxLeft = new SparkMax(CAN2.intakeArmLeft, MotorType.kBrushless); + maxRight = new SparkMax(CAN2.INTAKE_ARM_RIGHT, MotorType.kBrushless); + maxLeft = new SparkMax(CAN2.INTAKE_ARM_LEFT, MotorType.kBrushless); controller = maxRight.getClosedLoopController(); @@ -50,8 +50,8 @@ public IntakeArmIOSimSpark() { armConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); armConfig .encoder @@ -69,7 +69,7 @@ public IntakeArmIOSimSpark() { followerConfig = new SparkMaxConfig(); - followerConfig.apply(armConfig).follow(CAN2.intakeArmRight); + followerConfig.apply(armConfig).follow(CAN2.INTAKE_ARM_RIGHT); maxRight.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); maxSim = new SparkMaxSim(maxRight, gearbox); @@ -108,13 +108,13 @@ public void updateInputs(IntakeArmIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setPosition(Angle rotation, AngularVelocity velocity) { double feedforward = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * velocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 71e6370e..aff8e3e5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -49,8 +49,8 @@ public class IntakeArmIOSpark implements IntakeArmIO { private boolean relativeEncoderSeeded = false; public IntakeArmIOSpark() { - intakeArmLeft = new SparkMax(CAN2.intakeArmLeft, MotorType.kBrushless); - intakeArmRight = new SparkMax(CAN2.intakeArmRight, MotorType.kBrushless); + intakeArmLeft = new SparkMax(CAN2.INTAKE_ARM_LEFT, MotorType.kBrushless); + intakeArmRight = new SparkMax(CAN2.INTAKE_ARM_RIGHT, MotorType.kBrushless); absoluteEncoder = intakeArmLeft.getAbsoluteEncoder(); encoderSpark = intakeArmLeft.getEncoder(); intakeArmController = intakeArmLeft.getClosedLoopController(); @@ -67,8 +67,8 @@ public IntakeArmIOSpark() { leftArmConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); leftArmConfig .encoder @@ -92,7 +92,7 @@ public IntakeArmIOSpark() { rightArmConfig = new SparkMaxConfig(); - rightArmConfig.apply(leftArmConfig).follow(CAN2.intakeArmRight, true); + rightArmConfig.apply(leftArmConfig).follow(CAN2.INTAKE_ARM_RIGHT, true); leftArmConfig .signals @@ -140,7 +140,7 @@ public void setOpenLoop(Voltage volts) { @Override public void setPosition(Angle rotation, AngularVelocity velocity) { double feedforward = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * velocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); double setpoint = MathUtil.clamp(rotation.magnitude(), minPosRad, maxPosRad); From 4bedda0382efee6cfec8adc413c3657102082d88 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 10:19:55 -0400 Subject: [PATCH 13/29] make arm actuators independent --- src/main/java/frc/robot/Robot.java | 11 ++- .../frc/robot/subsystems/intake/Intake.java | 42 ++++++--- .../intake/IntakeArmIOSimSpark.java | 65 ++++++------- .../subsystems/intake/IntakeArmIOSpark.java | 93 +++++++------------ .../subsystems/intake/IntakeConstants.java | 8 ++ 5 files changed, 105 insertions(+), 114 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 60be7941..f01f1dfd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -68,6 +68,7 @@ import frc.robot.subsystems.intake.IntakeArmIOSimSpark; import frc.robot.subsystems.intake.IntakeArmIOSpark; import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.intake.IntakeConstants.ArmConstants; import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; import frc.robot.subsystems.intake.RollerIO; import frc.robot.subsystems.intake.RollerIOSimSpark; @@ -196,7 +197,8 @@ public Robot() { new Intake( new RollerIOSpark(RollerConstants.UPPER_ROLLER_CONFIG), new RollerIOSpark(RollerConstants.LOWER_ROLLER_CONFIG), - new IntakeArmIOSpark()); + new IntakeArmIOSpark(ArmConstants.LEFT_ARM_CONFIG), + new IntakeArmIOSpark(ArmConstants.RIGHT_ARM_CONFIG)); feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); // Start kernel log monitoring (singleton, starts automatically on first call) @@ -237,7 +239,8 @@ public Robot() { new Intake( new RollerIOSimSpark(RollerConstants.UPPER_ROLLER_CONFIG), new RollerIOSimSpark(RollerConstants.LOWER_ROLLER_CONFIG), - new IntakeArmIOSimSpark()); + new IntakeArmIOSimSpark(ArmConstants.LEFT_ARM_CONFIG), + new IntakeArmIOSimSpark(ArmConstants.RIGHT_ARM_CONFIG)); break; case REPLAY: // Replaying a log @@ -272,7 +275,9 @@ public Robot() { new FlywheelIO() {}, new HoodIO() {}); if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIO() {}); - intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}); + intake = + new Intake( + new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}, new IntakeArmIO() {}); feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); break; } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 77ca9b81..0b320c4a 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -18,29 +18,38 @@ public class Intake extends SubsystemBase { private final RollerIO upperRollerIO; private final RollerIO lowerRollerIO; - private final IntakeArmIO intakeArmIO; + private final IntakeArmIO leftArmIO; + private final IntakeArmIO rightArmIO; private final RollerIOInputsAutoLogged upperRollerInputs = new RollerIOInputsAutoLogged(); private final RollerIOInputsAutoLogged lowerRollerInputs = new RollerIOInputsAutoLogged(); - private final IntakeArmIOInputsAutoLogged intakeArmInputs = new IntakeArmIOInputsAutoLogged(); + private final IntakeArmIOInputsAutoLogged leftArmInputs = new IntakeArmIOInputsAutoLogged(); + private final IntakeArmIOInputsAutoLogged rightArmInputs = new IntakeArmIOInputsAutoLogged(); private final Alert upperRollerDisconnectedAlert; private final Alert lowerRollerDisconnectedAlert; - private final Alert intakeArmDisconnectedAlert; + private final Alert leftArmDisconnectedAlert; + private final Alert rightArmDisconnectedAlert; // Injected after both subsystems are created to avoid a circular dependency. // When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed. private BooleanSupplier hopperIsDeployed; private Supplier hopperDeployCommand; - public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intakeArmIO) { + public Intake( + RollerIO upperRollerIO, + RollerIO lowerRollerIO, + IntakeArmIO leftArmIO, + IntakeArmIO rightArmIO) { this.upperRollerIO = upperRollerIO; this.lowerRollerIO = lowerRollerIO; - this.intakeArmIO = intakeArmIO; + this.leftArmIO = leftArmIO; + this.rightArmIO = rightArmIO; upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError); lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError); - intakeArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); + leftArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); + rightArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); } @Override @@ -48,20 +57,24 @@ public void periodic() { long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; upperRollerIO.updateInputs(upperRollerInputs); lowerRollerIO.updateInputs(lowerRollerInputs); - intakeArmIO.updateInputs(intakeArmInputs); + leftArmIO.updateInputs(leftArmInputs); + rightArmIO.updateInputs(rightArmInputs); long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("UpperRoller", upperRollerInputs); Logger.processInputs("LowerRoller", lowerRollerInputs); - Logger.processInputs("IntakeArm", intakeArmInputs); + Logger.processInputs("LeftArm", leftArmInputs); + Logger.processInputs("RightArm", rightArmInputs); long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; upperRollerDisconnectedAlert.set(!upperRollerInputs.connected); lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected); - intakeArmDisconnectedAlert.set(!intakeArmInputs.connected); + leftArmDisconnectedAlert.set(!leftArmInputs.connected); + rightArmDisconnectedAlert.set(!rightArmInputs.connected); Logger.recordOutput("Faults/Intake/UpperRollerDisconnected", !upperRollerInputs.connected); Logger.recordOutput("Faults/Intake/LowerRollerDisconnected", !lowerRollerInputs.connected); - Logger.recordOutput("Faults/Intake/IntakeArmDisconnected", !intakeArmInputs.connected); + Logger.recordOutput("Faults/Intake/LeftArmDisconnected", !leftArmInputs.connected); + Logger.recordOutput("Faults/Intake/RightArmDisconnected", !rightArmInputs.connected); // Profiling output if (Constants.FeatureFlags.PROFILING_ENABLED) { @@ -82,15 +95,18 @@ public void periodic() { public void stop() { upperRollerIO.setOpenLoop(Volts.of(0.0)); lowerRollerIO.setOpenLoop(Volts.of(0.0)); - intakeArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); + leftArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); + rightArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); } public void deployArm() { - intakeArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0)); + leftArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0)); + rightArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0)); } public void retractArm() { - intakeArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); + leftArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); + rightArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); } public Boolean isDeployed() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index 1fe214af..a8b0cded 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -20,7 +20,6 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.NEOConstants; import frc.robot.Constants.RobotConstants; import frc.robot.Robot; @@ -31,84 +30,74 @@ public class IntakeArmIOSimSpark implements IntakeArmIO { private final DCMotorSim armSim; - private final SparkMax maxRight; - private final SparkMax maxLeft; + private final SparkMax motor; private final SparkClosedLoopController controller; - private final SparkMaxSim maxSim; + private final SparkMaxSim motorSim; - private final SparkMaxConfig armConfig; - private final SparkMaxConfig followerConfig; + private final SparkMaxConfig motorConfig; - public IntakeArmIOSimSpark() { - maxRight = new SparkMax(CAN2.INTAKE_ARM_RIGHT, MotorType.kBrushless); - maxLeft = new SparkMax(CAN2.INTAKE_ARM_LEFT, MotorType.kBrushless); + public IntakeArmIOSimSpark(ArmConfig armConfig) { + motor = new SparkMax(armConfig.port(), MotorType.kBrushless); - controller = maxRight.getClosedLoopController(); + controller = motor.getClosedLoopController(); - armConfig = new SparkMaxConfig(); + motorConfig = new SparkMaxConfig(); - armConfig - .inverted(false) + motorConfig + .inverted(armConfig.inverted()) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - armConfig + motorConfig .encoder .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - armConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); + motorConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); - armConfig + motorConfig .softLimit .forwardSoftLimit(maxPosRad) .forwardSoftLimitEnabled(true) .reverseSoftLimit(minPosRad) .reverseSoftLimitEnabled(true); - followerConfig = new SparkMaxConfig(); - - followerConfig.apply(armConfig).follow(CAN2.INTAKE_ARM_RIGHT); - - maxRight.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - maxSim = new SparkMaxSim(maxRight, gearbox); + motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + motorSim = new SparkMaxSim(motor, gearbox); armSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); armSim.setState(0.0, 0.0); - maxSim.setPosition(0.0); - - maxLeft.configure( - followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + motorSim.setPosition(0.0); } @Override public void updateInputs(IntakeArmIOInputs inputs) { // Update simulation state double busVoltage = RoboRioSim.getVInVoltage(); - armSim.setInput(maxSim.getAppliedOutput() * busVoltage); + armSim.setInput(motorSim.getAppliedOutput() * busVoltage); armSim.update(Robot.defaultPeriodSecs); - if (maxSim.getPosition() > maxPosRad) { + if (motorSim.getPosition() > maxPosRad) { armSim.setState(maxPosRad, 0.0); - maxSim.setPosition(maxPosRad); + motorSim.setPosition(maxPosRad); } - maxSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); + motorSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); // Update inputs inputs.connected = true; - inputs.position = maxSim.getPosition(); - inputs.velocityMetersPerSec = maxSim.getVelocity(); - inputs.appliedVolts = maxSim.getAppliedOutput() * maxSim.getBusVoltage(); - inputs.currentAmps = Math.abs(maxSim.getMotorCurrent()); + inputs.position = motorSim.getPosition(); + inputs.velocityMetersPerSec = motorSim.getVelocity(); + inputs.appliedVolts = motorSim.getAppliedOutput() * motorSim.getBusVoltage(); + inputs.currentAmps = Math.abs(motorSim.getMotorCurrent()); } @Override public void setOpenLoop(Voltage volts) { - maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); + motorSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override @@ -123,12 +112,12 @@ public void setPosition(Angle rotation, AngularVelocity velocity) { @Override public void configureSoftLimits(boolean enable) { - armConfig.softLimit.forwardSoftLimitEnabled(enable); - armConfig.softLimit.reverseSoftLimitEnabled(enable); + motorConfig.softLimit.forwardSoftLimitEnabled(enable); + motorConfig.softLimit.reverseSoftLimitEnabled(enable); } @Override public void resetEncoder() { - maxSim.setPosition(maxPosRad); + motorSim.setPosition(maxPosRad); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index aff8e3e5..46b4271e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -23,7 +23,6 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.NEOConstants; import frc.robot.Constants.RobotConstants; import frc.robot.util.SparkOdometryThread; @@ -33,27 +32,24 @@ public class IntakeArmIOSpark implements IntakeArmIO { private static final double kPPos = 1.0; private static final double kPVel = 1.0; - private final SparkMax intakeArmLeft; - private final SparkMax intakeArmRight; - private final AbsoluteEncoder absoluteEncoder; - private final RelativeEncoder encoderSpark; - private final SparkClosedLoopController intakeArmController; + private final SparkMax motor; + private final AbsoluteEncoder absEncoder; + private final RelativeEncoder relEncoder; + private final SparkClosedLoopController controller; private final SparkInputs sparkInputs; - private final SparkMaxConfig leftArmConfig; - private final SparkMaxConfig rightArmConfig; + private final SparkMaxConfig motorConfig; private final AbsoluteEncoderConfig absEncoderConfig; private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); private boolean relativeEncoderSeeded = false; - public IntakeArmIOSpark() { - intakeArmLeft = new SparkMax(CAN2.INTAKE_ARM_LEFT, MotorType.kBrushless); - intakeArmRight = new SparkMax(CAN2.INTAKE_ARM_RIGHT, MotorType.kBrushless); - absoluteEncoder = intakeArmLeft.getAbsoluteEncoder(); - encoderSpark = intakeArmLeft.getEncoder(); - intakeArmController = intakeArmLeft.getClosedLoopController(); + public IntakeArmIOSpark(ArmConfig armConfig) { + motor = new SparkMax(armConfig.port(), MotorType.kBrushless); + absEncoder = motor.getAbsoluteEncoder(); + relEncoder = motor.getEncoder(); + controller = motor.getClosedLoopController(); absEncoderConfig = new AbsoluteEncoderConfig(); @@ -62,64 +58,50 @@ public IntakeArmIOSpark() { .positionConversionFactor(absEncoderPositionFactor) .velocityConversionFactor(absEncoderVelocityFactor); - leftArmConfig = new SparkMaxConfig(); + motorConfig = new SparkMaxConfig(); - leftArmConfig - .inverted(false) + motorConfig + .inverted(armConfig.inverted()) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - leftArmConfig + motorConfig .encoder .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - leftArmConfig.absoluteEncoder.apply(absEncoderConfig); + motorConfig.absoluteEncoder.apply(absEncoderConfig); - leftArmConfig + motorConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) .pid(kPPos, 0.0, 0.0, ClosedLoopSlot.kSlot0) .pid(kPVel, 0.0, 0.0, ClosedLoopSlot.kSlot1); - leftArmConfig + motorConfig .softLimit .forwardSoftLimit(maxPosRad) .forwardSoftLimitEnabled(true) .reverseSoftLimit(minPosRad) .reverseSoftLimitEnabled(true); - rightArmConfig = new SparkMaxConfig(); + motorConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); - rightArmConfig.apply(leftArmConfig).follow(CAN2.INTAKE_ARM_RIGHT, true); - - leftArmConfig - .signals - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); - - tryUntilOk( - intakeArmLeft, - 5, - () -> - intakeArmLeft.configure( - leftArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); tryUntilOk( - intakeArmRight, + motor, 5, () -> - intakeArmRight.configure( - rightArmConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + motor.configure( + motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - sparkInputs = SparkOdometryThread.getInstance().registerSpark(intakeArmLeft, encoderSpark); + sparkInputs = SparkOdometryThread.getInstance().registerSpark(motor, relEncoder); } @Override public void updateInputs(IntakeArmIOInputs inputs) { if (!relativeEncoderSeeded) { - encoderSpark.setPosition(absoluteEncoder.getPosition()); + relEncoder.setPosition(absEncoder.getPosition()); relativeEncoderSeeded = true; } @@ -129,12 +111,12 @@ public void updateInputs(IntakeArmIOInputs inputs) { inputs.currentAmps = sparkInputs.getOutputCurrent(); inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); - inputs.absolutePosition = new Rotation2d(absoluteEncoder.getPosition()); + inputs.absolutePosition = new Rotation2d(absEncoder.getPosition()); } @Override public void setOpenLoop(Voltage volts) { - intakeArmLeft.setVoltage(volts); + motor.setVoltage(volts); } @Override @@ -144,38 +126,29 @@ public void setPosition(Angle rotation, AngularVelocity velocity) { * velocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); double setpoint = MathUtil.clamp(rotation.magnitude(), minPosRad, maxPosRad); - intakeArmController.setSetpoint( - setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); + controller.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } @Override public void setVelocity(AngularVelocity velocity) { - intakeArmController.setSetpoint( + controller.setSetpoint( velocity.in(RadiansPerSecond), ControlType.kVelocity, ClosedLoopSlot.kSlot1); } @Override public void configureSoftLimits(boolean enable) { - leftArmConfig.softLimit.forwardSoftLimitEnabled(enable); - leftArmConfig.softLimit.reverseSoftLimitEnabled(enable); - tryUntilOk( - intakeArmLeft, - 5, - () -> - intakeArmLeft.configure( - leftArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); + motorConfig.softLimit.forwardSoftLimitEnabled(enable); + motorConfig.softLimit.reverseSoftLimitEnabled(enable); tryUntilOk( - intakeArmRight, + motor, 5, () -> - intakeArmRight.configure( - rightArmConfig, - ResetMode.kNoResetSafeParameters, - PersistMode.kNoPersistParameters)); + motor.configure( + motorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); } @Override public void resetEncoder() { - encoderSpark.setPosition(0.0); + relEncoder.setPosition(0.0); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 94a39928..8ad2e0d3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -74,5 +74,13 @@ public static class ArmConstants { public static final Angle minPos = Degrees.of(0.0).minus(backlash); public static final double maxPosRad = maxPos.in(Radians); public static final double minPosRad = minPos.in(Radians); + + // Configs + public record ArmConfig(int port, CANBus bus, boolean inverted) {} + + public static final ArmConfig LEFT_ARM_CONFIG = + new ArmConfig(CAN2.INTAKE_ARM_LEFT, CAN2.BUS, false); + public static final ArmConfig RIGHT_ARM_CONFIG = + new ArmConfig(CAN2.INTAKE_ARM_RIGHT, CAN2.BUS, true); } } From 07c93197a019da26d4bf50d681dbd30074d65dac Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 10:21:49 -0400 Subject: [PATCH 14/29] rm unused setVelocity --- src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java | 2 -- .../java/frc/robot/subsystems/intake/IntakeArmIOSpark.java | 6 ------ 2 files changed, 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index 3c2a17b0..e4fa4370 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -24,8 +24,6 @@ public default void setOpenLoop(Voltage volts) {} public default void setPosition(Angle rotation, AngularVelocity velocity) {} - public default void setVelocity(AngularVelocity velocity) {} - public default void configureSoftLimits(boolean enable) {} public default void resetEncoder() {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 46b4271e..69d535db 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -129,12 +129,6 @@ public void setPosition(Angle rotation, AngularVelocity velocity) { controller.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } - @Override - public void setVelocity(AngularVelocity velocity) { - controller.setSetpoint( - velocity.in(RadiansPerSecond), ControlType.kVelocity, ClosedLoopSlot.kSlot1); - } - @Override public void configureSoftLimits(boolean enable) { motorConfig.softLimit.forwardSoftLimitEnabled(enable); From 079b9156c504a202a22820a173ce11402397f509 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 10:28:11 -0400 Subject: [PATCH 15/29] Drive intake arm off a single trapezoidal motion profile Intake now owns one TrapezoidProfile and advances it every periodic() cycle; both arm IOs independently track the resulting setpoint via their own closed-loop control. Commands (stop/deployArm/retractArm) only move the goal state, never call setPosition() directly. --- .../frc/robot/subsystems/intake/Intake.java | 29 +++++++++++++++---- .../subsystems/intake/IntakeConstants.java | 4 +++ 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 0b320c4a..ab176820 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,16 +1,20 @@ package frc.robot.subsystems.intake; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.Robot; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -31,6 +35,13 @@ public class Intake extends SubsystemBase { private final Alert leftArmDisconnectedAlert; private final Alert rightArmDisconnectedAlert; + // Both arms independently follow the same profiled setpoint; commands only ever move the goal. + private final TrapezoidProfile armProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints(PROFILE_MAX_VELOCITY, PROFILE_MAX_ACCELERATION)); + private State armGoal = new State(minPosRad, 0.0); + private State armSetpoint = new State(minPosRad, 0.0); + // Injected after both subsystems are created to avoid a circular dependency. // When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed. private BooleanSupplier hopperIsDeployed; @@ -76,6 +87,15 @@ public void periodic() { Logger.recordOutput("Faults/Intake/LeftArmDisconnected", !leftArmInputs.connected); Logger.recordOutput("Faults/Intake/RightArmDisconnected", !rightArmInputs.connected); + // Advance the arm motion profile and drive both arms to the resulting setpoint. Commands + // never set arm position directly — they only move armGoal, and this is the sole place + // setPosition() is called. + armSetpoint = armProfile.calculate(Robot.defaultPeriodSecs, armSetpoint, armGoal); + leftArmIO.setPosition( + Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); + rightArmIO.setPosition( + Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); + // Profiling output if (Constants.FeatureFlags.PROFILING_ENABLED) { long totalMs = (t2 - t0) / 1_000_000; @@ -95,18 +115,15 @@ public void periodic() { public void stop() { upperRollerIO.setOpenLoop(Volts.of(0.0)); lowerRollerIO.setOpenLoop(Volts.of(0.0)); - leftArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); - rightArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); + armGoal = new State(minPosRad, 0.0); } public void deployArm() { - leftArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0)); - rightArmIO.setPosition(maxPos, RadiansPerSecond.of(0.0)); + armGoal = new State(maxPosRad, 0.0); } public void retractArm() { - leftArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); - rightArmIO.setPosition(minPos, RadiansPerSecond.of(0.0)); + armGoal = new State(minPosRad, 0.0); } public Boolean isDeployed() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 8ad2e0d3..6c1507af 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -75,6 +75,10 @@ public static class ArmConstants { public static final double maxPosRad = maxPos.in(Radians); public static final double minPosRad = minPos.in(Radians); + // Motion profile + public static final double PROFILE_MAX_VELOCITY = maxAngularVelocity.in(RadiansPerSecond); + public static final double PROFILE_MAX_ACCELERATION = 20.0; // rad/s^2 — assumed, tune on robot + // Configs public record ArmConfig(int port, CANBus bus, boolean inverted) {} From be6e9afbb991dc3be5ea3f433a2a191ab7083eaa Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 10:34:54 -0400 Subject: [PATCH 16/29] Seed arm encoders and motion profile from Intake, not per-IO Only the left arm's Spark has an absolute encoder wired up, so self-seeding the relative encoder inside each IntakeArmIOSpark instance was wrong for the right side. IntakeArmIO.resetEncoder() now takes a target position; Intake reads the left arm's absolute encoder once it reports connected and uses that single reading to seed both arms' relative encoders and reset the trapezoidal profile's goal/setpoint. --- .../java/frc/robot/subsystems/intake/Intake.java | 15 +++++++++++++++ .../frc/robot/subsystems/intake/IntakeArmIO.java | 2 +- .../subsystems/intake/IntakeArmIOSimSpark.java | 4 ++-- .../robot/subsystems/intake/IntakeArmIOSpark.java | 11 ++--------- 4 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index ab176820..bf6e8c10 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -42,6 +42,10 @@ public class Intake extends SubsystemBase { private State armGoal = new State(minPosRad, 0.0); private State armSetpoint = new State(minPosRad, 0.0); + // Only the left arm's Spark has an absolute encoder wired up. Both arms' relative encoders, + // and the motion profile itself, are seeded from that one reading the first time it's valid. + private boolean armSeeded = false; + // Injected after both subsystems are created to avoid a circular dependency. // When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed. private BooleanSupplier hopperIsDeployed; @@ -87,6 +91,17 @@ public void periodic() { Logger.recordOutput("Faults/Intake/LeftArmDisconnected", !leftArmInputs.connected); Logger.recordOutput("Faults/Intake/RightArmDisconnected", !rightArmInputs.connected); + // Seed both relative encoders, and the motion profile, from the left arm's absolute encoder + // once it reports connected. Runs once at boot. + if (!armSeeded && leftArmInputs.connected) { + double seedPositionRad = leftArmInputs.absolutePosition.getRadians(); + leftArmIO.resetEncoder(Radians.of(seedPositionRad)); + rightArmIO.resetEncoder(Radians.of(seedPositionRad)); + armGoal = new State(seedPositionRad, 0.0); + armSetpoint = new State(seedPositionRad, 0.0); + armSeeded = true; + } + // Advance the arm motion profile and drive both arms to the resulting setpoint. Commands // never set arm position directly — they only move armGoal, and this is the sole place // setPosition() is called. diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index e4fa4370..aaf6c8d4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -26,5 +26,5 @@ public default void setPosition(Angle rotation, AngularVelocity velocity) {} public default void configureSoftLimits(boolean enable) {} - public default void resetEncoder() {} + public default void resetEncoder(Angle position) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index a8b0cded..648c36bc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -117,7 +117,7 @@ public void configureSoftLimits(boolean enable) { } @Override - public void resetEncoder() { - motorSim.setPosition(maxPosRad); + public void resetEncoder(Angle position) { + motorSim.setPosition(position.magnitude()); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 69d535db..c47ba6fd 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -43,8 +43,6 @@ public class IntakeArmIOSpark implements IntakeArmIO { private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private boolean relativeEncoderSeeded = false; - public IntakeArmIOSpark(ArmConfig armConfig) { motor = new SparkMax(armConfig.port(), MotorType.kBrushless); absEncoder = motor.getAbsoluteEncoder(); @@ -100,11 +98,6 @@ public IntakeArmIOSpark(ArmConfig armConfig) { @Override public void updateInputs(IntakeArmIOInputs inputs) { - if (!relativeEncoderSeeded) { - relEncoder.setPosition(absEncoder.getPosition()); - relativeEncoderSeeded = true; - } - inputs.position = sparkInputs.getPosition(); inputs.velocityMetersPerSec = sparkInputs.getVelocity(); inputs.appliedVolts = sparkInputs.getAppliedVolts(); @@ -142,7 +135,7 @@ public void configureSoftLimits(boolean enable) { } @Override - public void resetEncoder() { - relEncoder.setPosition(0.0); + public void resetEncoder(Angle position) { + relEncoder.setPosition(position.magnitude()); } } From 3ff1d38d605cd5bd98d764650f8064dee9f11551 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 10:43:05 -0400 Subject: [PATCH 17/29] Remove dead code from the intake subsystem Intake.isDeployed() and the commented-out initializeIntakeArmCommand() (stale from before the left/right arm split) had no callers. IntakeArmIO.setOpenLoop()/configureSoftLimits() were only reachable from that dead command, so drop them from the interface and both implementations. Also removes a stray no-op semicolon in RollerIOSpark.setOpenLoop(). --- src/main/java/frc/robot/Robot.java | 1 - .../frc/robot/subsystems/intake/Intake.java | 22 ------------------- .../robot/subsystems/intake/IntakeArmIO.java | 5 ----- .../intake/IntakeArmIOSimSpark.java | 12 ---------- .../subsystems/intake/IntakeArmIOSpark.java | 18 --------------- .../subsystems/intake/RollerIOSpark.java | 1 - 6 files changed, 59 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f01f1dfd..1dcfcbab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -315,7 +315,6 @@ public Robot() { feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop")); intake.setDefaultCommand(intake.getDefaultCommand()); - // intake.initializeIntakeArmCommand().andThen(intake.getDefaultCommand())); launcher.setDefaultCommand( launcher .initializeHoodCommand() diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index bf6e8c10..5c14a617 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -141,10 +141,6 @@ public void retractArm() { armGoal = new State(minPosRad, 0.0); } - public Boolean isDeployed() { - return false; - } - public boolean isStowed() { return false; } @@ -222,22 +218,4 @@ public Command getShakeIntakeCommand() { Commands.runOnce(this::deployArm, this)) .repeatedly(); } - - // public Command initializeIntakeArmCommand() { - // return new StartEndCommand( - // // initialize - // () -> { - // intakeArmIO.configureSoftLimits(false); - // intakeArmIO.setOpenLoop(Volts.of(1.0)); - // }, - // // end - // () -> { - // intakeArmIO.configureSoftLimits(true); - // intakeArmIO.resetEncoder(); - // }, - // // requirements - // this) - // .withTimeout(1.0) - // .withName("Initialize intake arm"); - // } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index aaf6c8d4..ca02862b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -3,7 +3,6 @@ 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.Voltage; import org.littletonrobotics.junction.AutoLog; public interface IntakeArmIO { @@ -20,11 +19,7 @@ public static class IntakeArmIOInputs { public default void updateInputs(IntakeArmIOInputs inputs) {} - public default void setOpenLoop(Voltage volts) {} - public default void setPosition(Angle rotation, AngularVelocity velocity) {} - public default void configureSoftLimits(boolean enable) {} - public default void resetEncoder(Angle position) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index 648c36bc..448c2481 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -17,7 +17,6 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import frc.robot.Constants.MotorConstants.NEOConstants; @@ -95,11 +94,6 @@ public void updateInputs(IntakeArmIOInputs inputs) { inputs.currentAmps = Math.abs(motorSim.getMotorCurrent()); } - @Override - public void setOpenLoop(Voltage volts) { - motorSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); - } - @Override public void setPosition(Angle rotation, AngularVelocity velocity) { double feedforward = @@ -110,12 +104,6 @@ public void setPosition(Angle rotation, AngularVelocity velocity) { rotation.magnitude(), ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } - @Override - public void configureSoftLimits(boolean enable) { - motorConfig.softLimit.forwardSoftLimitEnabled(enable); - motorConfig.softLimit.reverseSoftLimitEnabled(enable); - } - @Override public void resetEncoder(Angle position) { motorSim.setPosition(position.magnitude()); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index c47ba6fd..534cf9f7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -22,7 +22,6 @@ 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.Voltage; import frc.robot.Constants.MotorConstants.NEOConstants; import frc.robot.Constants.RobotConstants; import frc.robot.util.SparkOdometryThread; @@ -107,11 +106,6 @@ public void updateInputs(IntakeArmIOInputs inputs) { inputs.absolutePosition = new Rotation2d(absEncoder.getPosition()); } - @Override - public void setOpenLoop(Voltage volts) { - motor.setVoltage(volts); - } - @Override public void setPosition(Angle rotation, AngularVelocity velocity) { double feedforward = @@ -122,18 +116,6 @@ public void setPosition(Angle rotation, AngularVelocity velocity) { controller.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } - @Override - public void configureSoftLimits(boolean enable) { - motorConfig.softLimit.forwardSoftLimitEnabled(enable); - motorConfig.softLimit.reverseSoftLimitEnabled(enable); - tryUntilOk( - motor, - 5, - () -> - motor.configure( - motorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); - } - @Override public void resetEncoder(Angle position) { relEncoder.setPosition(position.magnitude()); diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index 4a422d2c..056a8d4a 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -70,7 +70,6 @@ public void updateInputs(RollerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { flex.setVoltage(volts); - ; } @Override From 53f3b968ee626cd198149132ab5e9c87aebec6d0 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 10:49:15 -0400 Subject: [PATCH 18/29] add arm actuator currents to battery simulator --- src/main/java/frc/robot/subsystems/intake/Intake.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 5c14a617..09127aaa 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -184,7 +184,10 @@ public Command getDeployCommand() { /** Returns the total motor current draw for battery simulation. */ public double getSimCurrentDrawAmps() { - return upperRollerInputs.currentAmps + lowerRollerInputs.currentAmps; + return upperRollerInputs.currentAmps + + lowerRollerInputs.currentAmps + + leftArmInputs.currentAmps + + rightArmInputs.currentAmps; } public Command getReverseCommand() { From c7dc16c00861a6b22edae1f6eca2da16dcc205ac Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 10:56:22 -0400 Subject: [PATCH 19/29] Don't drive the intake arm before it's seeded, and guard bad seeds Commands could move armGoal before the boot-time seed completed (or if it never completes on a sensor fault), and the pre-seed hold-still behavior only worked because minPosRad happened to equal the relative encoder's power-on zero. Gate profile advancement and setPosition() on armSeeded explicitly so the arm holds under brake mode instead of closing a position loop against an unseeded, false-zero encoder. Also clamp the absolute-encoder seed into the soft limit range and raise an alert when the clamp fires, so a miscalibrated absEncoderOffset produces a visible warning instead of silently wedging the arm against a soft limit. Drops the unused kSlot1/kPVel closed-loop config left over from the removed arm velocity control. --- .../frc/robot/subsystems/intake/Intake.java | 31 ++++++++++++++----- .../subsystems/intake/IntakeArmIOSpark.java | 4 +-- 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 09127aaa..41e5f34d 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.Alert; @@ -34,6 +35,7 @@ public class Intake extends SubsystemBase { private final Alert lowerRollerDisconnectedAlert; private final Alert leftArmDisconnectedAlert; private final Alert rightArmDisconnectedAlert; + private final Alert armSeedOutOfRangeAlert; // Both arms independently follow the same profiled setpoint; commands only ever move the goal. private final TrapezoidProfile armProfile = @@ -65,6 +67,11 @@ public Intake( lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError); leftArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); rightArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); + armSeedOutOfRangeAlert = + new Alert( + "Intake arm absolute encoder seed is outside the soft limit range — check" + + " absEncoderOffset", + AlertType.kWarning); } @Override @@ -92,9 +99,13 @@ public void periodic() { Logger.recordOutput("Faults/Intake/RightArmDisconnected", !rightArmInputs.connected); // Seed both relative encoders, and the motion profile, from the left arm's absolute encoder - // once it reports connected. Runs once at boot. + // once it reports connected. Runs once at boot. The reading is clamped into the soft limit + // range so a miscalibrated absEncoderOffset can't seed a position the arm can never leave; + // armSeedOutOfRangeAlert flags when that clamp actually did something. if (!armSeeded && leftArmInputs.connected) { - double seedPositionRad = leftArmInputs.absolutePosition.getRadians(); + double rawSeedPositionRad = leftArmInputs.absolutePosition.getRadians(); + double seedPositionRad = MathUtil.clamp(rawSeedPositionRad, minPosRad, maxPosRad); + armSeedOutOfRangeAlert.set(seedPositionRad != rawSeedPositionRad); leftArmIO.resetEncoder(Radians.of(seedPositionRad)); rightArmIO.resetEncoder(Radians.of(seedPositionRad)); armGoal = new State(seedPositionRad, 0.0); @@ -104,12 +115,16 @@ public void periodic() { // Advance the arm motion profile and drive both arms to the resulting setpoint. Commands // never set arm position directly — they only move armGoal, and this is the sole place - // setPosition() is called. - armSetpoint = armProfile.calculate(Robot.defaultPeriodSecs, armSetpoint, armGoal); - leftArmIO.setPosition( - Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); - rightArmIO.setPosition( - Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); + // setPosition() is called. Nothing drives the arm until it's seeded: both Sparks hold their + // last commanded state (brake mode, no output) rather than closing a position loop against + // the unseeded, false-zero relative encoder. + if (armSeeded) { + armSetpoint = armProfile.calculate(Robot.defaultPeriodSecs, armSetpoint, armGoal); + leftArmIO.setPosition( + Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); + rightArmIO.setPosition( + Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); + } // Profiling output if (Constants.FeatureFlags.PROFILING_ENABLED) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 534cf9f7..e09f74b5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -29,7 +29,6 @@ public class IntakeArmIOSpark implements IntakeArmIO { private static final double kPPos = 1.0; - private static final double kPVel = 1.0; private final SparkMax motor; private final AbsoluteEncoder absEncoder; @@ -73,8 +72,7 @@ public IntakeArmIOSpark(ArmConfig armConfig) { motorConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kPPos, 0.0, 0.0, ClosedLoopSlot.kSlot0) - .pid(kPVel, 0.0, 0.0, ClosedLoopSlot.kSlot1); + .pid(kPPos, 0.0, 0.0, ClosedLoopSlot.kSlot0); motorConfig .softLimit From ecd56b0adf01a8717f6b2fa80885f570e1c1499e Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:01:57 -0400 Subject: [PATCH 20/29] Implement Intake.isStowed() with a tolerance band Previously a hardcoded false stub. Now true once the arm is seeded and both sides' measured position is within STOWED_TOLERANCE_RAD of minPosRad, mirroring the atGoal()/atSetpoint() pattern ProfiledPID Controller uses. Gated on armSeeded so it can't report "stowed" off an unseeded, false-zero encoder reading before boot calibration completes. --- src/main/java/frc/robot/subsystems/intake/Intake.java | 5 ++++- .../java/frc/robot/subsystems/intake/IntakeConstants.java | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 41e5f34d..1ae876b9 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -156,8 +156,11 @@ public void retractArm() { armGoal = new State(minPosRad, 0.0); } + /** True once both arms are seeded and measured within tolerance of the stowed position. */ public boolean isStowed() { - return false; + return armSeeded + && MathUtil.isNear(minPosRad, leftArmInputs.position, STOWED_TOLERANCE_RAD) + && MathUtil.isNear(minPosRad, rightArmInputs.position, STOWED_TOLERANCE_RAD); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 6c1507af..f52eee69 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -78,6 +78,8 @@ public static class ArmConstants { // Motion profile public static final double PROFILE_MAX_VELOCITY = maxAngularVelocity.in(RadiansPerSecond); public static final double PROFILE_MAX_ACCELERATION = 20.0; // rad/s^2 — assumed, tune on robot + public static final double STOWED_TOLERANCE_RAD = + Degrees.of(5.0).in(Radians); // assumed, tune on robot // Configs public record ArmConfig(int port, CANBus bus, boolean inverted) {} From 0cce0de5d48cca413013a066b990331b1d6dd38a Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:07:37 -0400 Subject: [PATCH 21/29] Invert intake arm motors/encoders, swap stowed/deployed targets MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Both arm motors' inverted flags are flipped (also flips each side's relative encoder direction, since REVLib ties relative-encoder phase to motor invert in brushless mode) and the absolute encoder is now configured inverted to match. With the raw range flipped, stowed is now the top of the [minPosRad, maxPosRad] range and deployed is the bottom, so Intake now targets the new STOWED_POS_RAD (110°, aliases maxPosRad) / DEPLOYED_POS_RAD (0°, aliases minPosRad) constants instead of using minPosRad/maxPosRad directly as go-to targets. Soft limits and clamping in the IO classes are untouched since they still bound the same absolute [0°, 110°] range. --- .../java/frc/robot/subsystems/intake/Intake.java | 14 +++++++------- .../robot/subsystems/intake/IntakeArmIOSpark.java | 1 + .../robot/subsystems/intake/IntakeConstants.java | 9 +++++++-- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 1ae876b9..eb32a4b7 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -41,8 +41,8 @@ public class Intake extends SubsystemBase { private final TrapezoidProfile armProfile = new TrapezoidProfile( new TrapezoidProfile.Constraints(PROFILE_MAX_VELOCITY, PROFILE_MAX_ACCELERATION)); - private State armGoal = new State(minPosRad, 0.0); - private State armSetpoint = new State(minPosRad, 0.0); + private State armGoal = new State(STOWED_POS_RAD, 0.0); + private State armSetpoint = new State(STOWED_POS_RAD, 0.0); // Only the left arm's Spark has an absolute encoder wired up. Both arms' relative encoders, // and the motion profile itself, are seeded from that one reading the first time it's valid. @@ -145,22 +145,22 @@ public void periodic() { public void stop() { upperRollerIO.setOpenLoop(Volts.of(0.0)); lowerRollerIO.setOpenLoop(Volts.of(0.0)); - armGoal = new State(minPosRad, 0.0); + armGoal = new State(STOWED_POS_RAD, 0.0); } public void deployArm() { - armGoal = new State(maxPosRad, 0.0); + armGoal = new State(DEPLOYED_POS_RAD, 0.0); } public void retractArm() { - armGoal = new State(minPosRad, 0.0); + armGoal = new State(STOWED_POS_RAD, 0.0); } /** True once both arms are seeded and measured within tolerance of the stowed position. */ public boolean isStowed() { return armSeeded - && MathUtil.isNear(minPosRad, leftArmInputs.position, STOWED_TOLERANCE_RAD) - && MathUtil.isNear(minPosRad, rightArmInputs.position, STOWED_TOLERANCE_RAD); + && MathUtil.isNear(STOWED_POS_RAD, leftArmInputs.position, STOWED_TOLERANCE_RAD) + && MathUtil.isNear(STOWED_POS_RAD, rightArmInputs.position, STOWED_TOLERANCE_RAD); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index e09f74b5..fe6f9678 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -50,6 +50,7 @@ public IntakeArmIOSpark(ArmConfig armConfig) { absEncoderConfig = new AbsoluteEncoderConfig(); absEncoderConfig + .inverted(true) .zeroOffset(absEncoderOffset) .positionConversionFactor(absEncoderPositionFactor) .velocityConversionFactor(absEncoderVelocityFactor); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f52eee69..f5e95080 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -81,12 +81,17 @@ public static class ArmConstants { public static final double STOWED_TOLERANCE_RAD = Degrees.of(5.0).in(Radians); // assumed, tune on robot + // Target positions. Motors and encoders are mounted inverted, so the raw range is flipped: + // stowed reads as the top of the range and deployed reads as the bottom. + public static final double STOWED_POS_RAD = maxPosRad; + public static final double DEPLOYED_POS_RAD = minPosRad; + // Configs public record ArmConfig(int port, CANBus bus, boolean inverted) {} public static final ArmConfig LEFT_ARM_CONFIG = - new ArmConfig(CAN2.INTAKE_ARM_LEFT, CAN2.BUS, false); + new ArmConfig(CAN2.INTAKE_ARM_LEFT, CAN2.BUS, true); public static final ArmConfig RIGHT_ARM_CONFIG = - new ArmConfig(CAN2.INTAKE_ARM_RIGHT, CAN2.BUS, true); + new ArmConfig(CAN2.INTAKE_ARM_RIGHT, CAN2.BUS, false); } } From 26f17d44b0b7f2b5025cfd951cf116a06ae16fef Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:14:14 -0400 Subject: [PATCH 22/29] Remove redundant manual soft-limit clamp in arm sim motorSim.iterate() already enforces the configured soft limits (REVLib's SparkSim reads the same motorConfig.softLimit and zeroes output past it, mirroring firmware behavior), so the manual position/velocity snap to maxPosRad was duplicate and, unlike the soft-limit path, discontinuously zeroed velocity instead of letting the sim physics decelerate naturally. --- .../frc/robot/subsystems/intake/IntakeArmIOSimSpark.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index 448c2481..87a92946 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -79,11 +79,6 @@ public void updateInputs(IntakeArmIOInputs inputs) { armSim.setInput(motorSim.getAppliedOutput() * busVoltage); armSim.update(Robot.defaultPeriodSecs); - if (motorSim.getPosition() > maxPosRad) { - armSim.setState(maxPosRad, 0.0); - motorSim.setPosition(maxPosRad); - } - motorSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); // Update inputs From 6f657ef4b5406c607bf86e18eec9993fa9afb74a Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:15:15 -0400 Subject: [PATCH 23/29] clean up setpoint constants --- .../java/frc/robot/subsystems/intake/IntakeConstants.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f5e95080..c0b6cc88 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; @@ -69,11 +68,8 @@ public static class ArmConstants { public static final AngularVelocity maxAngularVelocity = RadiansPerSecond.of(gearbox.freeSpeedRadPerSec / motorReduction); - public static final Angle backlash = Degrees.of(0); // original was 25 - public static final Angle maxPos = Degrees.of(110.0); - public static final Angle minPos = Degrees.of(0.0).minus(backlash); - public static final double maxPosRad = maxPos.in(Radians); - public static final double minPosRad = minPos.in(Radians); + public static final double maxPosRad = Degrees.of(110.0).in(Radians); + public static final double minPosRad = Degrees.of(0.0).in(Radians); // Motion profile public static final double PROFILE_MAX_VELOCITY = maxAngularVelocity.in(RadiansPerSecond); From 4cda5ba96e4433fd09151221db9fe4802de58963 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:20:13 -0400 Subject: [PATCH 24/29] Add cos(position) gravity feedforward to the arm MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Position 0 is horizontal in the current coordinate convention, so gravity torque (and the voltage needed to hold against it) scales with cos(position) and vanishes at vertical. Adds ArmConstants.kG and folds kG * cos(rotation) into the feedforward voltage in both the real and sim IO. kG defaults to 0.0 — not yet measured, so this has no effect until tuned on the robot. --- .../frc/robot/subsystems/intake/IntakeArmIOSimSpark.java | 5 +++-- .../java/frc/robot/subsystems/intake/IntakeArmIOSpark.java | 5 +++-- .../java/frc/robot/subsystems/intake/IntakeConstants.java | 4 ++++ 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index 87a92946..5fcb7705 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -93,8 +93,9 @@ public void updateInputs(IntakeArmIOInputs inputs) { public void setPosition(Angle rotation, AngularVelocity velocity) { double feedforward = RobotConstants.NOMINAL_VOLTAGE - * velocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + * velocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond) + + kG * Math.cos(rotation.magnitude()); controller.setSetpoint( rotation.magnitude(), ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index fe6f9678..dad2fab6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -109,8 +109,9 @@ public void updateInputs(IntakeArmIOInputs inputs) { public void setPosition(Angle rotation, AngularVelocity velocity) { double feedforward = RobotConstants.NOMINAL_VOLTAGE - * velocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + * velocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond) + + kG * Math.cos(rotation.magnitude()); double setpoint = MathUtil.clamp(rotation.magnitude(), minPosRad, maxPosRad); controller.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index c0b6cc88..dc90aa26 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -77,6 +77,10 @@ public static class ArmConstants { public static final double STOWED_TOLERANCE_RAD = Degrees.of(5.0).in(Radians); // assumed, tune on robot + // Gravity feedforward. Position 0 is horizontal, so torque from gravity — and thus the + // holding voltage — scales with cos(position) and drops to 0 at vertical (±90°). + public static final double kG = 0.0; // volts — not yet measured, has no effect until tuned + // Target positions. Motors and encoders are mounted inverted, so the raw range is flipped: // stowed reads as the top of the range and deployed reads as the bottom. public static final double STOWED_POS_RAD = maxPosRad; From 156fadbf0cf172173b8c1a47b3de7183b8fa6c3a Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:29:24 -0400 Subject: [PATCH 25/29] Feed gravity feedforward from measured position; model gravity in sim MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Position readback doesn't carry the hundreds-of-ms latency REV NEO velocity measurement does (that's why velocity feedforward stays commanded, not measured), so kG*cos() now reads the actual sensor position: relEncoder.getPosition() in the real IO, motorSim.getPosition() in sim. Swaps the sim's armSim from a bare DCMotorSim (no gravity term) to SingleJointedArmSim, which models the same cos(position) gravity torque as a uniform rod pivoting at one end. Without this, a nonzero kG in sim would have fought a gravity force that didn't exist; now sim and real hardware are physically consistent for the same kG. Adds MOMENT_OF_INERTIA_KG_M2 (carries over the previously-inline 0.004 placeholder) and ARM_LENGTH_METERS (new placeholder, ~16in) — both unmeasured, flagged for tuning on the robot. --- .../intake/IntakeArmIOSimSpark.java | 22 ++++++++++++------- .../subsystems/intake/IntakeArmIOSpark.java | 2 +- .../subsystems/intake/IntakeConstants.java | 6 +++++ 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index 5fcb7705..a70d341e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -14,11 +14,10 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Constants.MotorConstants.NEOConstants; import frc.robot.Constants.RobotConstants; import frc.robot.Robot; @@ -27,7 +26,7 @@ public class IntakeArmIOSimSpark implements IntakeArmIO { private static final double kP = 1.0; private static final double kD = 1.0; - private final DCMotorSim armSim; + private final SingleJointedArmSim armSim; private final SparkMax motor; private final SparkClosedLoopController controller; @@ -66,9 +65,16 @@ public IntakeArmIOSimSpark(ArmConfig armConfig) { motorSim = new SparkMaxSim(motor, gearbox); armSim = - new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + new SingleJointedArmSim( + gearbox, + motorReduction, + MOMENT_OF_INERTIA_KG_M2, + ARM_LENGTH_METERS, + minPosRad, + maxPosRad, + true, + 0.0); - armSim.setState(0.0, 0.0); motorSim.setPosition(0.0); } @@ -76,10 +82,10 @@ public IntakeArmIOSimSpark(ArmConfig armConfig) { public void updateInputs(IntakeArmIOInputs inputs) { // Update simulation state double busVoltage = RoboRioSim.getVInVoltage(); - armSim.setInput(motorSim.getAppliedOutput() * busVoltage); + armSim.setInputVoltage(motorSim.getAppliedOutput() * busVoltage); armSim.update(Robot.defaultPeriodSecs); - motorSim.iterate(armSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); + motorSim.iterate(armSim.getVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); // Update inputs inputs.connected = true; @@ -95,7 +101,7 @@ public void setPosition(Angle rotation, AngularVelocity velocity) { RobotConstants.NOMINAL_VOLTAGE * velocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond) - + kG * Math.cos(rotation.magnitude()); + + kG * Math.cos(motorSim.getPosition()); controller.setSetpoint( rotation.magnitude(), ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index dad2fab6..0fbe1175 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -111,7 +111,7 @@ public void setPosition(Angle rotation, AngularVelocity velocity) { RobotConstants.NOMINAL_VOLTAGE * velocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond) - + kG * Math.cos(rotation.magnitude()); + + kG * Math.cos(relEncoder.getPosition()); double setpoint = MathUtil.clamp(rotation.magnitude(), minPosRad, maxPosRad); controller.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index dc90aa26..68382984 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -81,6 +81,12 @@ public static class ArmConstants { // holding voltage — scales with cos(position) and drops to 0 at vertical (±90°). public static final double kG = 0.0; // volts — not yet measured, has no effect until tuned + // Sim physics. SingleJointedArmSim models the arm as a uniform rod pivoting at one end, + // giving it the same cos(position) gravity torque that kG compensates for on the real arm. + public static final double MOMENT_OF_INERTIA_KG_M2 = 0.004; // assumed, tune on robot + public static final double ARM_LENGTH_METERS = + 0.4; // ~16in — assumed placeholder, tune on robot + // Target positions. Motors and encoders are mounted inverted, so the raw range is flipped: // stowed reads as the top of the range and deployed reads as the bottom. public static final double STOWED_POS_RAD = maxPosRad; From afb7677d6d63d640020162ad7cb7d79553bf6a2a Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:36:29 -0400 Subject: [PATCH 26/29] make disconnected alerts unique --- src/main/java/frc/robot/subsystems/intake/Intake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index eb32a4b7..4b96f956 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -65,8 +65,8 @@ public Intake( upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError); lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError); - leftArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); - rightArmDisconnectedAlert = new Alert("Disconnected intake arm", AlertType.kError); + leftArmDisconnectedAlert = new Alert("Disconnected left intake arm", AlertType.kError); + rightArmDisconnectedAlert = new Alert("Disconnected right intake arm", AlertType.kError); armSeedOutOfRangeAlert = new Alert( "Intake arm absolute encoder seed is outside the soft limit range — check" From 9ba98fdf8f3a039236dfbbff4c52cd7d7fbf45cb Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:39:35 -0400 Subject: [PATCH 27/29] Rename IntakeArmIOInputs fields to match their actual units MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit position/velocityMetersPerSec were carried over from the linear RollerIO naming, but the arm's encoder conversion factors (encoderPositionFactor = 2π/reduction, encoderVelocityFactor = 2π/(60·reduction)) produce radians and rad/s, not meters/s — confirmed by every consumer (STOWED_POS_RAD, STOWED_TOLERANCE_RAD, Radians.of(), RadiansPerSecond.of()) already treating them as such. Renamed to positionRad/velocityRadPerSec, matching the convention already used by HoodIO/TurretIO/ModuleIO for the same kind of angular measurement. --- src/main/java/frc/robot/subsystems/intake/Intake.java | 4 ++-- src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java | 4 ++-- .../java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java | 4 ++-- .../java/frc/robot/subsystems/intake/IntakeArmIOSpark.java | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4b96f956..35f931cf 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -159,8 +159,8 @@ public void retractArm() { /** True once both arms are seeded and measured within tolerance of the stowed position. */ public boolean isStowed() { return armSeeded - && MathUtil.isNear(STOWED_POS_RAD, leftArmInputs.position, STOWED_TOLERANCE_RAD) - && MathUtil.isNear(STOWED_POS_RAD, rightArmInputs.position, STOWED_TOLERANCE_RAD); + && MathUtil.isNear(STOWED_POS_RAD, leftArmInputs.positionRad, STOWED_TOLERANCE_RAD) + && MathUtil.isNear(STOWED_POS_RAD, rightArmInputs.positionRad, STOWED_TOLERANCE_RAD); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index ca02862b..bee116db 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -9,8 +9,8 @@ public interface IntakeArmIO { @AutoLog public static class IntakeArmIOInputs { public boolean connected = false; - public double position = 0.0; - public double velocityMetersPerSec = 0.0; + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; public double currentAmps = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index a70d341e..49dc791f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -89,8 +89,8 @@ public void updateInputs(IntakeArmIOInputs inputs) { // Update inputs inputs.connected = true; - inputs.position = motorSim.getPosition(); - inputs.velocityMetersPerSec = motorSim.getVelocity(); + inputs.positionRad = motorSim.getPosition(); + inputs.velocityRadPerSec = motorSim.getVelocity(); inputs.appliedVolts = motorSim.getAppliedOutput() * motorSim.getBusVoltage(); inputs.currentAmps = Math.abs(motorSim.getMotorCurrent()); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 0fbe1175..8318da1d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -96,8 +96,8 @@ public IntakeArmIOSpark(ArmConfig armConfig) { @Override public void updateInputs(IntakeArmIOInputs inputs) { - inputs.position = sparkInputs.getPosition(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity(); + inputs.positionRad = sparkInputs.getPosition(); + inputs.velocityRadPerSec = sparkInputs.getVelocity(); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); From c0b981249647ac5e0b0152039ded4a3c5b213e13 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Wed, 5 Aug 2026 11:46:26 -0400 Subject: [PATCH 28/29] Document the 2x-measurement convention for ARM_LENGTH_METERS SingleJointedArmSim hardcodes the center of mass at length/2. For a mass concentrated near the roller end rather than a uniform rod, the value to plug in is 2x the measured pivot-to-mass-concentration distance, not the raw measurement. --- .../java/frc/robot/subsystems/intake/IntakeConstants.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 68382984..88e57cbe 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -84,6 +84,11 @@ public static class ArmConstants { // Sim physics. SingleJointedArmSim models the arm as a uniform rod pivoting at one end, // giving it the same cos(position) gravity torque that kG compensates for on the real arm. public static final double MOMENT_OF_INERTIA_KG_M2 = 0.004; // assumed, tune on robot + + // SingleJointedArmSim hardcodes the center of mass at ARM_LENGTH_METERS / 2, so if the + // real mass is concentrated (e.g. at the roller end) rather than spread evenly like a + // uniform rod, set this to 2x the measured pivot-to-mass-concentration distance so the + // sim's assumed center of mass lands at the real one. public static final double ARM_LENGTH_METERS = 0.4; // ~16in — assumed placeholder, tune on robot From 3e916011ec7adacfa0c9fd34dcd4c43646e925e6 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sat, 15 Aug 2026 16:40:36 -0400 Subject: [PATCH 29/29] Fix intake arm sensor calibration and seeding The right arm never had an absolute encoder wired up, so it was reading noise from a floating input; stop constructing one for it. The boot-time seed could grab a stale first CAN frame instead of a settled reading, so gate it behind a discard-then-moving-average settle window. Swap the left/right motor inversion flags, which had the relative encoders running backwards, and recalibrate absEncoderOffset/minPosRad/maxPosRad from bench measurements (five-second holds at stowed, horizontal, and deployed) now that the sensors read correctly. Update the arm sim to match: start stowed like the real robot, and report absolutePosition only where the real IO does. Co-Authored-By: Claude Sonnet 5 --- .../frc/robot/subsystems/intake/Intake.java | 37 +++++++++++++------ .../intake/IntakeArmIOSimSpark.java | 14 ++++++- .../subsystems/intake/IntakeArmIOSpark.java | 27 ++++++++++---- .../subsystems/intake/IntakeConstants.java | 26 ++++++++++--- 4 files changed, 76 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 35f931cf..0b3c40b6 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -7,6 +7,7 @@ import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.Alert; @@ -45,8 +46,11 @@ public class Intake extends SubsystemBase { private State armSetpoint = new State(STOWED_POS_RAD, 0.0); // Only the left arm's Spark has an absolute encoder wired up. Both arms' relative encoders, - // and the motion profile itself, are seeded from that one reading the first time it's valid. + // and the motion profile itself, are seeded from that one reading once it's settled — its + // first CAN frame after connecting can be a stale default rather than a real sample. private boolean armSeeded = false; + private final LinearFilter armSeedFilter = LinearFilter.movingAverage(ARM_SEED_SETTLE_SAMPLES); + private int armSeedSampleCount = 0; // Injected after both subsystems are created to avoid a circular dependency. // When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed. @@ -99,18 +103,27 @@ public void periodic() { Logger.recordOutput("Faults/Intake/RightArmDisconnected", !rightArmInputs.connected); // Seed both relative encoders, and the motion profile, from the left arm's absolute encoder - // once it reports connected. Runs once at boot. The reading is clamped into the soft limit - // range so a miscalibrated absEncoderOffset can't seed a position the arm can never leave; - // armSeedOutOfRangeAlert flags when that clamp actually did something. + // once it reports connected and its reading has settled. The first CAN frame(s) after + // connecting can be a stale default rather than a real sample, so those are discarded outright + // — never fed to the moving average — and only samples known to be past that go into the + // average the seed actually trusts. The settled reading is clamped into the soft limit range + // so a miscalibrated absEncoderOffset can't seed a position the arm can never leave; + // armSeedOutOfRangeAlert flags when that clamp did something. if (!armSeeded && leftArmInputs.connected) { - double rawSeedPositionRad = leftArmInputs.absolutePosition.getRadians(); - double seedPositionRad = MathUtil.clamp(rawSeedPositionRad, minPosRad, maxPosRad); - armSeedOutOfRangeAlert.set(seedPositionRad != rawSeedPositionRad); - leftArmIO.resetEncoder(Radians.of(seedPositionRad)); - rightArmIO.resetEncoder(Radians.of(seedPositionRad)); - armGoal = new State(seedPositionRad, 0.0); - armSetpoint = new State(seedPositionRad, 0.0); - armSeeded = true; + armSeedSampleCount++; + if (armSeedSampleCount > ARM_SEED_DISCARD_SAMPLES) { + double filteredSeedPositionRad = + armSeedFilter.calculate(leftArmInputs.absolutePosition.getRadians()); + if (armSeedSampleCount >= ARM_SEED_DISCARD_SAMPLES + ARM_SEED_SETTLE_SAMPLES) { + double seedPositionRad = MathUtil.clamp(filteredSeedPositionRad, minPosRad, maxPosRad); + armSeedOutOfRangeAlert.set(seedPositionRad != filteredSeedPositionRad); + leftArmIO.resetEncoder(Radians.of(seedPositionRad)); + rightArmIO.resetEncoder(Radians.of(seedPositionRad)); + armGoal = new State(seedPositionRad, 0.0); + armSetpoint = new State(seedPositionRad, 0.0); + armSeeded = true; + } + } } // Advance the arm motion profile and drive both arms to the resulting setpoint. Commands diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java index 49dc791f..460fdb75 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +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.wpilibj.simulation.RoboRioSim; @@ -31,11 +32,13 @@ public class IntakeArmIOSimSpark implements IntakeArmIO { private final SparkMax motor; private final SparkClosedLoopController controller; private final SparkMaxSim motorSim; + private final boolean hasAbsoluteEncoder; private final SparkMaxConfig motorConfig; public IntakeArmIOSimSpark(ArmConfig armConfig) { motor = new SparkMax(armConfig.port(), MotorType.kBrushless); + hasAbsoluteEncoder = armConfig.hasAbsoluteEncoder(); controller = motor.getClosedLoopController(); @@ -64,6 +67,7 @@ public IntakeArmIOSimSpark(ArmConfig armConfig) { motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); motorSim = new SparkMaxSim(motor, gearbox); + // Starts stowed (maxPosRad), matching where the real arm sits at boot. armSim = new SingleJointedArmSim( gearbox, @@ -73,9 +77,9 @@ public IntakeArmIOSimSpark(ArmConfig armConfig) { minPosRad, maxPosRad, true, - 0.0); + maxPosRad); - motorSim.setPosition(0.0); + motorSim.setPosition(maxPosRad); } @Override @@ -93,6 +97,12 @@ public void updateInputs(IntakeArmIOInputs inputs) { inputs.velocityRadPerSec = motorSim.getVelocity(); inputs.appliedVolts = motorSim.getAppliedOutput() * motorSim.getBusVoltage(); inputs.currentAmps = Math.abs(motorSim.getMotorCurrent()); + + if (hasAbsoluteEncoder) { + // No offset/calibration to simulate — the sim arm's position is already ground truth, so + // it's reported directly, matching the real absolute encoder's convention. + inputs.absolutePosition = new Rotation2d(motorSim.getPosition()); + } } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java index 8318da1d..b9d9e0bf 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -43,17 +43,20 @@ public class IntakeArmIOSpark implements IntakeArmIO { public IntakeArmIOSpark(ArmConfig armConfig) { motor = new SparkMax(armConfig.port(), MotorType.kBrushless); - absEncoder = motor.getAbsoluteEncoder(); + absEncoder = armConfig.hasAbsoluteEncoder() ? motor.getAbsoluteEncoder() : null; relEncoder = motor.getEncoder(); controller = motor.getClosedLoopController(); absEncoderConfig = new AbsoluteEncoderConfig(); - absEncoderConfig - .inverted(true) - .zeroOffset(absEncoderOffset) - .positionConversionFactor(absEncoderPositionFactor) - .velocityConversionFactor(absEncoderVelocityFactor); + if (armConfig.hasAbsoluteEncoder()) { + // AbsoluteEncoderConfig.inverted() only applies in brushed mode (see its javadoc); this + // is a brushless NEO. + absEncoderConfig + .zeroOffset(absEncoderOffset) + .positionConversionFactor(absEncoderPositionFactor) + .velocityConversionFactor(absEncoderVelocityFactor); + } motorConfig = new SparkMaxConfig(); @@ -68,7 +71,9 @@ public IntakeArmIOSpark(ArmConfig armConfig) { .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - motorConfig.absoluteEncoder.apply(absEncoderConfig); + if (armConfig.hasAbsoluteEncoder()) { + motorConfig.absoluteEncoder.apply(absEncoderConfig); + } motorConfig .closedLoop @@ -102,7 +107,13 @@ public void updateInputs(IntakeArmIOInputs inputs) { inputs.currentAmps = sparkInputs.getOutputCurrent(); inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); - inputs.absolutePosition = new Rotation2d(absEncoder.getPosition()); + if (absEncoder != null) { + // The absolute encoder is a separate physical sensor from the relative encoder, so + // armConfig.inverted() (which flips the relative encoder's sign, along with motor output) + // has no effect on it. Measured on the bench: absEncoder.getPosition() decreases as the + // arm moves from stowed to deployed, matching the relative encoder's convention. + inputs.absolutePosition = new Rotation2d(absEncoder.getPosition()); + } } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 88e57cbe..f5d5b2e7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -62,14 +62,27 @@ public static class ArmConstants { public static final double absEncoderPositionFactor = 2 * Math.PI; public static final double absEncoderVelocityFactor = (2 * Math.PI) / 60.0; - public static final double absEncoderOffset = 0; + // Measured on the bench: raw absolute-encoder reading at horizontal (0 rad — see the kG + // feedforward comment below), in rotations. Averaged from two 5-second pauses at horizontal, + // approached from opposite directions, which agreed within ~7° of each other. + public static final double absEncoderOffset = 0.7297; + + // Seed settling. The absolute encoder's first CAN frame(s) after connecting can be a stale + // default rather than a real reading, so those samples are discarded outright — never fed to + // the moving average — before the average starts filling on samples known to be past that. + public static final int ARM_SEED_DISCARD_SAMPLES = 5; // 0.1s @ 50Hz + public static final int ARM_SEED_SETTLE_SAMPLES = 25; // 0.5s @ 50Hz, after the discard public static final DCMotor gearbox = DCMotor.getNEO(2); public static final AngularVelocity maxAngularVelocity = RadiansPerSecond.of(gearbox.freeSpeedRadPerSec / motorReduction); - public static final double maxPosRad = Degrees.of(110.0).in(Radians); - public static final double minPosRad = Degrees.of(0.0).in(Radians); + // Measured on the bench with the absolute encoder, relative to horizontal (0): the stowed and + // deployed hardstops, each held for 5 seconds. Deployed sits past horizontal, not at it — the + // gravity feedforward's zero reference is a physical fact about the mechanism, not a hardstop. + // No margin included — these are the exact hand-measured hardstop positions. + public static final double maxPosRad = Degrees.of(81.0).in(Radians); + public static final double minPosRad = Degrees.of(-51.7).in(Radians); // Motion profile public static final double PROFILE_MAX_VELOCITY = maxAngularVelocity.in(RadiansPerSecond); @@ -98,11 +111,12 @@ public static class ArmConstants { public static final double DEPLOYED_POS_RAD = minPosRad; // Configs - public record ArmConfig(int port, CANBus bus, boolean inverted) {} + // Only the left arm's Spark has an absolute encoder wired up; see Intake's seeding comment. + public record ArmConfig(int port, CANBus bus, boolean inverted, boolean hasAbsoluteEncoder) {} public static final ArmConfig LEFT_ARM_CONFIG = - new ArmConfig(CAN2.INTAKE_ARM_LEFT, CAN2.BUS, true); + new ArmConfig(CAN2.INTAKE_ARM_LEFT, CAN2.BUS, false, true); public static final ArmConfig RIGHT_ARM_CONFIG = - new ArmConfig(CAN2.INTAKE_ARM_RIGHT, CAN2.BUS, false); + new ArmConfig(CAN2.INTAKE_ARM_RIGHT, CAN2.BUS, true, false); } }