arm subsystem

src/main/java/org/frc6423/lib/subsystems/Roller.java

@@ -0,0 +1,111 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.lib.subsystems;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.function.DoubleSupplier;
+
+/** Generic Roller Subsystem */
+public class Roller extends SubsystemBase implements AutoCloseable {
+  @Logged(name = "Roller Hardware Loggables")
+  private final RollerIO hardware;
+
+  // TODO check value
+  private LinearFilter currentFilter = LinearFilter.movingAverage(3);
+
+  @Logged(name = "Filtered Roller Motor Current (Amps)")
+  private double filteredCurrent = 0.0;
+
+  public Roller(String name, RollerIO hardware) {
+    this.setName(name);
+    this.hardware = hardware;
+  }
+
+  @Override
+  public void periodic() {
+    hardware.periodic();
+
+    filteredCurrent = currentFilter.calculate(hardware.getStatorCurrentAmps());
+  }
+
+  /**
+   * @param minStallAmps minimum roller motor stator current considered stalling in amps
+   * @return true if roller motor is stalling
+   */
+  protected boolean isStalling(double minStallAmps) {
+    return Math.abs(filteredCurrent) > 30.0;
+  }
+
+  protected void setCurrentFilterTaps(int taps) {
+    currentFilter = LinearFilter.movingAverage(taps);
+  }
+
+  /**
+   * Run roller at specified volts
+   *
+   * @param volts desired volts
+   * @return {@link Command}
+   */
+  protected Command runVolts(DoubleSupplier volts) {
+    return this.run(() -> hardware.setVolts(volts.getAsDouble()));
+  }
+
+  /**
+   * Run roller at specified volts
+   *
+   * @param volts desired volts
+   * @return {@link Command}
+   */
+  protected Command runVolts(double volts) {
+    return runVolts(() -> volts);
+  }
+
+  /**
+   * Run roller at specified speed
+   *
+   * @param speedRpm desired speed in revs per minute
+   * @return {@link Command}
+   */
+  protected Command runSpeed(DoubleSupplier speedRpm) {
+    return this.run(() -> hardware.setSpeed(speedRpm.getAsDouble()));
+  }
+
+  /**
+   * Run roller at specified speed
+   *
+   * @param speedRpm desired speed in revs per minute
+   * @return {@link Command}
+   */
+  protected Command runSpeed(double speedRpm) {
+    return runSpeed(() -> speedRpm);
+  }
+
+  /**
+   * Hold rollers at current speed
+   *
+   * @return {@link Command}
+   */
+  protected Command holdSpeed() {
+    return Commands.sequence(
+        this.run(
+                () -> {
+                  var currentSpeed = hardware.getSpeedRpm();
+                  hardware.setSpeed(currentSpeed);
+                })
+            .until(() -> true),
+        this.run(() -> {}));
+  }
+
+  @Override
+  public void close() throws Exception {
+    hardware.close();
+  }
+}

src/main/java/org/frc6423/lib/subsystems/RollerIO.java

@@ -0,0 +1,44 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.lib.subsystems;
+
+import edu.wpi.first.epilogue.Logged;
+
+/** Generalized hardware methods for {@link Roller} subsystem */
+public interface RollerIO extends AutoCloseable {
+  /** Run periodic hardware logic */
+  public void periodic();
+
+  @Logged(name = "Roller Applied Voltage")
+  public double getAppliedVolts();
+
+  /**
+   * @return speed of roller motor in revs per minute
+   */
+  @Logged(name = "Roller Motor Speed (Revs Per Minute)")
+  public double getSpeedRpm();
+
+  /**
+   * @return stator current of roller motor in amps
+   */
+  @Logged(name = "Roller Motor Stator Current (Amps)")
+  public double getStatorCurrentAmps();
+
+  /**
+   * Set roller motor voltage setpoint
+   *
+   * @param volts desired voltage setpoint
+   */
+  public void setVolts(double volts);
+
+  /**
+   * Set roller motor speed setpoint
+   *
+   * @param speedRpm desired speed setpoint in revs per minute
+   */
+  public void setSpeed(double speedRpm);
+}

src/main/java/org/frc6423/lib/subsystems/RollerIONone.java

@@ -0,0 +1,37 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.lib.subsystems;
+
+/** Null {@link RollerIO} */
+public class RollerIONone implements RollerIO {
+  @Override
+  public void periodic() {}
+
+  @Override
+  public double getAppliedVolts() {
+    return 0.0;
+  }
+
+  @Override
+  public double getSpeedRpm() {
+    return 0.0;
+  }
+
+  @Override
+  public double getStatorCurrentAmps() {
+    return 0.0;
+  }
+
+  @Override
+  public void setVolts(double volts) {}
+
+  @Override
+  public void setSpeed(double speedRpm) {}
+
+  @Override
+  public void close() throws Exception {}
+}

