Merge branch 'master' into driver-preference-controls

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

@@ -366,7 +366,7 @@ public final class Constants {
 
         // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
 
-        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17);
+        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
         public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
         public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
         public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
@@ -419,7 +419,7 @@ public final class Constants {
         //Max for elevator = 50%
         
         public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
-        public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
+        public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
         public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
         public static final double SCORING_THREE_ELEVATOR = -9.25;
         public static final double DEALGAE_L2_ELEVATOR = -23.5;

src/main/java/frc4388/robot/Robot.java

@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.CanDevice;
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.DeferredBlockMulti;
 import frc4388.utility.RobotTime;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
@@ -113,6 +114,7 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledExit() {
     DeferredBlock.execute();
+    DeferredBlockMulti.execute();
   }
 
   /**

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

@@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.DriverStation;
 import java.io.File;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.function.BooleanSupplier;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -42,6 +43,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
 // Autos
 import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoLastApril;
+import frc4388.robot.commands.IfCommand;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.waitElevatorRefrence;
@@ -64,8 +66,10 @@ import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.DeferredBlockMulti;
 import frc4388.utility.ReefPositionHelper;
 import frc4388.utility.Subsystem;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.configurable.ConfigurableString;
 
@@ -118,11 +122,15 @@ public class RobotContainer {
     //        true, false);
     // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
     private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
 
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
+        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),
 
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -137,35 +145,51 @@ public class RobotContainer {
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), 500, true),
         
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
     private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
-        // new InstantCommand(() -> System.out.println("Soup")),
-        // new WaitCommand(1),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+
+        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 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, FieldConstants.L4_DISTANCE_1, Side.LEFT),
+
         new LidarAlign(m_robotSwerveDrive, m_lidar),
+
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
+
         new waitEndefectorRefrence(m_robotElevator),
+
         new MoveForTimeCommand(m_robotSwerveDrive, 
             new Translation2d(0,1), new Translation2d(), 500, true),
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
+
+
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
     private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
-        // new InstantCommand(() -> System.out.println("Soup")),
-        // new WaitCommand(1),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+
+        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 waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
+
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -173,13 +197,19 @@ public class RobotContainer {
         new waitElevatorRefrence(m_robotElevator),
         // new MoveForTimeCommand(m_robotSwerveDrive, 
         // new Translation2d(0,1), new Translation2d(), 500, true),
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
     private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
+        
+        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 InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
 
         new waitEndefectorRefrence(m_robotElevator),
@@ -195,11 +225,12 @@ public class RobotContainer {
         new waitElevatorRefrence(m_robotElevator),
         // new MoveForTimeCommand(m_robotSwerveDrive, 
         // new Translation2d(0,1), new Translation2d(), 500, true),
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
+        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
     
     private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
         
         new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -208,12 +239,13 @@ public class RobotContainer {
         new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), 500, true),
-        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
 
     );
 
     private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
         
         new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -222,8 +254,8 @@ public class RobotContainer {
         new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), 500, true),
-        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
-
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
     private Command placeCoral = new SequentialCommandGroup(
@@ -245,7 +277,7 @@ public class RobotContainer {
     );
 
     private Command leftAlgaeRemove = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -254,11 +286,12 @@ public class RobotContainer {
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), 500, true),
-        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
     private Command rightAlgaeRemove = new SequentialCommandGroup(
-        new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
+        new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
@@ -267,7 +300,8 @@ public class RobotContainer {
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
         new Translation2d(0,1), new Translation2d(), 500, true),
-        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
+        new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
+        new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
     
 
@@ -293,7 +327,12 @@ public class RobotContainer {
 
         configureButtonBindings();        
         configureVirtualButtonBindings();
-        new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
+        new DeferredBlock(() -> { // Called on first robot enable
+            m_robotSwerveDrive.resetGyro();
+        });
+        new DeferredBlockMulti(() -> { // Called on every robot enable
+            TimesNegativeOne.update();
+        });
         DriverStation.silenceJoystickConnectionWarning(true);
         // CameraServer.startAutomaticCapture();
 
@@ -451,9 +490,13 @@ public class RobotContainer {
                 .onTrue(rightAlgaeRemove);
             
 
-            // Cancel button
-            new JoystickButton(getButtonBox(), ButtonBox.White)
-                .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
+        // Cancel button
+        new JoystickButton(getButtonBox(), ButtonBox.White)
+            .onTrue(new InstantCommand(() -> {
+                m_robotElevator.elevatorStop(); 
+                m_robotElevator.endefectorStop();
+                m_robotSwerveDrive.endSlowPeriod();
+            }, m_robotElevator));
 
             // Score prep        
             // Prime 4
@@ -616,7 +659,7 @@ public class RobotContainer {
     //     return autoCommand;
 	// } catch (Exception e) {
 	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
-	    return Commands.none();
+	    return autoCommand;
 	// }
     // return new PathPlannerAuto("Line-up-no-arm");
 	// zach told me to do the below comment
@@ -638,9 +681,9 @@ public class RobotContainer {
 
         autoChooser.onChange((filename) -> {
             autoCommand = new PathPlannerAuto(filename);
-            System.out.println("Robot Auto Changed");
+            System.out.println("Robot Auto Changed " + filename);
         });
-        SmartDashboard.putData(autoChooser);
+        // SmartDashboard.putData(autoChooser);
 
     }
 

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

@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Vision;
 import frc4388.utility.Gains;
 import frc4388.utility.ReefPositionHelper;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
 import frc4388.utility.UtilityStructs.AutoRecordingFrame;
@@ -75,19 +76,26 @@ public class GotoLastApril extends Command {
 
     @Override
     public void execute() {
-        
-        if (alliance == Alliance.Red) {
-            xerr = -(targetpos.getX() - vision.getPose2d().getX());
-            yerr = (targetpos.getY() - vision.getPose2d().getY());
-        }
-        else{
-            
-            xerr = targetpos.getX() - vision.getPose2d().getX();
-            yerr = targetpos.getY() - vision.getPose2d().getY();
+        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
+        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
+        // xerr = targetpos.getX() - vision.getPose2d().getX();
+        // yerr = targetpos.getX() - vision.getPose2d().getY();
+
+        // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
+
+        roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
+
+        boolean invert = Math.abs(roterr) > 180;
+
+        if(roterr > 180){
+            roterr -= 360;
+        }else if(roterr < -180){
+            roterr += 360;
         }
 
-            
-        roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
+        SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
+        SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
+        SmartDashboard.putNumber("Rotational PID error", roterr);
 
         // SmartDashboard.putNumber("PID X Error", xerr);
         // SmartDashboard.putNumber("PID Y Error", yerr);

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

@@ -0,0 +1,33 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+// Command that is called if something is true
+public class IfCommand extends InstantCommand {
+    BooleanSupplier isTrue;
+    Command trueCommand;
+    Command falseCommand;
+
+    public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){
+        this.isTrue = isTrue;
+        this.trueCommand = trueCommand;
+        this.falseCommand = falseCommand;
+    }
+
+    public IfCommand(BooleanSupplier isTrue, Command trueCommand){
+        this(isTrue, trueCommand, Commands.none());
+    }
+
+    @Override
+    public void execute() {
+        if(isTrue.getAsBoolean())
+            trueCommand.schedule();
+        else
+            falseCommand.schedule();
+    }
+}

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

@@ -21,6 +21,7 @@ public class LidarAlign extends Command {
   private boolean foundReef;
   private boolean headedRight;
   private double speed;
+  private int bounces;
   // private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
@@ -40,6 +41,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.bounces = 0;
   }
 
 
@@ -55,6 +57,8 @@ public class LidarAlign extends Command {
     if (currentFinderTick > 10) { //arbutrary threshhold for now.
       headedRight = !headedRight;
       currentFinderTick *= -1;
+      bounces++;
+      if (bounces == 2) return;
     }
     double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
     double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
@@ -78,7 +82,10 @@ public class LidarAlign extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (foundReef && lidar.withinDistance()) { // spot on
+    if (lidar.getDistance() < 4) {
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && lidar.withinDistance()) { // spot on
       swerveDrive.stopModules();
       return true;
     } else if (foundReef && !lidar.withinDistance()) { // over shot
@@ -87,6 +94,9 @@ public class LidarAlign extends Command {
       currentFinderTick = 0;
       foundReef = false;
       return false;
+    } else if (bounces == 2) {
+      swerveDrive.stopModules();
+      return true;
     } else {
       return false;
     }

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

@@ -5,6 +5,7 @@ import java.time.Instant;
 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 MoveForTimeCommand extends Command {

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

@@ -181,20 +181,26 @@ public class Elevator extends SubsystemBase {
   public boolean elevatorAtReference() {
     // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
     double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
-    boolean atPos = Math.abs(elevatorRefrence - elevatorPosition) <= 0.5;
-    if (atPos) {
-      SmartDashboard.putNumber("Elevator Refrence", elevatorRefrence);
-      SmartDashboard.putNumber("Elevator Pos", elevatorPosition);
-    }
+    double diffrence = elevatorRefrence - elevatorPosition;
 
-    return atPos;
+    boolean headedUp = diffrence < 0;
+    boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
+    boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
   }
 
   public boolean endeffectorAtReference() {
     // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
     double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
+    double endefectorPosition = endefectorMotor.getPosition().getValueAsDouble();
+    double diffrence = endefectorRefrence - endefectorPosition;
 
-    return Math.abs(endeffectorRefrence - endeffectorPosition) <= 0.2;
+    boolean headedUp = diffrence < 0;
+    boolean forwardLimit = endefectorMotor.getForwardLimit().asSupplier().get().value == 0;
+    boolean reverseLimit = endefectorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
   }
   // public void driveElevatorStick(Translation2d stick) {
   //   if (stick.getNorm() > 0.05) {
@@ -261,6 +267,14 @@ public class Elevator extends SubsystemBase {
     // }
   }
 
+  public boolean isL4Primed(){
+    return currentState == CoordinationState.PrimedFour;
+  }
+
+  public boolean isL3Primed(){
+    return currentState == CoordinationState.PrimedThree;
+  }
+
   public void armShuffle(){
     if(!basinBeamBreak.get()){
       //shuffle the coral with the arm until coral hits beam break

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

@@ -72,7 +72,7 @@ public class Lidar extends Subsystem {
         if(distance == -1){
             s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED");
         }else{
-            s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED");
+            s.addReport(ReportLevel.INFO, "LIDAR CONNECTED");
         }
         return s;
     }

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

@@ -35,9 +35,8 @@ import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
+import frc4388.utility.TimesNegativeOne;
 import frc4388.utility.Status.ReportLevel;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.config.PIDConstants;
@@ -103,11 +102,11 @@ public class SwerveDrive extends Subsystem {
                     // This will flip the path being followed to the red side of the field.
                     // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
 
-                    var alliance = DriverStation.getAlliance();
-                    if (alliance.isPresent()) {
-                        return alliance.get() == DriverStation.Alliance.Red;
-                    }
-                    return false;
+                    // var alliance = DriverStation.getAlliance();
+                    // if (alliance.isPresent()) {
+                    //     return alliance.get() == DriverStation.Alliance.Red;
+                    // }
+                    return TimesNegativeOne.isRed;
                 },
                 this // Reference to this subsystem to set requirements
         );
@@ -140,23 +139,14 @@ public class SwerveDrive extends Subsystem {
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
             return; // don't bother doing swerve drive math and return early.
 
-        leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
-
-        Optional<Alliance> alliance = DriverStation.getAlliance();
-        
-        if(!alliance.isEmpty()){
-            if (alliance.get() == Alliance.Red) 
-                leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
-            else 
-                leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
-            // if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
-        }
-        if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
-
-
+            leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         
         stopped = false;
         if (fieldRelative) {
+            
+            leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+            rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);    
+
             // ! drift correction
             if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
                 rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
@@ -218,7 +208,7 @@ public class SwerveDrive extends Subsystem {
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
             return; // don't bother doing swerve drive math and return early.
 
-        leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+        leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * speedAdjust)
@@ -282,7 +272,7 @@ public class SwerveDrive extends Subsystem {
         // if (leftStick.getNorm() < 0.05) //if no imput
         // return; // don't bother doing swerve drive math and return early.
 
-        leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * -speedAdjust)
@@ -390,6 +380,18 @@ public class SwerveDrive extends Subsystem {
         rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
     }
 
+    private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
+
+    public void startSlowPeriod() {
+        tmp_gear_index = gear_index;
+        setToSlow();
+    }
+
+    public void endSlowPeriod() {
+        setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
+        gear_index = tmp_gear_index;
+    }
+
     @Override
     public String getSubsystemName() {
         return "Swerve Drive Controller";

src/main/java/frc4388/utility/Alliance.java

@@ -0,0 +1,5 @@
+package frc4388.utility;
+
+public class Alliance {
+
+}

src/main/java/frc4388/utility/DeferredBlockMulti.java

@@ -0,0 +1,18 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DeferredBlockMulti {
+    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
+
+    public DeferredBlockMulti(Runnable block) {
+        m_blocks.add(block);
+    }
+
+    public static void execute() {
+
+        for (Runnable block : m_blocks) {
+            block.run();
+        }
+    }
+}

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -63,14 +63,11 @@ public class ReefPositionHelper {
      * Function to find closest tag location based on side
      */
     public static Pose2d getNearestTag(Pose2d position) {
-        Optional<Alliance> ally = DriverStation.getAlliance();
-        if (!ally.isPresent()) 
-            return new Pose2d();
-        if (ally.get() == Alliance.Red) 
+
+        if(TimesNegativeOne.isRed)
             return getNearestTag(RED_TAGS, position);
-        if (ally.get() == Alliance.Blue) 
-            return getNearestTag(BLUE_TAGS, position);   
-        return new Pose2d();
+        else
+            return getNearestTag(BLUE_TAGS, position);
     }
 
     public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {

src/main/java/frc4388/utility/TimesNegativeOne.java

@@ -0,0 +1,51 @@
+package frc4388.utility;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.Constants.SwerveDriveConstants;
+
+// Class that holds weather the drivers sticks should be inverted
+public class TimesNegativeOne {
+
+    public static boolean XAxis = SwerveDriveConstants.INVERT_X;
+    public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
+    public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+    public static boolean isRed = false;
+    public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
+
+    private static boolean isRed() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if(alliance.isEmpty()) return false;
+        return (alliance.get() == Alliance.Red);
+    }
+
+    public static void update(){
+        isRed = isRed();
+        XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
+        YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
+        RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+        ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
+        SmartDashboard.putBoolean("Is red alliance", isRed);
+    }
+
+    public static double invert(double num, boolean invert){
+        return invert ? -num : num;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertXY){
+        if(invertXY) return stick.times(-1);
+        else return stick;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
+        return new Translation2d(
+            invert(stick.getX(), invertX), 
+            invert(stick.getY(), invertY)
+        );
+    }
+}