Merge pull request #29 from Team-Optix-3749/elevator

made elevator changes in code review
Team-Optix-3749 · Feb 8, 2025 · e90e8f0 · e90e8f0
2 parents ad0bdef + 081e146
commit e90e8f0
Showing 1 changed file with 11 additions and 6 deletions.
diff --git a/Team-3749-2025/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/Team-3749-2025/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -121,6 +121,12 @@ public boolean getIsStableState() {
             case L4:
                 return UtilityFunctions.withinMargin(0.01, data.positionMeters,
                         ElevatorConstants.StateHeights.l4Height);
+            case MAX:
+                return UtilityFunctions.withinMargin(0.01, data.positionMeters,
+                    ElevatorConstants.ElevatorSpecs.maxHeightMeters);
+            case STOW:
+                return UtilityFunctions.withinMargin(0.01, data.positionMeters,
+                    ElevatorConstants.ElevatorSpecs.baseHeight);
             default:
                 return false;
         }
@@ -134,7 +140,7 @@ public void setState(ElevatorStates state) {
         this.state = state;
         switch (state) {
             case STOP:
-                runStateStop();
+                stop();
                 break;
             case L1:
                 setGoal(ElevatorConstants.StateHeights.l1Height);
@@ -149,9 +155,11 @@ public void setState(ElevatorStates state) {
                 setGoal(ElevatorConstants.StateHeights.l4Height);
                 break;
             case MAX:
-                setGoal(6);
+                setGoal(ElevatorConstants.ElevatorSpecs.maxHeightMeters);
                 break;
             case STOW:
+                setGoal(ElevatorConstants.ElevatorSpecs.baseHeight);
+                break;
             default:
                 setGoal(0);
                 break;
@@ -165,7 +173,7 @@ public void setGoal(double height) {
     private void runState() {
         switch (state) {
             case STOP:
-                runStateStop();
+                stop();
                 break;
             default:
                 moveToGoal();
@@ -183,9 +191,6 @@ private void moveToGoal() {
         elevatorio.setPosition(firstState.position, ffVoltage);
     }
 
-    private void runStateStop() {
-        stop();
-    }
 
     public void stop() {
         elevatorio.setVoltage(0);