src/main/java/org/frc6423/robot/Constants.java

@@ -24,7 +24,7 @@ public class Flags {
     public static final Time PERIOD = Seconds.of(0.02);
 
     /** When true, the robot will enable debug menus and log debug information */
-    public static final boolean debugMode = false;
+    @Deprecated public static final boolean debugMode = false;
   }
 
   /** Constants representing port IDs that devices are connected to */

src/main/java/org/frc6423/robot/Robot.java

@@ -19,10 +19,10 @@
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import org.frc6423.lib.drivers.CommandRobot;
 import org.frc6423.robot.Constants.Flags;
 
@@ -35,11 +35,11 @@
 @Logged
 public class Robot extends CommandRobot {
   // * IO INIT
-  private final XboxController driverController = new XboxController(0);
-  private final XboxController operatorController = new XboxController(1);
+  private final CommandXboxController driverController = new CommandXboxController(0);
+  private final CommandXboxController operatorController = new CommandXboxController(1);
 
   // * SUBSYSTEM INIT
-  /** Replace this line with subsystem declerations */
+  /** Init subsytems here */
 
   // * ALERT INIT
   private final Alert batteryBrownout = new Alert("Battery voltage output low", AlertType.kWarning);
@@ -84,21 +84,16 @@ public Robot() {
             config.errorHandler = ErrorHandler.crashOnError();
           }
 
-          if (Flags.debugMode || isSimulation()) {
-            // Log everything
-            config.minimumImportance = Logged.Importance.DEBUG;
-          } else {
-            // Only log information
-            config.minimumImportance = Logged.Importance.INFO;
-          }
+          // Log everything
+          config.minimumImportance = Logged.Importance.DEBUG;
         });
     Epilogue.bind(this);
 
     // Update drive dashboard periodically
     addPeriodic(this::updateDashboard, 0.02);
 
-    configureBindings();
     configureGameBehavior();
+    configureBindings();
   }
 
   /** Update all driver dashboard values on NetworkTables */
@@ -115,12 +110,12 @@ private void updateDashboard() {
     operatorDisconnected.set(!operatorController.isConnected());
   }
 
-  /** Configure all Driver & Operator controller bindings */
-  private void configureBindings() {}
-
   /** Configure behavior during different match sections */
   private void configureGameBehavior() {}
 
+  /** Configure all Driver & Operator controller bindings */
+  private void configureBindings() {}
+
   @Override
   protected Command getAutonCommand() {
     // TODO Replace placeholder

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/Arm.java

@@ -0,0 +1,70 @@
+// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
+// https://github.com/wmironpatriots
+// 
+// Open Source Software; you can modify and/or share it under the terms of
+// MIT license file in the root directory of this project
+
+package org.frc6423.robot.subsystems.superstructure.arm;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.frc6423.lib.subsystems.RollerIO;
+
+/** Arm Subsystem */
+public class Arm extends SubsystemBase {
+  @Logged(name = "Pivot Subsystem-Component")
+  private final ArmPivot pivot;
+
+  @Logged(name = "Roller Subsystem-Component")
+  private final ArmRoller roller;
+
+  @Logged(name = "Arm Setpoint State")
+  private ArmState setpointState = ArmState.STOWED;
+
+  public Arm(ArmPivotIO pivotHardware, RollerIO rollerHardware) {
+    this.pivot = new ArmPivot(pivotHardware);
+    this.roller = new ArmRoller(rollerHardware);
+  }
+
+  @Override
+  public void periodic() {}
+
+  /**
+   * @return true if arm is within a certain tolerance of the setpoint angle
+   */
+  public boolean isNearSetpointAngle() {
+    return pivot.isNearSetpointAngle();
+  }
+
+  /**
+   * @return true if arm has vectored coral piece
+   */
+  public boolean hasCoralVectored() {
+    return roller.isStalling();
+  }
+
+  /**
+   * Run arm to specified {@link ArmState}
+   *
+   * @param state desired {@link ArmState}
+   * @return {@link Command}
+   */
+  public Command runState(ArmState state) {
+    return Commands.parallel(
+        this.run(() -> setpointState = state).until(() -> true),
+        pivot.runAngle(state.angle.getMeasure()),
+        roller.runSpeed(state.rollerSpeedRpm));
+  }
+
+  /**
+   * Hold arm at current angle and roller speed
+   *
+   * @return {@link Command}
+   */
+  public Command holdState() {
+    return Commands.parallel(
+        this.run(() -> {}).until(() -> true), pivot.holdAngle(), roller.holdSpeed());
+  }
+}