defined the measurement of the consts

Author: BrodyKarr

src/main/java/frc/robot/Constants.java

@@ -458,19 +458,21 @@ public static class LIMELIGHT_LEFT {
   }
 
   public static class constRobotPoses {
-    public static final Double ROOT_X = 1.5; // TODO: Replace with actual measurement
-    public static final Double ROOT_Y = 0.6; // TODO: Replace with actual measurement
-
-    public static final Double ELEVATOR_PIVOT_LENGTH = 0.1; // TODO: Replace with actual measurement
-    public static final Double ELEVATOR_PIVOT_DEFAULT_ANGLE = 0.0; // TODO: Replace with actual measurement
-    public static final Double ELEVATOR_PIVOT_WIDTH = 0.1; // TODO: Replace with actual measurement
-
-    public static final Double ELEVATOR_LENGTH = 0.5; // TODO: Replace with actual measurement
-    public static final Double ELEVATOR_DEFAULT_ANGLE = 90.0; // TODO: Replace with actual measurement
-    public static final Double ELEVATOR_WIDTH = 0.1; // TODO: Replace with actual measurement
-
-    public static final Double INTAKE_WRIST_LENGTH = 0.5; // TODO: Replace with actual measurement
-    public static final Double INTAKE_WRIST_DEFAULT_ANGLE = 95.0; // TODO: Replace with actual measurement
-    public static final Double INTAKE_WRIST_WIDTH = 10.0; // TODO: Replace with actual measurement
+    public static final Distance ROOT_X = Units.Inches.of(0); // TODO: Replace with actual measurement
+    public static final Distance ROOT_Y = Units.Inches.of(0); // TODO: Replace with actual measurement
+
+    public static final Distance ELEVATOR_PIVOT_LENGTH = Units.Inches.of(3.95);// TODO: Replace with actual measurement
+    public static final Angle ELEVATOR_PIVOT_DEFAULT_ANGLE = Units.Degrees.of(90); // TODO: Replace with actual
+                                                                                   // measurement
+    public static final Distance ELEVATOR_PIVOT_WIDTH = Units.Inches.of(3.95); // TODO: Replace with actual measurement
+
+    public static final Distance ELEVATOR_LENGTH = Units.Inches.of(3.95); // TODO: Replace with actual measurement
+    public static final Angle ELEVATOR_DEFAULT_ANGLE = Units.Degrees.of(90); // TODO: Replace with actual measurement
+    public static final Distance ELEVATOR_WIDTH = Units.Inches.of(3.95); // TODO: Replace with actual measurement
+
+    public static final Distance INTAKE_WRIST_LENGTH = Units.Inches.of(3.95); // TODO: Replace with actual measurement
+    public static final Angle INTAKE_WRIST_DEFAULT_ANGLE = Units.Degrees.of(90); // TODO: Replace with actual
+                                                                                 // measurement
+    public static final Distance INTAKE_WRIST_WIDTH = Units.Inches.of(3.95); // TODO: Replace with actual measurement
   }
 }

src/main/java/frc/robot/subsystems/RobotPoses.java

@@ -4,9 +4,11 @@
 
 package frc.robot.subsystems;
 
+import edu.wpi.first.units.Units;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.epilogue.NotLogged;
 import edu.wpi.first.math.geometry.Pose3d;
+
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -41,21 +43,26 @@ public RobotPoses(Drivetrain subDrivetrain, Elevator subElevator, Intake subInta
     // the main mechanism object
 
     // the mechanism root node
-    MechanismRoot2d root = mech.getRoot("drivetrain", constRobotPoses.ROOT_X, constRobotPoses.ROOT_Y);
+    MechanismRoot2d root = mech.getRoot("drivetrain", constRobotPoses.ROOT_X.in(Units.Meters),
+        constRobotPoses.ROOT_Y.in(Units.Meters));
 
-    elevatorPivot = root.append(new MechanismLigament2d("elevator-pivot", constRobotPoses.ELEVATOR_PIVOT_LENGTH,
-        constRobotPoses.ELEVATOR_PIVOT_DEFAULT_ANGLE, constRobotPoses.ELEVATOR_PIVOT_WIDTH,
-        new Color8Bit(Color.kGreen)));
+    elevatorPivot = root
+        .append(new MechanismLigament2d("elevator-pivot", constRobotPoses.ELEVATOR_PIVOT_LENGTH.in(Units.Meters),
+            constRobotPoses.ELEVATOR_PIVOT_DEFAULT_ANGLE.in(Units.Degrees),
+            constRobotPoses.ELEVATOR_PIVOT_WIDTH.in(Units.Meters),
+            new Color8Bit(Color.kGreen)));
     elevator = elevatorPivot.append(
-        new MechanismLigament2d("elevator", constRobotPoses.ELEVATOR_LENGTH, constRobotPoses.ELEVATOR_DEFAULT_ANGLE,
-            constRobotPoses.ELEVATOR_WIDTH, new Color8Bit(Color.kBlue)));
+        new MechanismLigament2d("elevator", constRobotPoses.ELEVATOR_LENGTH.in(Units.Meters),
+            constRobotPoses.ELEVATOR_DEFAULT_ANGLE.in(Units.Degrees),
+            constRobotPoses.ELEVATOR_WIDTH.in(Units.Meters), new Color8Bit(Color.kBlue)));
     // Set default motor configurations if needed
     // e.g., elevatorLeftMotor.configFactoryDefault();
     // post the mechanism to the dashboard
 
     intakeWrist = elevator.append(
-        new MechanismLigament2d("intake-wrist", constRobotPoses.INTAKE_WRIST_LENGTH,
-            constRobotPoses.INTAKE_WRIST_DEFAULT_ANGLE, constRobotPoses.INTAKE_WRIST_WIDTH, new Color8Bit(Color.kRed)));
+        new MechanismLigament2d("intake-wrist", constRobotPoses.INTAKE_WRIST_LENGTH.in(Units.Meters),
+            constRobotPoses.INTAKE_WRIST_DEFAULT_ANGLE.in(Units.Degrees),
+            constRobotPoses.INTAKE_WRIST_WIDTH.in(Units.Meters), new Color8Bit(Color.kRed)));
   }
 
   @Override