feat: Add frame perimeter distances.

src/main/java/frc/robot/RobotConstants.java

@@ -1,5 +1,6 @@
 package frc.robot;
 
+import edu.wpi.first.math.util.Units;
 import java.util.EnumSet;
 import java.util.Set;
 
@@ -21,6 +22,15 @@ public class RobotConstants {
    */
   public static final double DISABLE_COAST_DELAY = 3.0;
 
+  /** Distance from the frame perimeter to the origin in meters. */
+  public static final double FRAME_PERIMETER_TO_ORIGIN_DISTANCE = Units.inchesToMeters(14);
+
+  /** Maximum horizontal extension distance in meters. */
+  public static final double MAX_HORIZONTAL_EXTENSION_DISTANCE = Units.feetToMeters(1);
+
+  /** Maximum vertical extension distance in meters. */
+  public static final double MAX_VERTICAL_EXTENSION_DISTANCE = Units.inchesToMeters(48);
+
   /** Robot subsystems. */
   public enum Subsystem {
     ARM,

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

@@ -1,6 +1,7 @@
 package frc.robot.arm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import frc.lib.CAN;
@@ -29,6 +30,10 @@ public static class ShoulderMotorConstants {
     /** Tolerance of the shoulder joint. */
     public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0);
 
+    /** Translation of the shoulder joint relative to the origin. */
+    public static final Translation2d SHOULDER_TRANSLATION =
+        new Translation2d(Units.inchesToMeters(11.361), Units.inchesToMeters(7.721));
+
     /** Shoulder pivot to elbow pivot distance in meters. */
     public static final double SHOULDER_TO_ELBOW_DISTANCE = Units.inchesToMeters(16.775);
 

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.RobotConstants;
 import frc.robot.arm.ArmConstants.ElbowMotorConstants;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
 
@@ -24,22 +25,33 @@ public class ArmMechanism {
   private final double THICKNESS = Units.inchesToMeters(2) * 100;
 
   private ArmMechanism() {
-    mechanism = new Mechanism2d(1, 1);
-    root = mechanism.getRoot("arm", 0.15, 0);
+    mechanism =
+        new Mechanism2d(
+            2
+                * (RobotConstants.FRAME_PERIMETER_TO_ORIGIN_DISTANCE
+                    + RobotConstants.MAX_HORIZONTAL_EXTENSION_DISTANCE),
+            RobotConstants.MAX_VERTICAL_EXTENSION_DISTANCE);
+    root =
+        mechanism.getRoot(
+            "arm",
+            RobotConstants.FRAME_PERIMETER_TO_ORIGIN_DISTANCE
+                + RobotConstants.MAX_HORIZONTAL_EXTENSION_DISTANCE
+                - ShoulderMotorConstants.SHOULDER_TRANSLATION.getX(),
+            ShoulderMotorConstants.SHOULDER_TRANSLATION.getY());
     shoulder =
         root.append(
             new MechanismLigament2d(
                 "shoulder",
                 ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
-                90,
+                0,
                 THICKNESS,
                 new Color8Bit(Color.kOrange)));
     elbow =
         shoulder.append(
             new MechanismLigament2d(
                 "elbow",
                 ElbowMotorConstants.ELBOW_TO_WRIST_DISTANCE,
-                90,
+                0,
                 THICKNESS,
                 new Color8Bit(Color.kGreen)));
   }