end of day one

src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto

@@ -19,7 +19,7 @@
         {
           "type": "named",
           "data": {
-            "name": "place-coral-left-l4"
+            "name": "place-coral-right-l4"
           }
         },
         {
@@ -46,6 +46,12 @@
             "pathName": "Bottom Feed to Reef 2"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -76,6 +82,12 @@
             "pathName": "Bottom Feed to Reef 2"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "await-coral"
+          }
+        },
         {
           "type": "named",
           "data": {

src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path

@@ -41,7 +41,7 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 0.0,
     "rotation": 59.28109573597083
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Cage 5 to Reef.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.959532828282828,
+        "x": 6.670197826108406,
-        "y": 1.5754419191919191
+        "y": 2.0396270358261894
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.349684343434343,
+        "x": 5.754837328767123,
-        "y": 2.2869318181818175
+        "y": 2.492380136986301
       },
       "prevControl": {
-        "x": 5.747433712121213,
+        "x": 6.077263063386753,
-        "y": 1.9321212121212112
+        "y": 2.432264238502225
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,18 +30,7 @@
   ],
   "rotationTargets": [],
   "constraintZones": [],
-  "pointTowardsZones": [
+  "pointTowardsZones": [],
-    {
-      "fieldPosition": {
-        "x": 4.8,
-        "y": 3.0
-      },
-      "rotationOffset": 0.0,
-      "minWaypointRelativePos": 0.0,
-      "maxWaypointRelativePos": 1.0,
-      "name": "Point Towards Zone"
-    }
-  ],
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
@@ -52,14 +41,14 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 1.0,
-    "rotation": 122.79876573246848
+    "rotation": 119.99999999999999
   },
   "reversed": false,
   "folder": "Barge to Reef",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 126.34745820888533
+    "rotation": 123.5866498700801
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path

@@ -41,7 +41,7 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 1.0,
     "rotation": 55.37584492005105
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.703267045454546,
+        "x": 4.106435503617635,
-        "y": 1.075625
+        "y": 1.8391383607824139
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 1.3559375
       },
       "prevControl": {
-        "x": 2.0014457070707063,
+        "x": 2.9441094150144833,
-        "y": 2.5936426767676766
+        "y": 1.668218251495801
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,15 +33,15 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 5.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 1.0,
     "rotation": 55.05578949900953
   },
   "reversed": false,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": 121.42956561483854
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path

@@ -41,7 +41,7 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 1.0,
     "rotation": 52.857102599919905
   },
   "reversed": false,

src/main/java/frc4388/robot/Constants.java

@@ -54,6 +54,7 @@ import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.ReefPositionHelper;
 import frc4388.utility.Trim;
+import frc4388.utility.configurable.ConfigurableDouble;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -193,7 +194,7 @@ public final class Constants {
                 .withKS(0).withKV(0.124);
             
             public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
-            public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(2.5, 0.2, 1);
+            public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
         }
 
         public static final class Configurations {
@@ -298,7 +299,11 @@ public final class Constants {
     }
 
     public static final class AutoConstants {
-        public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
+        // public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
+        public static final Gains XY_GAINS = new Gains(8,0,0.0);
+        // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
+        // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
+        // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
        // public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
 
         // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
@@ -316,24 +321,37 @@ public final class Constants {
         public static final double MIN_XY_PID_OUTPUT = 0.0;
         public static final double MIN_ROT_PID_OUTPUT = 0.0;
 
-        public static final double VELOCITY_THRESHHOLD = 0.005;
+        public static final double VELOCITY_THRESHHOLD = 0.01;
                 
         // X is tangent to reef side
         // Y is normal to reef side
         public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
         public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
+        public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
 
-        public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
-        public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
+        public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
-        // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
+        // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
         
-        public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
-        public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
+        public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
         
-        public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
-        public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
+        public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5);
 
-        public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
+        public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
+        
+        // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
+        // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
+        
+        // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
+        
+        // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
+
+        // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
 
         public static final int L4_DRIVE_TIME = 250; //Milliseconds
         // public static final int L3_DRIVE_TIME = 500;
@@ -437,7 +455,8 @@ public final class Constants {
         public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
         public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
-        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value
+        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
 
         public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()

src/main/java/frc4388/robot/RobotContainer.java

@@ -38,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 // Commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -49,6 +50,7 @@ import frc4388.robot.commands.DriveUntilLiDAR;
 import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
+import frc4388.robot.commands.MoveUntilSuply;
 import frc4388.robot.commands.WhileTrueCommand;
 import frc4388.robot.commands.waitElevatorRefrence;
 import frc4388.robot.commands.waitEndefectorRefrence;
@@ -148,6 +150,48 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
+        waitDebuger.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
+        new ParallelDeadlineGroup(
+            new WaitCommand(1),
+            new MoveUntilSuply(
+                m_robotSwerveDrive, 
+                new Translation2d(0,-0.5), 
+                new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
+        ),
+        new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator),
+
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+
+        new waitEndefectorRefrence(m_robotElevator),
+
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+        new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
+
+        new ConditionalCommand(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+             () -> m_robotElevator.hasCoral()),
+
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    );
+
+    /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+        
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
+        ), () -> m_robotElevator.isL4Primed()),
+
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
         waitDebuger.asProxy(),
         new LidarAlign(m_robotSwerveDrive, m_reefLidar),
