diff --git a/simgui-ds.json b/simgui-ds.json
index f178b77..e664daf 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -6,18 +11,20 @@
           "decKey": 65,
           "incKey": 68
         },
-        {
-          "decKey": 87,
-          "incKey": 83
-        },
+        {},
         {
           "decKey": 69,
           "decayRate": 0.0,
           "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 83,
+          "incKey": 87
         }
       ],
-      "axisCount": 3,
+      "axisCount": 6,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -90,12 +97,10 @@
     }
   ],
   "robotJoysticks": [
+    {},
     {
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true
-    },
-    {
-      "useGamepad": true
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
index c01ca5a..4c7efc4 100644
--- a/simgui.json
+++ b/simgui.json
@@ -5,6 +5,7 @@
       "/Shuffleboard/Auto/Auto Chooser": "String Chooser",
       "/Shuffleboard/Auto/SendableChooser[0]": "String Chooser",
       "/Shuffleboard/Vision/Field": "Field2d",
+      "/SmartDashboard/Arm Mechanism": "Mechanism2d",
       "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": "Field2d"
     },
     "windows": {
@@ -13,6 +14,11 @@
           "visible": true
         }
       },
+      "/SmartDashboard/Arm Mechanism": {
+        "window": {
+          "visible": true
+        }
+      },
       "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": {
         "window": {
           "visible": true
@@ -23,6 +29,15 @@
   "NetworkTables": {
     "transitory": {
       "Shuffleboard": {
+        "Arm": {
+          "Position": {
+            "open": true
+          },
+          "Setpoint": {
+            "open": true
+          },
+          "open": true
+        },
         "Odometry": {
           "Velocity": {
             "open": true
diff --git a/src/main/java/frc/lib/ArmFeedforwardCalculator.java b/src/main/java/frc/lib/ArmFeedforwardCalculator.java
new file mode 100644
index 0000000..e3b423f
--- /dev/null
+++ b/src/main/java/frc/lib/ArmFeedforwardCalculator.java
@@ -0,0 +1,19 @@
+package frc.lib;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/** Helper class to assist with arm feedforward calculations. */
+public class ArmFeedforwardCalculator {
+
+  /**
+   * Calculates the gravity compensation constant for an arm given the voltage applied at an
+   * equilibrium position.
+   *
+   * @param angle the equilibrium position of the arm.
+   * @param volts the voltage applied to the arm to hold it the equilibrium position.
+   * @return the gravity compensation constant for the arm.
+   */
+  public static double calculateArmGravityCompensation(Rotation2d angle, double volts) {
+    return volts / angle.getCos();
+  }
+}
diff --git a/src/main/java/frc/lib/MotionProfileCalculator.java b/src/main/java/frc/lib/MotionProfileCalculator.java
new file mode 100644
index 0000000..7c37ac5
--- /dev/null
+++ b/src/main/java/frc/lib/MotionProfileCalculator.java
@@ -0,0 +1,17 @@
+package frc.lib;
+
+/** Helper class to assist with motion profile calculations. */
+public class MotionProfileCalculator {
+
+  /**
+   * Calculates an acceleration using a ramp duration.
+   *
+   * @param maximumSpeed the maximum speed in units per second.
+   * @param desiredRampDurationSeconds the desired duration to ramp from no speed to full speed.
+   * @return the acceleration in units per second per second.
+   */
+  public static double calculateAcceleration(
+      double maximumSpeed, double desiredRampDurationSeconds) {
+    return maximumSpeed / desiredRampDurationSeconds;
+  }
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ee03663..182a5ca 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -1,9 +1,12 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.Telemetry;
 import frc.robot.arm.Arm;
+import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+import frc.robot.arm.ArmMechanism;
 import frc.robot.auto.Auto;
 import frc.robot.climber.Climber;
 import frc.robot.intake.Intake;
@@ -56,6 +59,8 @@ public static RobotContainer getInstance() {
   private void initializeTelemetry() {
     Telemetry.initializeShuffleboards(
         arm, auto, climber, intake, lights, odometry, shooter, swerve, vision);
+
+    SmartDashboard.putData("Arm Mechanism", ArmMechanism.getInstance().getMechanism());
   }
 
   /** Configures operator controller bindings. */
@@ -75,7 +80,17 @@ private void configureBindings() {
     operatorController.rightBumper().whileTrue(intake.outtake()).whileTrue(shooter.shoot());
     operatorController.rightTrigger().whileTrue(shooter.smartShoot());
 
-    operatorController.a().whileTrue(arm.driveShoulderWithJoystick(operatorController::getLeftY));
+    operatorController
+        .x()
+        .whileTrue(
+            arm.runSetpoint(
+                () -> arm.getState().withShoulder(ShoulderMotorConstants.MINIMUM_ANGLE)));
+    operatorController
+        .y()
+        .whileTrue(
+            arm.runSetpoint(
+                () -> arm.getState().withShoulder(ShoulderMotorConstants.MAXIMUM_ANGLE)));
+
     operatorController.b().whileTrue(arm.driveElbowWithJoystick(operatorController::getLeftY));
   }
 
diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
index bc4fa64..15fe30f 100644
--- a/src/main/java/frc/robot/arm/Arm.java
+++ b/src/main/java/frc/robot/arm/Arm.java
@@ -1,13 +1,13 @@
 package frc.robot.arm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
 import frc.robot.arm.ArmConstants.ElbowMotorConstants;
-import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
 import frc.robot.arm.ElbowMotorIO.ElbowMotorIOValues;
 import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
 import java.util.function.DoubleSupplier;
@@ -31,12 +31,16 @@ public class Arm extends Subsystem {
   /** Elbow motor values. */
   private final ElbowMotorIOValues elbowMotorValues = new ElbowMotorIOValues();
 
+  private ArmState setpoint;
+
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
     shoulderMotor = ArmFactory.createShoulderMotor();
     elbowMotor = ArmFactory.createElbowMotor();
 
     setState(new ArmState(Rotation2d.fromDegrees(90), new Rotation2d(), new Rotation2d()));
+
+    setpoint = getState();
   }
 
   /**
@@ -55,17 +59,25 @@ public static Arm getInstance() {
   @Override
   public void periodic() {
     shoulderMotor.update(shoulderMotorValues);
+
+    ArmMechanism.getInstance().setState(getState());
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout shoulder = Telemetry.addColumn(tab, "Shoulder");
+    ShuffleboardLayout position = Telemetry.addColumn(tab, "Position");
 
-    shoulder.addDouble("Position (rot)", () -> shoulderMotorValues.positionRotations);
+    position.addDouble(
+        "Shoulder Position (deg)",
+        () -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations));
+    position.addDouble(
+        "Elbow Position (deg)", () -> Units.rotationsToDegrees(elbowMotorValues.positionRotations));
 
-    ShuffleboardLayout elbow = Telemetry.addColumn(tab, "Elbow");
+    ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint");
 
-    elbow.addDouble("Position (rot)", () -> elbowMotorValues.positionRotations);
+    setpoint.addDouble("Shoulder Setpoint (deg)", () -> this.setpoint.shoulder().getDegrees());
+    setpoint.addDouble("Elbow Setpoint (deg)", () -> this.setpoint.elbow().getDegrees());
+    setpoint.addDouble("Wrist Setpoint (deg)", () -> this.setpoint.wrist().getDegrees());
   }
 
   public void setState(ArmState state) {
@@ -86,31 +98,32 @@ public ArmState getState() {
         Rotation2d.fromRotations(0));
   }
 
-  private void runSetpoint(ArmState state) {
-    shoulderMotor.runSetpoint(state.shoulder().getRotations());
-    elbowMotor.runSetpoint(state.elbow().getRotations());
-    // wristMotor.runSetpoint(state.wrist().getRotations());
+  public ArmState getSetpoint() {
+    return setpoint;
   }
 
-  public Command runSetpoint(Supplier<ArmState> stateSupplier) {
-    return run(() -> runSetpoint(stateSupplier.get()));
+  private void setSetpoint(ArmState setpoint) {
+    this.setpoint = setpoint;
+
+    shoulderMotor.setSetpoint(setpoint.shoulder().getRotations());
+    elbowMotor.setSetpoint(setpoint.elbow().getRotations());
+    // wristMotor.runSetpoint(state.wrist().getRotations());
   }
 
-  public Command hold() {
-    return runSetpoint(this::getState);
+  public Command runSetpoint(Supplier<ArmState> setpointSupplier) {
+    return run(() -> setSetpoint(setpointSupplier.get()));
   }
 
-  public Command driveShoulderWithJoystick(DoubleSupplier joystickSupplier) {
-    return run(() ->
-            shoulderMotor.setVoltage(
-                joystickSupplier.getAsDouble() * ShoulderMotorConstants.MAXIMUM_VOLTAGE))
-        .finallyDo(shoulderMotor::stop);
+  public Command hold() {
+    return runOnce(() -> setSetpoint(getState())).andThen(runSetpoint(this::getSetpoint));
   }
 
   public Command driveElbowWithJoystick(DoubleSupplier joystickSupplier) {
     return run(() ->
             elbowMotor.setVoltage(
-                joystickSupplier.getAsDouble() * ElbowMotorConstants.MAXIMUM_VOLTAGE))
-        .finallyDo(shoulderMotor::stop);
+                joystickSupplier.getAsDouble()
+                    * -1
+                    * Math.abs(ElbowMotorConstants.MAXIMUM_VOLTAGE)))
+        .finallyDo(elbowMotor::stop);
   }
 }
diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java
index e588542..e7c0590 100644
--- a/src/main/java/frc/robot/arm/ArmConstants.java
+++ b/src/main/java/frc/robot/arm/ArmConstants.java
@@ -1,17 +1,49 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import frc.lib.CAN;
+import frc.lib.MotionProfileCalculator;
+
 /** Constants for the arm subsystem. */
 public class ArmConstants {
 
   /** Constants for the shoulder motor. */
   public static class ShoulderMotorConstants {
-    /** Maximum voltage that can be applied to the shoulder motor. */
-    public static final double MAXIMUM_VOLTAGE = 2.0;
+    public static final CAN CAN = new CAN(32);
+
+    /** Gearing between the soulder motor and the shoulder joint. */
+    public static final double GEARING = 51.2;
+
+    /** Moment of intertia of the shoulder, in kilograms meters squared. */
+    public static final double MOI = 0.15093;
+
+    /** Minumum angle of the shoulder joint. */
+    public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(20);
+
+    /** Maximum angle of the shoulder joint. */
+    public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90);
+
+    /** Shoulder pivot to elbow pivot distance in meters. */
+    public static final double SHOULDER_TO_ELBOW_DISTANCE = Units.inchesToMeters(16.775);
+
+    /** Proportional gain in volts per rotation. */
+    public static final double KP = 36.0;
+
+    /** Maximum speed of the shoulder joint in rotations per second. */
+    public static final double MAXIMUM_SPEED = 1.5;
+
+    /** Maximum acceleration of the shoulder joint in rotations per second per second. */
+    public static final double MAXIMUM_ACCELERATION =
+        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3);
   }
 
   /** Constants for the elbow motor. */
   public static class ElbowMotorConstants {
     /** Maximum voltage that can be applied to the elbow motor. */
     public static final double MAXIMUM_VOLTAGE = 2.0;
+
+    /** Elbow pivot to wrist pivot distance in meters. */
+    public static final double ELBOW_TO_WRIST_DISTANCE = Units.inchesToMeters(16.825);
   }
 }
diff --git a/src/main/java/frc/robot/arm/ArmFactory.java b/src/main/java/frc/robot/arm/ArmFactory.java
index 4feacfd..17940c7 100644
--- a/src/main/java/frc/robot/arm/ArmFactory.java
+++ b/src/main/java/frc/robot/arm/ArmFactory.java
@@ -13,7 +13,7 @@ public class ArmFactory {
   public static ShoulderMotorIO createShoulderMotor() {
     if (Robot.isReal()) return new ShoulderMotorIOSparkMax();
 
-    return new ShoulderMotorIOSparkMax();
+    return new ShoulderMotorIOSim();
   }
 
   /**
@@ -22,8 +22,6 @@ public static ShoulderMotorIO createShoulderMotor() {
    * @return an elbow motor.
    */
   public static ElbowMotorIO createElbowMotor() {
-    if (Robot.isReal()) return new ElbowMotorIOSparkMax();
-
-    return new ElbowMotorIOSparkMax();
+    return new ElbowMotorIOSim();
   }
 }
diff --git a/src/main/java/frc/robot/arm/ArmMechanism.java b/src/main/java/frc/robot/arm/ArmMechanism.java
new file mode 100644
index 0000000..fda369b
--- /dev/null
+++ b/src/main/java/frc/robot/arm/ArmMechanism.java
@@ -0,0 +1,62 @@
+package frc.robot.arm;
+
+import edu.wpi.first.math.util.Units;
+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.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.arm.ArmConstants.ElbowMotorConstants;
+import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+
+/** Helper class for rendering the arm using WPILib mechanisms. */
+public class ArmMechanism {
+
+  private static ArmMechanism instance = null;
+
+  private final Mechanism2d mechanism;
+
+  private final MechanismRoot2d root;
+
+  private final MechanismLigament2d shoulder, elbow;
+
+  private final double THICKNESS = Units.inchesToMeters(2) * 100;
+
+  private ArmMechanism() {
+    mechanism = new Mechanism2d(1, 1);
+    root = mechanism.getRoot("arm", 0.15, 0);
+    shoulder =
+        root.append(
+            new MechanismLigament2d(
+                "shoulder",
+                ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
+                90,
+                THICKNESS,
+                new Color8Bit(Color.kOrange)));
+    elbow =
+        shoulder.append(
+            new MechanismLigament2d(
+                "elbow",
+                ElbowMotorConstants.ELBOW_TO_WRIST_DISTANCE,
+                90,
+                THICKNESS,
+                new Color8Bit(Color.kGreen)));
+  }
+
+  public static ArmMechanism getInstance() {
+    if (instance == null) {
+      instance = new ArmMechanism();
+    }
+
+    return instance;
+  }
+
+  public Mechanism2d getMechanism() {
+    return mechanism;
+  }
+
+  public void setState(ArmState state) {
+    shoulder.setAngle(state.shoulder());
+    elbow.setAngle(state.elbow().minus(state.shoulder()));
+  }
+}
diff --git a/src/main/java/frc/robot/arm/ElbowMotorIO.java b/src/main/java/frc/robot/arm/ElbowMotorIO.java
index 4087b05..5559b13 100644
--- a/src/main/java/frc/robot/arm/ElbowMotorIO.java
+++ b/src/main/java/frc/robot/arm/ElbowMotorIO.java
@@ -33,7 +33,7 @@ public static class ElbowMotorIOValues {
    *
    * @param positionRotations the elbow motor's setpoint.
    */
-  public void runSetpoint(double positionRotations);
+  public void setSetpoint(double positionRotations);
 
   // TODO Remove, only for characterization
   /**
diff --git a/src/main/java/frc/robot/arm/ElbowMotorIOSim.java b/src/main/java/frc/robot/arm/ElbowMotorIOSim.java
new file mode 100644
index 0000000..d967276
--- /dev/null
+++ b/src/main/java/frc/robot/arm/ElbowMotorIOSim.java
@@ -0,0 +1,23 @@
+package frc.robot.arm;
+
+/** Simulated elbow motor. */
+public class ElbowMotorIOSim implements ElbowMotorIO {
+
+  @Override
+  public void configure() {}
+
+  @Override
+  public void update(ElbowMotorIOValues values) {}
+
+  @Override
+  public void setPosition(double positionRotations) {}
+
+  @Override
+  public void setSetpoint(double positionRotations) {}
+
+  @Override
+  public void setVoltage(double volts) {}
+
+  @Override
+  public void stop() {}
+}
diff --git a/src/main/java/frc/robot/arm/ElbowMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ElbowMotorIOSparkMax.java
deleted file mode 100644
index c22d69c..0000000
--- a/src/main/java/frc/robot/arm/ElbowMotorIOSparkMax.java
+++ /dev/null
@@ -1,84 +0,0 @@
-package frc.robot.arm;
-
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkMax;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
-import frc.lib.Configurator;
-import frc.robot.arm.ArmConstants.ElbowMotorConstants;
-
-/** Elbow motor using a Spark Max. */
-public class ElbowMotorIOSparkMax implements ElbowMotorIO {
-
-  /** Hardware Spark Max. */
-  private final CANSparkMax sparkMax;
-
-  /** Feedback controller for elbow position. */
-  private final ProfiledPIDController feedback =
-      new ProfiledPIDController(0, 0, 0, new Constraints(0, 0));
-
-  /** Feedforward controller for elbow position. */
-  private final ArmFeedforward feedforward = new ArmFeedforward(0, 0, 0);
-
-  /** Creates a new elbow motor using a Spark Max. */
-  public ElbowMotorIOSparkMax() {
-    sparkMax = new CANSparkMax(33, MotorType.kBrushless);
-  }
-
-  @Override
-  public void configure() {
-    Configurator.configureREV(sparkMax::restoreFactoryDefaults);
-
-    Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake));
-  }
-
-  @Override
-  public void update(ElbowMotorIOValues values) {
-    values.positionRotations = getPositionRotations();
-    values.currentAmps = sparkMax.getOutputCurrent();
-  }
-
-  @Override
-  public void setPosition(double positionRotations) {
-    // TODO
-  }
-
-  @Override
-  public void runSetpoint(double positionRotations) {
-    double measuredPositionRotations = getPositionRotations();
-
-    double feedbackVolts = feedback.calculate(positionRotations, measuredPositionRotations);
-
-    double feedforwardVolts =
-        feedforward.calculate(measuredPositionRotations, feedback.getSetpoint().velocity);
-
-    setVoltage(feedbackVolts + feedforwardVolts);
-  }
-
-  // TODO Remove, only for characterization
-  @Override
-  public void setVoltage(double volts) {
-    volts =
-        MathUtil.clamp(
-            volts, -ElbowMotorConstants.MAXIMUM_VOLTAGE, ElbowMotorConstants.MAXIMUM_VOLTAGE);
-
-    sparkMax.setVoltage(volts);
-  }
-
-  @Override
-  public void stop() {
-    setVoltage(0);
-  }
-
-  /**
-   * Gets the absolute position of the elbow in rotations.
-   *
-   * @return the absolute position of the elbow in rotations.
-   */
-  private double getPositionRotations() {
-    return sparkMax.getEncoder().getPosition();
-  }
-}
diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIO.java b/src/main/java/frc/robot/arm/ShoulderMotorIO.java
index 902a436..9a96561 100644
--- a/src/main/java/frc/robot/arm/ShoulderMotorIO.java
+++ b/src/main/java/frc/robot/arm/ShoulderMotorIO.java
@@ -34,16 +34,5 @@ public static class ShoulderMotorIOValues {
    *
    * @param positionRotations the shoulder motor's setpoint.
    */
-  public void runSetpoint(double positionRotations);
-
-  // TODO Remove, only for characterization
-  /**
-   * Run the shoulder motor with the specified voltage.
-   *
-   * @param volts volts to apply to the shoulder motor.
-   */
-  public void setVoltage(double volts);
-
-  /** Stop the shoulder motor. */
-  public void stop();
+  public void setSetpoint(double positionRotations);
 }
diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java
new file mode 100644
index 0000000..eb81b94
--- /dev/null
+++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java
@@ -0,0 +1,78 @@
+package frc.robot.arm;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.lib.ArmFeedforwardCalculator;
+import frc.robot.RobotConstants;
+import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+
+/** Simulated shoulder motor. */
+public class ShoulderMotorIOSim implements ShoulderMotorIO {
+
+  private final SingleJointedArmSim singleJointedArmSim;
+
+  private final ProfiledPIDController feedback;
+
+  private final ArmFeedforward feedforward;
+
+  /** Creates a new simulated shoulder motor. */
+  public ShoulderMotorIOSim() {
+    singleJointedArmSim =
+        new SingleJointedArmSim(
+            DCMotor.getNEO(1),
+            ShoulderMotorConstants.GEARING,
+            ShoulderMotorConstants.MOI,
+            ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
+            ShoulderMotorConstants.MINIMUM_ANGLE.getRadians(),
+            ShoulderMotorConstants.MAXIMUM_ANGLE.getRadians(),
+            true,
+            Units.degreesToRadians(90));
+
+    feedback =
+        new ProfiledPIDController(
+            ShoulderMotorConstants.KP,
+            0,
+            0,
+            new Constraints(
+                ShoulderMotorConstants.MAXIMUM_SPEED, ShoulderMotorConstants.MAXIMUM_ACCELERATION));
+
+    feedforward =
+        new ArmFeedforward(
+            0,
+            ArmFeedforwardCalculator.calculateArmGravityCompensation(
+                Rotation2d.fromDegrees(70.81), 0.101859),
+            0);
+  }
+
+  @Override
+  public void configure() {}
+
+  @Override
+  public void update(ShoulderMotorIOValues values) {
+    singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION);
+
+    values.positionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads());
+    values.currentAmps = singleJointedArmSim.getCurrentDrawAmps();
+  }
+
+  @Override
+  public void setPosition(double positionRotations) {
+    singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0.0);
+  }
+
+  @Override
+  public void setSetpoint(double positionRotations) {
+    double measuredPositionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads());
+
+    double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
+
+    double feedforwardVolts = feedforward.calculate(measuredPositionRotations, 0.0);
+
+    singleJointedArmSim.setInputVoltage(feedbackVolts + feedforwardVolts);
+  }
+}
diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java
index fa1fb08..2e9fa36 100644
--- a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java
+++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java
@@ -3,10 +3,11 @@
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import frc.lib.ArmFeedforwardCalculator;
 import frc.lib.Configurator;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
 
@@ -17,15 +18,29 @@ public class ShoulderMotorIOSparkMax implements ShoulderMotorIO {
   private final CANSparkMax sparkMax;
 
   /** Feedback controller for shoulder position. */
-  private final ProfiledPIDController feedback =
-      new ProfiledPIDController(0, 0, 0, new Constraints(0, 0));
+  private final ProfiledPIDController feedback;
 
   /** Feedforward controller for shoulder position. */
-  private final ArmFeedforward feedforward = new ArmFeedforward(0, 0, 0);
+  private final ArmFeedforward feedforward;
 
   /** Creates a new shoulder motor using a Spark Max. */
   public ShoulderMotorIOSparkMax() {
-    sparkMax = new CANSparkMax(32, MotorType.kBrushless);
+    sparkMax = new CANSparkMax(ShoulderMotorConstants.CAN.id(), MotorType.kBrushless);
+
+    feedback =
+        new ProfiledPIDController(
+            ShoulderMotorConstants.KP,
+            0,
+            0,
+            new Constraints(
+                ShoulderMotorConstants.MAXIMUM_SPEED, ShoulderMotorConstants.MAXIMUM_ACCELERATION));
+
+    feedforward =
+        new ArmFeedforward(
+            0,
+            ArmFeedforwardCalculator.calculateArmGravityCompensation(
+                Rotation2d.fromDegrees(18.0), 0.1222),
+            0);
   }
 
   @Override
@@ -43,34 +58,19 @@ public void update(ShoulderMotorIOValues values) {
 
   @Override
   public void setPosition(double positionRotations) {
-    // TODO
+    sparkMax.getEncoder().setPosition(positionRotations * ShoulderMotorConstants.GEARING);
   }
 
   @Override
-  public void runSetpoint(double positionRotations) {
+  public void setSetpoint(double positionRotations) {
     double measuredPositionRotations = getPositionRotations();
 
-    double feedbackVolts = feedback.calculate(positionRotations, measuredPositionRotations);
+    double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
 
     double feedforwardVolts =
         feedforward.calculate(measuredPositionRotations, feedback.getSetpoint().velocity);
 
-    setVoltage(feedbackVolts + feedforwardVolts);
-  }
-
-  // TODO Remove, only for characterization
-  @Override
-  public void setVoltage(double volts) {
-    volts =
-        MathUtil.clamp(
-            volts, -ShoulderMotorConstants.MAXIMUM_VOLTAGE, ShoulderMotorConstants.MAXIMUM_VOLTAGE);
-
-    sparkMax.setVoltage(volts);
-  }
-
-  @Override
-  public void stop() {
-    setVoltage(0);
+    sparkMax.setVoltage(feedbackVolts + feedforwardVolts);
   }
 
   /**
@@ -79,6 +79,6 @@ public void stop() {
    * @return the absolute position of the shoulder in rotations.
    */
   private double getPositionRotations() {
-    return sparkMax.getEncoder().getPosition();
+    return sparkMax.getEncoder().getPosition() / ShoulderMotorConstants.GEARING;
   }
 }
diff --git a/src/main/java/frc/robot/intake/IntakeFactory.java b/src/main/java/frc/robot/intake/IntakeFactory.java
index 4c827d7..9621fed 100644
--- a/src/main/java/frc/robot/intake/IntakeFactory.java
+++ b/src/main/java/frc/robot/intake/IntakeFactory.java
@@ -1,7 +1,5 @@
 package frc.robot.intake;
 
-import frc.robot.Robot;
-
 /** Helper class for creating hardware for the intake subsystem. */
 public class IntakeFactory {
 
@@ -11,7 +9,7 @@ public class IntakeFactory {
    * @return a roller motor.
    */
   public static RollerMotorIO createRollerMotor() {
-    if (Robot.isReal()) return new RollerMotorIOSparkMax();
+    // if (Robot.isReal()) return new RollerMotorIOSparkMax();
 
     return new RollerMotorIOSim();
   }
diff --git a/src/main/java/frc/robot/lights/LightsFactory.java b/src/main/java/frc/robot/lights/LightsFactory.java
index 0eabdff..392494f 100644
--- a/src/main/java/frc/robot/lights/LightsFactory.java
+++ b/src/main/java/frc/robot/lights/LightsFactory.java
@@ -1,7 +1,5 @@
 package frc.robot.lights;
 
-import frc.robot.Robot;
-
 /** Helper class for creating hardware for the lights subsystem. */
 public class LightsFactory {
 
@@ -11,7 +9,7 @@ public class LightsFactory {
    * @return an LED controller.
    */
   public static LEDControllerIO createLEDController() {
-    if (Robot.isReal()) return new LEDControllerIOCANdle();
+    // if (Robot.isReal()) return new LEDControllerIOCANdle();
 
     return new LEDControllerIOSim();
   }
diff --git a/src/main/java/frc/robot/odometry/OdometryFactory.java b/src/main/java/frc/robot/odometry/OdometryFactory.java
index 1290178..987f7a3 100644
--- a/src/main/java/frc/robot/odometry/OdometryFactory.java
+++ b/src/main/java/frc/robot/odometry/OdometryFactory.java
@@ -2,7 +2,6 @@
 
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import frc.lib.CAN;
-import frc.robot.Robot;
 
 /** Helper class for creating hardware for the odometry subsystem. */
 public class OdometryFactory {
@@ -13,7 +12,7 @@ public class OdometryFactory {
    * @return a gyroscope.
    */
   public static GyroscopeIO createGyroscope(CAN gyroscopeCAN, Odometry odometry) {
-    if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN);
+    // if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN);
 
     return new GyroscopeIOSim(odometry);
   }
diff --git a/src/main/java/frc/robot/shooter/ShooterFactory.java b/src/main/java/frc/robot/shooter/ShooterFactory.java
index 2340bdf..6a28bee 100644
--- a/src/main/java/frc/robot/shooter/ShooterFactory.java
+++ b/src/main/java/frc/robot/shooter/ShooterFactory.java
@@ -22,7 +22,7 @@ public static BeamBreakSensorIO createBeamBreakSensor() {
    * @return a serializer motor.
    */
   public static SerializerMotorIO createSerializerMotor() {
-    if (Robot.isReal()) return new SerializerMotorIOSparkMax();
+    // if (Robot.isReal()) return new SerializerMotorIOSparkMax();
 
     return new SerializerMotorIOSim();
   }
@@ -33,7 +33,7 @@ public static SerializerMotorIO createSerializerMotor() {
    * @return a flywheel motor.
    */
   public static FlywheelMotorIO createFlywheelMotor() {
-    if (Robot.isReal()) return new FlywheelMotorIOSparkMax(); // TODO
+    // if (Robot.isReal()) return new FlywheelMotorIOSparkMax(); // TODO
 
     return new FlywheelMotorIOSim();
   }
diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java
index aea6b86..56209cc 100644
--- a/src/main/java/frc/robot/swerve/SwerveConstants.java
+++ b/src/main/java/frc/robot/swerve/SwerveConstants.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import frc.lib.MotionProfileCalculator;
 import frc.lib.PIDFConstants;
 
 /** Constants for the swerve subsystem. */
@@ -93,20 +94,9 @@ private static double calculateMaximumAttainableSpeed(
   /** Maximum speed in meters per second. */
   public static final double MAXIMUM_SPEED = MAXIMUM_ATTAINABLE_SPEED;
 
-  /**
-   * Calculates an acceleration using a ramp duration.
-   *
-   * @param maximumSpeed the maximum speed in units per second.
-   * @param desiredRampDurationSeconds the desired duration to ramp from no speed to full speed.
-   * @return the acceleration in units per second per second.
-   */
-  private static double calculateAcceleration(
-      double maximumSpeed, double desiredRampDurationSeconds) {
-    return maximumSpeed / desiredRampDurationSeconds;
-  }
-
   /** Maximum acceleration in meters per second per second. */
-  public static final double MAXIMUM_ACCELERATION = calculateAcceleration(MAXIMUM_SPEED, 0.1);
+  public static final double MAXIMUM_ACCELERATION =
+      MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1);
 
   /** Maximum attainable rotational speed in rotations per second. */
   public static final double MAXIMUM_ATTAINABLE_ROTATION_SPEED =
@@ -117,7 +107,7 @@ private static double calculateAcceleration(
 
   /** Maximum acceleration in rotations per second per second. */
   public static final double MAXIMUM_ROTATION_ACCELERATION =
-      calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1);
+      MotionProfileCalculator.calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1);
 
   public static final PIDFConstants DRIVE_PIDF_CONSTANTS = new PIDFConstants();
 
diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java
index 47a5c81..4585e94 100644
--- a/src/main/java/frc/robot/swerve/SwerveFactory.java
+++ b/src/main/java/frc/robot/swerve/SwerveFactory.java
@@ -23,8 +23,8 @@ public static SwerveModuleIO createModule(SwerveModuleConfig config) {
    * @return an azimuth encoder.
    */
   public static AzimuthEncoderIO createAzimuthEncoder(SwerveModuleConfig config) {
-    if (Robot.isReal())
-      return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset());
+    // if (Robot.isReal())
+    //   return new AzimuthEncoderIOCANcoder(config.moduleCAN().azimuth(), config.offset());
 
     return new AzimuthEncoderIOSim();
   }
@@ -46,8 +46,9 @@ public static CANcoderConfiguration createAzimuthEncoderConfig() {
    * @return a steer motor.
    */
   public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
-    if (Robot.isReal())
-      return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), config.moduleCAN().azimuth());
+    // if (Robot.isReal())
+    //   return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(),
+    // config.moduleCAN().azimuth());
 
     return new SteerMotorIOSim();
   }
@@ -58,7 +59,7 @@ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
    * @return a drive motor.
    */
   public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) {
-    if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive());
+    // if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive());
 
     return new DriveMotorIOSim();
   }