From 6769b47214ff8d4a113d7f4ecefc17734c3dc202 Mon Sep 17 00:00:00 2001
From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com>
Date: Fri, 4 Oct 2024 13:16:14 -0700
Subject: [PATCH] continuous state actions (#44)

* continous state actions

* spotless

* removed speaker/feeding from after transition
---
 src/main/java/frc/robot/arm/ArmSubsystem.java | 30 +++++++++++--------
 1 file changed, 17 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java
index f754cd1..3b9f33e 100644
--- 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,6 +175,7 @@ protected void afterTransition(ArmState newState) {
             positionRequest.withPosition(
                 Units.degreesToRotations(clamp(ArmAngle.PASS.getDegrees()))));
       }
+      default -> {}
     }
   }
 
@@ -195,6 +183,22 @@ protected void afterTransition(ArmState newState) {
   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());