refactor: WIP.

src/main/java/frc/robot/arm/Arm.java

@@ -1,27 +1,63 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
+import frc.lib.config.AbsoluteEncoderConfig;
+import frc.lib.config.FeedbackControllerConfig;
+import frc.lib.config.FeedforwardControllerConfig;
+import frc.lib.config.MechanismConfig;
+import frc.lib.config.MotionProfileConfig;
+import frc.lib.config.MotorConfig;
 import frc.lib.controller.PositionControllerIO;
 import frc.lib.controller.PositionControllerIO.PositionControllerIOValues;
-import frc.robot.arm.ArmConstants.ShoulderConstants;
 
 /** Arm subsystem. */
 public class Arm extends Subsystem {
 
   /** Arm subsystem singleton. */
   private static Arm instance = null;
 
+  /** Shoulder's config. */
+  private final MechanismConfig shoulderConfig =
+      new MechanismConfig()
+          .withAbsoluteEncoderConfig(
+              new AbsoluteEncoderConfig()
+                  .withCCWPositive(false)
+                  .withOffset(Rotation2d.fromDegrees(-173.135)))
+          .withMotorConfig(
+              new MotorConfig()
+                  .withCCWPositive(true)
+                  .withNeutralBrake(true)
+                  .withMotorToMechanismRatio(39.771428571))
+          .withMotionProfileConfig(
+              new MotionProfileConfig()
+                  .withMaximumVelocity(Units.degreesToRotations(240.0)) // rotations per second
+                  .withMaximumAcceleration(Units.degreesToRadians(240.0)) // rotations per second
+              )
+          .withFeedforwardConfig(
+              new FeedforwardControllerConfig()
+                  .withStaticFeedforward(0.14) // volts
+                  .withGravityFeedforward(0.5125) // volts
+                  .withVelocityFeedforward(4.0) // volts per rotation per second
+              )
+          .withFeedbackConfig(
+              new FeedbackControllerConfig().withProportionalGain(4.0) // volts per rotation
+              );
+
   /** Shoulder controller. */
   private final PositionControllerIO shoulder;
 
   /** Shoulder controller values. */
   private final PositionControllerIOValues shoulderValues;
 
+  /** Motion profile of the shoulder. */
+  private final TrapezoidProfile shoulderMotionProfile;
+
   /** Arm's goal. Set by superstructure. */
   private ArmState goal;
 
@@ -36,11 +72,13 @@ public class Arm extends Subsystem {
 
   /** Creates the arm subsystem and configures arm hardware. */
   private Arm() {
-    shoulder = ArmFactory.createShoulder();
+    shoulder = ArmFactory.createShoulder(shoulderConfig);
     shoulder.configure();
 
     shoulderValues = new PositionControllerIOValues();
 
+    shoulderMotionProfile = shoulderConfig.motionProfileConfig().createTrapezoidProfile();
+
     previousTimeSeconds = Timer.getFPGATimestamp();
 
     setPosition(ArmState.STOW);
@@ -72,7 +110,7 @@ public void periodic() {
 
     setpoint =
         new ArmState(
-            ShoulderConstants.MOTION_PROFILE.calculate(
+            shoulderMotionProfile.calculate(
                 timeElapsedSeconds, setpoint.shoulderRotations(), goal.shoulderRotations()));
 
     shoulder.setSetpoint(

src/main/java/frc/robot/arm/ArmFactory.java

@@ -1,12 +1,13 @@
 package frc.robot.arm;
 
+import frc.lib.CAN;
+import frc.lib.config.MechanismConfig;
 import frc.lib.controller.PositionControllerIO;
 import frc.lib.controller.PositionControllerIOSim;
 import frc.lib.controller.PositionControllerIOTalonFX2;
 import frc.robot.Robot;
 import frc.robot.RobotConstants;
 import frc.robot.RobotConstants.Subsystem;
-import frc.robot.arm.ArmConstants.ShoulderConstants;
 
 /** Helper class for creating hardware for the arm subsystem. */
 public class ArmFactory {
@@ -16,15 +17,10 @@ public class ArmFactory {
    *
    * @return a shoulder motor.
    */
-  public static PositionControllerIO createShoulder() {
+  public static PositionControllerIO createShoulder(MechanismConfig config) {
     if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) {
       return new PositionControllerIOTalonFX2(
-          ShoulderConstants.LEADER_CAN,
-          ShoulderConstants.FOLLOWER_CAN,
-          ShoulderConstants.ENCODER_CAN,
-          ShoulderConstants.CONFIG,
-          false,
-          true);
+          new CAN(48), new CAN(46), new CAN(52), config, false, true);
     }
 
     return new PositionControllerIOSim();

src/main/java/frc/robot/arm/ArmState.java

@@ -4,22 +4,21 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
-import frc.robot.arm.ArmConstants.ShoulderConstants;
 import java.util.Objects;
 
 public record ArmState(State shoulderRotations) {
 
-  public static final ArmState STOW = new ArmState(ShoulderConstants.STOW_ANGLE);
+  public static final ArmState STOW = new ArmState(Rotation2d.fromDegrees(-26));
 
-  public static final ArmState SUBWOOFER = new ArmState(ShoulderConstants.STOW_ANGLE);
+  public static final ArmState SUBWOOFER = new ArmState(Rotation2d.fromDegrees(-26));
 
-  public static final ArmState EJECT = new ArmState(ShoulderConstants.FLAT_ANGLE);
+  public static final ArmState EJECT = new ArmState(Rotation2d.fromDegrees(30));
 
-  public static final ArmState SKIM = new ArmState(ShoulderConstants.FLAT_ANGLE);
+  public static final ArmState SKIM = new ArmState(Rotation2d.fromDegrees(30));
 
-  public static final ArmState LOB = new ArmState(ShoulderConstants.STOW_ANGLE);
+  public static final ArmState LOB = new ArmState(Rotation2d.fromDegrees(-26));
 
-  public static final ArmState AMP = new ArmState(ShoulderConstants.AMP_ANGLE);
+  public static final ArmState AMP = new ArmState(Rotation2d.fromDegrees(60));
 
   public ArmState {
     Objects.requireNonNull(shoulderRotations);

src/main/java/frc/robot/intake/IntakeConstants.java

@@ -4,6 +4,7 @@
 import frc.lib.config.FeedbackControllerConfig;
 import frc.lib.config.FeedforwardControllerConfig;
 import frc.lib.config.MechanismConfig;
+import frc.lib.config.MotionProfileConfig;
 import frc.lib.config.MotorConfig;
 
 /** Constants for the intake subsystem. */
@@ -21,6 +22,9 @@ public static class FrontRollerConstants {
                     .withCCWPositive(false)
                     .withNeutralBrake(false)
                     .withMotorToMechanismRatio(24.0 / 16.0))
+            .withMotionProfileConfig(
+                new MotionProfileConfig().withMaximumVelocity(66) // rotations per second
+                )
             .withFeedforwardConfig(
                 new FeedforwardControllerConfig()
                     .withStaticFeedforward(0.13) // volts
@@ -31,13 +35,6 @@ public static class FrontRollerConstants {
                     .withProportionalGain(0.1) // volts per rotation per second
                 );
 
-    public static final double INTAKE_SPEED = 34;
-
-    public static final double EJECT_SPEED = -34;
-
-    /** Maximum speed of the roller in rotations per second. */
-    public static final double MAXIMUM_SPEED = 67;
-
     public static final double NOTE_AMPS = 40;
   }
 
@@ -53,6 +50,9 @@ public static class BackRollerConstants {
                     .withCCWPositive(false)
                     .withNeutralBrake(false)
                     .withMotorToMechanismRatio(24.0 / 16.0))
+            .withMotionProfileConfig(
+                new MotionProfileConfig().withMaximumVelocity(66) // rotations per second
+                )
             .withFeedforwardConfig(
                 new FeedforwardControllerConfig()
                     .withStaticFeedforward(0.13) // volts
@@ -63,13 +63,6 @@ public static class BackRollerConstants {
                     .withProportionalGain(0.1) // volts per rotation per second
                 );
 
-    public static final double INTAKE_SPEED = 34;
-
-    public static final double EJECT_SPEED = -34;
-
-    /** Maximum speed of the roller in rotations per second. */
-    public static final double MAXIMUM_SPEED = 67;
-
     public static final double NOTE_AMPS = 40;
   }
 }

src/main/java/frc/robot/intake/IntakeState.java

@@ -1,20 +1,16 @@
 package frc.robot.intake;
 
 import edu.wpi.first.math.MathUtil;
-import frc.robot.intake.IntakeConstants.BackRollerConstants;
-import frc.robot.intake.IntakeConstants.FrontRollerConstants;
 import java.util.Objects;
 
 public record IntakeState(
     double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) {
 
   public static final IntakeState IDLE = new IntakeState(0, 0);
 
-  public static final IntakeState INTAKE =
-      new IntakeState(FrontRollerConstants.INTAKE_SPEED, BackRollerConstants.INTAKE_SPEED);
+  public static final IntakeState INTAKE = new IntakeState((double) 34, (double) 34);
 
-  public static final IntakeState EJECT =
-      new IntakeState(FrontRollerConstants.EJECT_SPEED, BackRollerConstants.EJECT_SPEED);
+  public static final IntakeState EJECT = new IntakeState((double) -34, (double) -34);
 
   public IntakeState {
     Objects.requireNonNull(frontRollerVelocityRotationsPerSecond);