get rid of unnecessary movement, still need to imcorporate wrist somehow

Yeti-Robotics · Mar 5, 2025 · d3f7f70 · d3f7f70
1 parent 41667ec
commit d3f7f70
Showing 1 changed file with 18 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/subsystems/coral/CoralManipulatorSystem.java b/src/main/java/frc/robot/subsystems/coral/CoralManipulatorSystem.java
@@ -84,32 +84,40 @@ protected StatusCode initializeTransition(CoralManipulatorState targetState) {
         Command coralManipulatorCommand;
 
         if (getCurrentState()
-                        .getElevatorPosition()
-                        .getHeight()
-                        .lt(ElevatorPosition.SAFE_POSITION.getHeight())
+                .getElevatorPosition()
+                .getHeight()
+                .lt(ElevatorPosition.SAFE_POSITION.getHeight())
                 && getCurrentState() != targetState) {
-            if (arm.getCurrentState().getAngle().lt(ArmPosition.AWAY.getAngle())
+            if (targetState == CoralManipulatorState.L2
+                    && arm.getCurrentState() != ArmPosition.DOWN) {
+                coralManipulatorCommand =
+                        arm.transitionTo(targetState.getArmPosition())
+                                .alongWith(elevator.transitionTo(targetState.getElevatorPosition()))
+                                .andThen(wrist.transitionTo(targetState.getWristPosition()))
+                                .andThen(grabber.transitionTo(targetState.getGrabberState()));
+            } else if (arm.getCurrentState().getAngle().lt(ArmPosition.AWAY.getAngle())
                     || targetState.getArmPosition().getAngle().lt(ArmPosition.AWAY.getAngle())) {
                 // safety stuff
                 coralManipulatorCommand =
                         elevator.transitionTo(ElevatorPosition.SAFE_POSITION)
+                                .andThen(wrist.transitionTo(targetState.getWristPosition()))
                                 .andThen(arm.transitionTo(targetState.getArmPosition()))
-                                .andThen(wrist.transitionTo(targetState.getWristPosition()));
-                elevator.transitionTo(targetState.getElevatorPosition())
-                        .andThen(grabber.transitionTo(targetState.getGrabberState()));
+                                .andThen(elevator.transitionTo(targetState.getElevatorPosition()))
+                                .andThen(grabber.transitionTo(targetState.getGrabberState()));
+
             } else {
                 // not safety stuff
                 coralManipulatorCommand =
-                        elevator.transitionTo(targetState.getElevatorPosition())
+                        arm.transitionTo(targetState.getArmPosition())
+                                .alongWith(elevator.transitionTo(targetState.getElevatorPosition()))
                                 .andThen(wrist.transitionTo(targetState.getWristPosition()))
-                                .andThen(arm.transitionTo(targetState.getArmPosition()))
                                 .andThen(grabber.transitionTo(targetState.getGrabberState()));
             }
         } else {
             coralManipulatorCommand =
                     wrist.transitionTo(targetState.getWristPosition())
                             .alongWith(elevator.transitionTo(targetState.getElevatorPosition()))
-                            .andThen(arm.transitionTo(targetState.getArmPosition()))
+                            .alongWith(arm.transitionTo(targetState.getArmPosition()))
                             .andThen(grabber.transitionTo(targetState.getGrabberState()));
         }