Fixed the elevator in real mode, will fix in sim.

Author: spderman3333

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -57,7 +57,7 @@ public Command setElevatorPositionCommand(ElevatorHeight height) {
     public enum ElevatorHeight {
         // Positions taken from offseason bot code, inturn taken from onshape.
         UP(Inches.of(56.0)),
-        MIDDLE(Inches.of(4.0)),
+        MIDDLE(Inches.of(28.0)),
         DOWN(Inches.of(0.0));
 
         public final Distance position;
@@ -71,8 +71,9 @@ public Distance getPosition() {
         }
 
         public Angle getPositionAngle() {
-            // NOTE: yes this code is bs, but i wanna see it work first.
-            return Rotations.of(getPosition().in(Inches) / ElevatorConstants.ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION);
+            // NOTE: Divide by 2 because the motor controls the first stage only, not the second stage
+            return Rotations.of(
+                    (getPosition().in(Inches) / 2) / ElevatorConstants.ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION);
         }
     }
 }

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -17,13 +17,14 @@ public class ElevatorConstants {
     // Correct calculation for Inches of travel per 1 Motor Rotation:
     // (2 * pi * Spool Radius) / Gearing
     // NOTE to daniel: Will need to recalculate this with the new ELEVATOR_SPOOL_RADIUS
-    public static final double ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION = 0.349727;
+    public static final double ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION = 0.681933; // 0.272049
 
     // Guess what, I asked design again.
     public static final Distance ELEVATOR_SPOOL_RADIUS = Inches.of(0.8755);
 
     public static final Distance ELEVATOR_MIN_HEIGHT = Inches.of(0.0);
-    public static final Distance ELEVATOR_MAX_HEIGHT = Inches.of(56.0);
+    public static final Distance ELEVATOR_MAX_HEIGHT =
+            Inches.of(28.0); // This is the max height of the first stage, which we simulate.
 
     public static final double ELEVATOR_kG = 0.0; // 0.29
     public static final double ELEVATOR_kS = 0.0; // 0.11

src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java

@@ -81,6 +81,7 @@ public Distance getCarriagePosition() {
     }
 
     private static Distance motorRotationToCarriagePosition(Angle motorPosition) {
-        return Inches.of(motorPosition.in(Rotations) * ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION);
+        return Inches.of(motorPosition.in(Rotations) * ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION)
+                .times(2);
     }
 }