continuous state actions (#44)

* continous state actions * spotless * removed speaker/feeding from after transition
team581 · Oct 4, 2024 · 6769b47 · 6769b47
1 parent dd3efa9
commit 6769b47
Showing 1 changed file with 17 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java
@@ -159,19 +159,6 @@ protected void afterTransition(ArmState newState) {
                 Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER.getDegrees()))));
       }
 
-      case FEEDING -> {
-        double newAngle =
-            Units.degreesToRotations(clamp(feedSpotDistanceToAngle.get(distanceToFeedSpot)));
-        leftMotor.setControl(positionRequest.withPosition(newAngle));
-        rightMotor.setControl(positionRequest.withPosition(newAngle));
-      }
-      case SPEAKER_SHOT -> {
-        var newAngle =
-            Units.degreesToRotations(clamp(speakerDistanceToAngle.get(distanceToSpeaker)));
-
-        leftMotor.setControl(positionRequest.withPosition(newAngle));
-        rightMotor.setControl(positionRequest.withPosition(newAngle));
-      }
       case AMP -> {
         leftMotor.setControl(
             positionRequest.withPosition(
@@ -188,13 +175,30 @@ protected void afterTransition(ArmState newState) {
             positionRequest.withPosition(
                 Units.degreesToRotations(clamp(ArmAngle.PASS.getDegrees()))));
       }
+      default -> {}
     }
   }
 
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
 
+    switch (getState()) {
+      case SPEAKER_SHOT -> {
+        var newAngle =
+            Units.degreesToRotations(clamp(speakerDistanceToAngle.get(distanceToSpeaker)));
+        leftMotor.setControl(positionRequest.withPosition(newAngle));
+        rightMotor.setControl(positionRequest.withPosition(newAngle));
+      }
+      case FEEDING -> {
+        double newAngle =
+            Units.degreesToRotations(clamp(feedSpotDistanceToAngle.get(distanceToFeedSpot)));
+        leftMotor.setControl(positionRequest.withPosition(newAngle));
+        rightMotor.setControl(positionRequest.withPosition(newAngle));
+      }
+      default -> {}
+    }
+
     DogLog.log("Arm/Left/StatorCurrent", leftMotor.getStatorCurrent().getValueAsDouble());
     DogLog.log("Arm/Left/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble());
     DogLog.log("Arm/Left/ArmAngle", leftMotor.getPosition().getValueAsDouble());