Slight Refactoring

Author: EvanWoodfin

src/main/java/frc/robot/Constants.java

@@ -394,10 +394,8 @@ public static final class WristConstants{
 
       public static double zeroOffset = 1.0 - ((5.05128918995 - (Math.PI / 2.0)) / (2.0 * Math.PI));
 
-      public static double freeHangAngle = 90;
-
       public static enum WristAngleRad {
-        FREEHANG(Units.degreesToRadians(freeHangAngle)),
+        FREEHANG(Units.degreesToRadians(90)),
         FUNNEL_ANGLE(Units.degreesToRadians(66)),
         L1(Units.degreesToRadians(252)),  //  Needs to be changed
         L2L3(Units.degreesToRadians(252)),

src/main/java/frc/robot/RobotContainer.java

@@ -106,7 +106,6 @@ public RobotContainer() {
 
 
         //  Should get called right after subsystems are instantiated
-        addAllProfiledSubsystemsToArray();
 
 
         constructorThings();
@@ -248,28 +247,17 @@ private void registerNamedCommands(){
 
     private Command robotGoToHeightCommandCreator(ElevatorHeight height){
 
-        Command command;
-
-        boolean elevatorIsAlreadyAtTheBottomAndWeAreTryingToMoveThere = false;
-            // height == ElevatorHeight.HOME &&
-            // elevator.getCurrentGoal() == ElevatorHeight.HOME;
 
+        Command command;
 
-        if(elevatorIsAlreadyAtTheBottomAndWeAreTryingToMoveThere){
-            //  Should end immediately
-            command = Commands.sequence(
-                new ElevatorGoToPositionPositionCommand(elevator, height),
-                new WristGoToPositionCommand(wrist, getEndWristAngleForGivenElevatorHeight(height))
-            ).withInterruptBehavior(InterruptionBehavior.kCancelSelf);
-        }else{
+        //INEFFICIENT FOR GETTING FROM CORAL STATION
             command = Commands.sequence(
                 new WristGoToPositionCommand(wrist, WristAngleRad.L2L3),
                 new ElevatorGoToPositionPositionCommand(elevator, height),
                 new WristGoToPositionCommand(wrist, getEndWristAngleForGivenElevatorHeight(height))
             ).withInterruptBehavior(InterruptionBehavior.kCancelSelf);
-        }
-
 
+       
         return command;
 
     }
@@ -295,49 +283,6 @@ private WristAngleRad getEndWristAngleForGivenElevatorHeight(ElevatorHeight heig
     }
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
     // private void testingButtonBindings() {
 
     //     configureDriveBindings();
@@ -383,47 +328,11 @@ public Command getAutonomousCommand() {
     }
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-    private void addAllProfiledSubsystemsToArray(){
-        subsystems = new IProfiledReset[]{
-            wrist,
-            manipulator,
-            elevator
-        };
-    }
-
     //  Should get called in teleopInit, and maybe autonomousInit
     public void ResetProfiledSubsystemsOnEnable(){
-        for (IProfiledReset sub : subsystems) {
-            sub.ProfileReset();
-        }
+        wrist.ProfileReset();
+        elevator.ProfileReset();
+        manipulator.ProfileReset();
     }