Replace pneumatic intake arm deploy with motorized NEO control

src/main/java/frc/robot/Constants.java

@@ -103,6 +103,8 @@ public static final class CAN2 {
       // Intake
       public static final int INTAKE_ROLLER_LOWER = 22;
       public static final int INTAKE_ROLLER_UPPER = 23;
+      public static final int INTAKE_ARM_RIGHT = 27;
+      public static final int INTAKE_ARM_LEFT = 26;
     }
 
     public static final class CANHD {

src/main/java/frc/robot/Robot.java

@@ -7,12 +7,10 @@
 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.event.EventLoop;
 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;
@@ -33,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.CANBusPorts.CAN2;
@@ -68,9 +65,10 @@
 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.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;
@@ -139,8 +137,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,9 +197,9 @@ public Robot() {
             new Intake(
                 new RollerIOSpark(RollerConstants.UPPER_ROLLER_CONFIG),
                 new RollerIOSpark(RollerConstants.LOWER_ROLLER_CONFIG),
-                new IntakeArmIOReal());
+                new IntakeArmIOSpark(ArmConstants.LEFT_ARM_CONFIG),
+                new IntakeArmIOSpark(ArmConstants.RIGHT_ARM_CONFIG));
         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();
@@ -239,14 +235,12 @@ public Robot() {
                 new HoodIOSimSpark());
         feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark());
         if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOSim());
-        var intakeArmIOSim = new IntakeArmIOSim();
         intake =
             new Intake(
                 new RollerIOSimSpark(RollerConstants.UPPER_ROLLER_CONFIG),
                 new RollerIOSimSpark(RollerConstants.LOWER_ROLLER_CONFIG),
-                intakeArmIOSim);
-        pneumaticsSimulator =
-            new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1));
+                new IntakeArmIOSimSpark(ArmConstants.LEFT_ARM_CONFIG),
+                new IntakeArmIOSimSpark(ArmConstants.RIGHT_ARM_CONFIG));
         break;
 
       case REPLAY: // Replaying a log
@@ -281,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;
     }
@@ -348,7 +344,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();
 
@@ -437,9 +432,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. */
@@ -466,11 +458,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(
@@ -480,7 +469,6 @@ public void simulationPeriodic() {
                     launcher.getSimCurrentDrawAmps(),
                     feeder.getSimCurrentDrawAmps(),
                     intake.getSimCurrentDrawAmps(),
-                    pneumaticsSimulator.getCompressorCurrentAmps(),
                     ELECTRONICS_OVERHEAD_AMPS))));
   }
 

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -1,62 +1,143 @@
 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.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;
 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;
 import frc.robot.Constants;
+import frc.robot.Robot;
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 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 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 =
+      new TrapezoidProfile(
+          new TrapezoidProfile.Constraints(PROFILE_MAX_VELOCITY, PROFILE_MAX_ACCELERATION));
+  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 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.
   private BooleanSupplier hopperIsDeployed;
   private Supplier<Command> 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);
+    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"
+                + " absEncoderOffset",
+            AlertType.kWarning);
   }
 
   @Override
   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);
+    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/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 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) {
+      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
+    // never set arm position directly — they only move armGoal, and this is the sole place
+    // 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) {
@@ -77,23 +158,22 @@ public void periodic() {
   public void stop() {
     upperRollerIO.setOpenLoop(Volts.of(0.0));
     lowerRollerIO.setOpenLoop(Volts.of(0.0));
-    intakeArmIO.retract();
+    armGoal = new State(STOWED_POS_RAD, 0.0);
   }
 
   public void deployArm() {
-    intakeArmIO.deploy();
+    armGoal = new State(DEPLOYED_POS_RAD, 0.0);
   }
 
   public void retractArm() {
-    intakeArmIO.retract();
-  }
-
-  public Boolean isDeployed() {
-    return intakeArmInputs.isDeployed == DoubleSolenoid.Value.kForward;
+    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 intakeArmInputs.isDeployed == DoubleSolenoid.Value.kReverse;
+    return armSeeded
+        && MathUtil.isNear(STOWED_POS_RAD, leftArmInputs.positionRad, STOWED_TOLERANCE_RAD)
+        && MathUtil.isNear(STOWED_POS_RAD, rightArmInputs.positionRad, STOWED_TOLERANCE_RAD);
   }
 
   /**
@@ -135,7 +215,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() {

src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java

@@ -1,17 +1,25 @@
 package frc.robot.subsystems.intake;
 
-import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
 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 positionRad = 0.0;
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
+    public double currentAmps = 0.0;
+
+    public Rotation2d absolutePosition = Rotation2d.kZero;
   }
 
   public default void updateInputs(IntakeArmIOInputs inputs) {}
 
-  public default void deploy() {}
+  public default void setPosition(Angle rotation, AngularVelocity velocity) {}
 
-  public default void retract() {}
+  public default void resetEncoder(Angle position) {}
 }