Fix limit switch and maybe autos

src/main/deploy/pathplanner/autos/Line-up-no-arm.auto

@@ -13,7 +13,7 @@
         {
           "type": "named",
           "data": {
-            "name": "lineup-no-arm"
+            "name": "place-coral-left-l4"
           }
         }
       ]

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

@@ -377,6 +377,13 @@ public final class Constants {
         
         public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
 
+        public static final int L4_DRIVE_TIME = 250; //Milliseconds
+        // public static final int L3_DRIVE_TIME = 500;
+        public static final int L2_DRIVE_TIME = 250; //Milliseconds
+
+        
+        public static final int ALGAE_DRIVE_TIME = 500;
+
 
 
         // Test april tag field layout

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

@@ -124,10 +124,10 @@ public class RobotContainer {
     private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
 
-        new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
+        // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
-        )),
+        // )),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
         // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
@@ -143,7 +143,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
 
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), 500, true),
+        new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
         
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -152,10 +152,10 @@ public class RobotContainer {
     private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
 
-        new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
+        // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
-        )),
+        // )),
         
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
         // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -172,7 +172,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
 
         new MoveForTimeCommand(m_robotSwerveDrive, 
-            new Translation2d(0,1), new Translation2d(), 500, true),
+            new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
 
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
@@ -182,10 +182,10 @@ public class RobotContainer {
     private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
 
-        new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
+        // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
-        )),
+        // )),
 
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -204,10 +204,10 @@ public class RobotContainer {
     private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         
-        new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
+        // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT)
+            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
-        )),
+        // )),
         
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
@@ -238,7 +238,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), 500, true),
+        new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
 
@@ -253,7 +253,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), 500, true),
+        new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
@@ -313,10 +313,10 @@ public class RobotContainer {
         
         NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
         NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
-        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL3Left);
+        NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
-        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL3Right);
+        NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
-        NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL2Left);
+        NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
-        NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL2Right);
+        NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
 
         NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
             new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -659,9 +659,9 @@ public class RobotContainer {
     //     return autoCommand;
 	// } catch (Exception e) {
 	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
-	    return autoCommand;
+	    // return autoCommand;
 	// }
-    // return new PathPlannerAuto("Line-up-no-arm");
+    return new PathPlannerAuto("Line-up-no-arm");
 	// zach told me to do the below comment
 	//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
       //  return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);

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

@@ -22,6 +22,7 @@ public class LidarAlign extends Command {
   private boolean headedRight;
   private double speed;
   private int bounces;
+  private double additionalDistance = 0;
   // private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
@@ -41,6 +42,7 @@ public class LidarAlign extends Command {
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
     this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
+    this.additionalDistance = 0;
     this.bounces = 0;
   }
 
@@ -54,11 +56,12 @@ public class LidarAlign extends Command {
       return;
     }
 
-    if (currentFinderTick > 10) { //arbutrary threshhold for now.
+    if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
       headedRight = !headedRight;
       currentFinderTick *= -1;
       bounces++;
-      if (bounces == 2) return;
+      additionalDistance += 5;
+      if (bounces == 5) return;
     }
     double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
     double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef

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

@@ -229,6 +229,8 @@ public class Elevator extends SubsystemBase {
   private void periodicWaiting() {
     if (!basinBeamBreak.get()) 
       transitionState(CoordinationState.Ready);
+    if(!endeffectorLimitSwitch.get())
+      transitionState(CoordinationState.Hovering);
   }
 
   // private void periodicWaitingTripped() {
@@ -237,12 +239,15 @@ public class Elevator extends SubsystemBase {
   // }
   
   private void periodicReady() {
-    if (elevatorAtReference())
+    if (elevatorAtReference() && !endeffectorLimitSwitch.get())
       transitionState(CoordinationState.Hovering);
+    if(elevatorAtReference() && endeffectorLimitSwitch.get())
+      transitionState(CoordinationState.Waiting);
   }
 
   private void periodicScoring() {
-    if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting);
+    if (!endeffectorLimitSwitch.get()) 
+      transitionState(CoordinationState.Waiting);
   }
 
   public void manualElevatorVel(double velocity) {