@@ -204,6 +248,53 @@ public class RobotContainer {
 
 
 
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
+    ); */
+
+    private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+
+        // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
+
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
+        ), () -> m_robotElevator.isL4Primed()),
+        
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+
+        new waitEndefectorRefrence(m_robotElevator),
+        new waitElevatorRefrence(m_robotElevator),
+
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
+        waitDebuger.asProxy(),
+        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        waitDebuger.asProxy(),
+        new ParallelDeadlineGroup(
+            new WaitCommand(1),
+            new MoveUntilSuply(
+                m_robotSwerveDrive, 
+                new Translation2d(0,-0.5), 
+                new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
+        ),
+        new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator),
+        
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+
+        new waitEndefectorRefrence(m_robotElevator),
+
+        
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+            new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
+
+        new ConditionalCommand(
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+            new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+                () -> m_robotElevator.hasCoral()),
+
+
+
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
@@ -408,7 +499,7 @@ public class RobotContainer {
                 new Translation2d(0, 1), 
                 new Translation2d(), 300, true
                 
-        ), new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
+        ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
         new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
         
         NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
@@ -421,7 +512,7 @@ public class RobotContainer {
         NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup(
             // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)),
             // new waitElevatorRefrence(m_robotElevator),
-            new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
+            // new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)),
             new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour))
         ));
 
@@ -545,14 +636,15 @@ public class RobotContainer {
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(new DriveUntilLiDAR(m_robotSwerveDrive, 
+               .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, 
-            new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
+               new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
+        //     .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true));
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
             .onTrue(thrustIntake.asProxy());
           
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar)); 
+            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar)); 
         
             
         // ?  /* Operator Buttons */

src/main/java/frc4388/robot/RobotMap.java

@@ -55,11 +55,14 @@ public class RobotMap {
     /* Elevator Subsystem */
     public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
     public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
+    
 
     public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
     public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
     public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
-    void configureDriveMotorControllers() {}
+    void configureDriveMotorControllers() {
+        // endeffector.saf
+    }
         
 }

src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java

@@ -45,6 +45,10 @@ public class DriveUntilLiDAR extends Command {
 
     @Override
     public boolean isFinished() {
-        return Math.abs(m_lidar.getDistance()) < mindistance;
+        if (Math.abs(m_lidar.getDistance()) < mindistance) {
+            swerveDrive.softStop();
+            return true;
+        }
+        return false;
     }
 }

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -50,8 +50,8 @@ public class GotoLastApril extends Command {
         this.vision = vision;
         this.distance = distance;
         this.side = side;
-        this.waitVelocityZero = waitVelocityZero;
+        this.waitVelocityZero = waitVelocityZero && false;
-        // addRequirements(swerveDrive);
+        addRequirements(swerveDrive);
     }
 
 
@@ -62,6 +62,23 @@ public class GotoLastApril extends Command {
 
     @Override
     public void initialize() {
+        // double kP = AutoConstants.P_XY_GAINS.get();
+        // double kI = AutoConstants.I_XY_GAINS.get();
+        // double kD = AutoConstants.D_XY_GAINS.get();
+        // xPID = new PID(new Gains(
+        //     kP, 
+        //     kI, 
+        //     kD), 
+        // 0);
+        // yPID = new PID(new Gains(
+        //     kP, 
+        //     kI, 
+        //     kD),
+        // 0);
+
+        // System.out.println("kP: "+kP);
+        // System.out.println("kI: "+kI);
+        // System.out.println("kD: "+kD);
         xPID.initialize();
         yPID.initialize();
         this.targetpos = ReefPositionHelper.getNearestPosition(

src/main/java/frc4388/robot/commands/MoveUntilSuply.java

@@ -0,0 +1,47 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.TimesNegativeOne;
+
+// Command to repeat a joystick movement for a specific time.
+public class MoveUntilSuply extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final Supplier<Boolean> truth;
+    private final boolean robotRelative;
+
+    public MoveUntilSuply(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        Supplier<Boolean> truth,
+        boolean robotRelative) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.truth = truth;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return truth.get();
+    }
+}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -162,7 +162,7 @@ public class Elevator extends Subsystem {
       
       case PrimedFour: {
         PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR);
+        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
         led.setMode(LEDConstants.SCORING_PATTERN);
         break;
       }
@@ -255,6 +255,10 @@ public class Elevator extends Subsystem {
   //   }
   // }
 
+  public boolean getEndeffectorLimit() {
+    return endeffectorLimitSwitch.get();
+  }
+
   private void periodicWaiting() {
     if (!basinBeamBreak.get()) 
       transitionState(CoordinationState.Ready);
@@ -363,7 +367,7 @@ public class Elevator extends Subsystem {
   }
 
   public boolean hasCoral() {
-    return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get();
+    return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
   }
 
   public boolean readyToMove() {