e

Author: dabeycorn

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

@@ -10,6 +10,7 @@
 import static edu.wpi.first.units.Units.Radians;
 
 import edu.wpi.first.epilogue.Logged;
+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.wpilibj2.command.Command;
@@ -23,6 +24,37 @@
 
 /** Arm Subsystem */
 public class Arm extends SubsystemBase {
+  /** Represents a state the {@link Arm} subsystem can be in */
+  // TODO CHECK ROLLER SPEEDS
+  public static enum ArmState {
+    /** Resting state */
+    STOWED(Rotation2d.fromDegrees(90), 0.0),
+    /** Avoidance state for preventing collisions */
+    AVOIDING(Rotation2d.fromDegrees(65), 0.0),
+    /** Flipped state for intaking */
+    INTAKING(Rotation2d.fromDegrees(-90), 20.0),
+    /** L2 pose but not scoring */
+    L2_PRIMED(Rotation2d.fromDegrees(21.975), 0.0),
+    /** L3 pose but not scoring */
+    L3_PRIMED(Rotation2d.fromDegrees(21.975), 0.0),
+    /** L4 pose but not scoring */
+    L4_PRIMED(Rotation2d.fromDegrees(74.5), 0.0),
+    /** L2 scoring */
+    L2_SCORING(Rotation2d.fromDegrees(21.975), -40.0),
+    /** L3 scoring */
+    L3_SCORING(Rotation2d.fromDegrees(21.975), -40.0),
+    /** L4 scoring */
+    L4_SCORING(Rotation2d.fromDegrees(74.5), -40.0);
+
+    public final Rotation2d angle;
+    public final double rollerSpeedRpm;
+
+    private ArmState(Rotation2d angle, double rollerSpeedRpm) {
+      this.angle = angle;
+      this.rollerSpeedRpm = rollerSpeedRpm;
+    }
+  }
+
   @Logged(name = "Pivot Subsystem-Component")
   private final ArmPivot pivot;
 
@@ -67,6 +99,13 @@ public boolean isNearSetpointAngle() {
     return pivot.isNearSetpointAngle();
   }
 
+  /**
+   * @return {@link Rotation2d} representing the arm angle
+   */
+  public Rotation2d getRotation2d() {
+    return pivot.getRotation2d();
+  }
+
   /**
    * @return true if arm has vectored coral piece
    */

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

@@ -10,22 +10,28 @@
 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 static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
 
 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.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.function.DoubleSupplier;
-import org.frc6423.robot.subsystems.superstructure.Visualizer;
 
 /** Pivot subsystem-component of the {@link Arm} Subsystem */
 public class ArmPivot extends SubsystemBase implements AutoCloseable {
-  // * CONSTANTS
+  /** Name of the CAN bus hardware is on */
   public static final String CANBUS = "RIO";
+
+  /** Pivot motor CAN ID */
   public static final int MOTOR_ID = 14;
 
   /** Gear ratio of the pivot gearbox */
@@ -46,18 +52,22 @@ public class ArmPivot extends SubsystemBase implements AutoCloseable {
   /** The max allowable angle error */
   public static final Angle TOLERANCE = Degrees.of(1.5);
 
+  /** The velocity limit of the arm's trapezoid profile */
+  public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(5.5);
+
+  /** The acceleration of the arm's trapezoid profile */
+  public static final AngularAcceleration MAX_ACCELERATION = RadiansPerSecondPerSecond.of(17);
+
   @Logged(name = "Arm Pivot Hardware Loggables")
   private final ArmPivotIO hardware;
 
-  private final LinearFilter pivotCurrentFilter = LinearFilter.movingAverage(5);
+  private final LinearFilter currentFilter = LinearFilter.movingAverage(5);
 
   @Logged(name = "Filted Pivot Motor Stator Current (Amps)")
-  private double filteredPivotCurrent;
+  private double filteredCurrent;
 
   private boolean isZeroed = false;
 
-  private final Visualizer visualizer = Visualizer.getInstance();
-
   public ArmPivot(ArmPivotIO hardware) {
     this.hardware = hardware;
   }
@@ -66,9 +76,7 @@ public ArmPivot(ArmPivotIO hardware) {
   public void periodic() {
     hardware.periodic();
 
-    filteredPivotCurrent = pivotCurrentFilter.calculate(hardware.getStatorCurrentAmps());
-
-    visualizer.setArmAngle(hardware.getAngleRads());
+    filteredCurrent = currentFilter.calculate(hardware.getStatorCurrentAmps());
   }
 
   /**
@@ -88,6 +96,14 @@ public boolean isNearSetpointAngle() {
         hardware.getSetpointAngleRads(), hardware.getAngleRads(), TOLERANCE.in(Radians));
   }
 
+  /**
+   * @return {@link Rotation2d} representing the pivot angle
+   */
+  @Logged(name = "Angle (Rotation2d)")
+  public Rotation2d getRotation2d() {
+    return Rotation2d.fromRadians(hardware.getAngleRads());
+  }
+
   /**
    * Run arm into hardstop to determine home (aka, zero)
    *
@@ -96,7 +112,7 @@ public boolean isNearSetpointAngle() {
   // TODO CHECK VALUES
   public Command runCurrentHoming() {
     return this.run(() -> hardware.setVolts(-2.5))
-        .until(() -> Math.abs(filteredPivotCurrent) > 50.0)
+        .until(() -> Math.abs(filteredCurrent) > 50.0)
         .finallyDo(
             (interupted) -> {
               if (!interupted) {

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

@@ -6,13 +6,16 @@
 
 package org.frc6423.robot.subsystems.superstructure.arm;
 
-import static edu.wpi.first.units.Units.MetersPerSecond;
-import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.CANBUS;
+import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.GEAR_REDUCTION;
+import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.MAX_ACCELERATION;
 import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.MAX_ANGLE;
+import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.MAX_VELOCITY;
 import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.MIN_ANGLE;
 import static org.frc6423.robot.subsystems.superstructure.arm.ArmPivot.MOTOR_ID;
-import static org.frc6423.robot.subsystems.superstructure.elevator.Elevator.*;
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -62,8 +65,8 @@ public ArmPivotIOReal() {
     conf.Slot0.kI = 0.0;
     conf.Slot0.kD = 0.0;
 
-    conf.MotionMagic.MotionMagicCruiseVelocity = MAX_VELOCITY.in(MetersPerSecond);
-    conf.MotionMagic.MotionMagicAcceleration = MAX_ACCELERATION.in(MetersPerSecondPerSecond);
+    conf.MotionMagic.MotionMagicCruiseVelocity = MAX_VELOCITY.in(RadiansPerSecond);
+    conf.MotionMagic.MotionMagicAcceleration = MAX_ACCELERATION.in(RadiansPerSecondPerSecond);
 
     motor.getConfigurator().apply(conf);