feat: create arm subsytem

Signed-off-by: Dasun Abeykoon <[email protected]>

Author: dabeycorn

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

@@ -6,158 +6,65 @@
 
 package org.frc6423.robot.subsystems.arm;
 
-import static edu.wpi.first.units.Units.Centimeters;
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.Inches;
-import static edu.wpi.first.units.Units.KilogramSquareMeters;
-import static edu.wpi.first.units.Units.Radians;
-
 import edu.wpi.first.epilogue.Logged;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.LinearFilter;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.units.measure.MomentOfInertia;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj.util.Color8Bit;
 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;
 
-public class Arm extends SubsystemBase implements AutoCloseable {
-  /** CONSTANTS */
-  public static final double PIVOT_GEARING = 50;
-
-  public static final double ROLLER_GEARING = 5.8677; // ! this is wrong
-
-  public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.001);
-  public static final Distance LENGTH = Inches.of(7.5);
-
-  public static final Angle MAX_ANGLE = Degrees.of(180);
-  public static final Angle TOLERANCE = Degrees.of(1.5);
-
-  @Logged(name = "Arm Hardware Loggables")
-  private final ArmIO hardware;
-
-  private final LinearFilter pivotCurrentFilter = LinearFilter.movingAverage(5);
-  private final LinearFilter rollerCurrentFilter = LinearFilter.movingAverage(3);
+/** Arm Subsystem */
+public class Arm extends SubsystemBase {
+  @Logged(name = "Pivot Subsystem-Component")
+  private final ArmPivot pivot;
 
-  @Logged(name = "Filted Pivot Motor Current (Amps)")
-  private double filteredPivotCurrent;
+  @Logged(name = "Roller Subsystem-Component")
+  private final ArmRoller roller;
 
-  @Logged(name = "Filted Roller Motor Current (Amps)")
-  private double filteredRollerCurrent;
+  @Logged(name = "Arm Setpoint State")
+  private ArmState setpointState = ArmState.STOWED;
 
-  @Logged(name = "Is Zeroed (bool)")
-  private boolean isZeroed = false;
-
-  private final Mechanism2d canvas =
-      new Mechanism2d(LENGTH.in(Centimeters), LENGTH.in(Centimeters) * 2);
-  private final MechanismRoot2d root =
-      canvas.getRoot("pivot", LENGTH.in(Centimeters), LENGTH.in(Centimeters));
-  private final MechanismLigament2d visualizer =
-      root.append(
-          new MechanismLigament2d(
-              "arm", LENGTH.in(Centimeters), 0.0, 10, new Color8Bit(Color.kAliceBlue)));
-
-  public Arm(ArmIO hardware) {
-    this.hardware = hardware;
-
-    SmartDashboard.putData("ArmVisualizer", canvas);
+  public Arm(ArmPivotIO pivotHardware, RollerIO rollerHardware) {
+    this.pivot = new ArmPivot(pivotHardware);
+    this.roller = new ArmRoller(rollerHardware);
   }
 
   @Override
-  public void periodic() {
-    hardware.periodic();
-
-    filteredPivotCurrent = pivotCurrentFilter.calculate(hardware.getPivotStatorCurrentAmps());
-    filteredRollerCurrent = rollerCurrentFilter.calculate(hardware.getRollerStatorCurrentAmps());
-
-    visualizer.setAngle(
-        Rotation2d.fromRadians(hardware.getPivotAngleRads()).plus(Rotation2d.kCCW_90deg));
-  }
+  public void periodic() {}
 
   /**
    * @return true if arm is within a certain tolerance of the setpoint angle
    */
-  @Logged(name = "Near Setpoint Angle (bool)")
-  public boolean nearSetpointAngle() {
-    return MathUtil.isNear(
-        hardware.getPivotSetpointAngleRads(), hardware.getPivotAngleRads(), TOLERANCE.in(Radians));
+  public boolean isNearSetpointAngle() {
+    return pivot.isNearSetpointAngle();
   }
 
   /**
-   * @return true when coral hits hardstop
+   * @return true if arm has vectored coral piece
    */
-  @Logged(name = "Coral Vectored (bool)")
-  public boolean coralVectored() {
-    // Checks if roller motors are stalled
-    // TODO check this value
-    return Math.abs(filteredRollerCurrent) > 30.0;
+  public boolean hasCoralVectored() {
+    return roller.isStalling();
   }
 
   /**
-   * Run arm backwards until it hits hardstop to determine zero
+   * Run arm to specified {@link ArmState}
    *
-   * @return {@link Command}
-   */
-  public Command runCurrentHoming() {
-    return this.run(
-            () -> {
-              hardware.runPivotVolts(-2.5);
-              // TODO check this value
-            })
-        .until(() -> Math.abs(filteredPivotCurrent) > 50.0)
-        .finallyDo(
-            (interupted) -> {
-              if (!interupted) {
-                hardware.resetPivotEncoder(0.0);
-                isZeroed = true;
-              }
-            });
-  }
-
-  /**
-   * Run Arm to specified {@link ArmState}
-   *
-   * @param state {@link ArmState} representing hardware setpoints
+   * @param state desired {@link ArmState}
    * @return {@link Command}
    */
   public Command runState(ArmState state) {
-    return this.run(
-        () -> {
-          hardware.runPivotAngle(state.angle.getRadians());
-          hardware.runRollerSpeed(state.rollerSpeedRpm);
-        });
+    return Commands.parallel(
+        this.run(() -> setpointState = state).until(() -> true),
+        pivot.runAngle(state.angle.getMeasure()),
+        roller.runSpeed(state.rollerSpeedRpm));
   }
 
   /**
-   * Hold Arm at specified {@link ArmState}
+   * Hold arm at current angle and roller speed
    *
-   * @param state {@link ArmState} representing hardware setpoints
    * @return {@link Command}
    */
-  public Command holdState(ArmState state) {
-    return Commands.sequence(
-        this.run(
-                () -> {
-                  var currentAngle = hardware.getPivotAngleRads();
-                  var currentSpeed = hardware.getRollerSpeedRpm();
-
-                  hardware.runPivotAngle(currentAngle);
-                  hardware.runRollerSpeed(currentSpeed);
-                })
-            .until(() -> true),
-        this.run(() -> {}));
-  }
-
-  @Override
-  public void close() throws Exception {
-    hardware.close();
+  public Command holdState() {
+    return Commands.parallel(
+        this.run(() -> {}).until(() -> true), pivot.holdAngle(), roller.holdSpeed());
   }
 }