Robot Reveal

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

@@ -371,6 +371,8 @@ public final class Constants {
         public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
         public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
 
+        public static final double L2_ALGAE_REMOVAL = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(3);
+
 
 
         // Test april tag field layout
@@ -423,6 +425,7 @@ public final class Constants {
         //Max for endefector = 60%
 
         public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
+        public static final double DEALGAE_L2_EENDEFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR;
         public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
         public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
         public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value

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

@@ -334,6 +334,7 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
             .onTrue(AprilLidarAlign);
         
+
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
             .onTrue(AprilLidarAlign);
 
@@ -359,8 +360,17 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(AprilLidarAlignL3);
 
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
+            .onTrue(new SequentialCommandGroup(
+                new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
+                new waitEndefectorRefrence(m_robotElevator),
+                new waitElevatorRefrence(m_robotElevator),
+                new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
+                new LidarAlign(m_robotSwerveDrive, m_lidar),
+                new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
+                new MoveForTimeCommand(m_robotSwerveDrive, 
+                    new Translation2d(0,1), new Translation2d(), 500, true)
+            ));
         
             
         // ? /* Programer Buttons (Controller 3)*/

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

@@ -39,7 +39,7 @@ public class LidarAlign extends Command {
     this.currentFinderTick = 0;
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
-    this.headedRight = (GotoLastApril.tagRelativeXError < 0);
+    this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
   }
 
 
@@ -52,7 +52,7 @@ public class LidarAlign extends Command {
       return;
     }
 
-    if (currentFinderTick > 40) { //arbutrary threshhold for now.
+    if (currentFinderTick > 10) { //arbutrary threshhold for now.
       headedRight = !headedRight;
       currentFinderTick *= -1;
     }

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

@@ -130,6 +130,18 @@ public class Elevator extends SubsystemBase {
         break;
       }
 
+      case BallRemoverL2Primed: {
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFECTOR);
+        break;
+      }
+
+      case BallRemoverL2Go: {
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.DEALGAE_L2_EENDEFECTOR);
+        break;
+      }
+
       default: {
         assert false;
       }

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

@@ -304,8 +304,9 @@ public class SwerveDrive extends Subsystem {
     }
 
     public void stopModules() {
-        stopped = true;
-        swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
+        // stopped = true;
+        // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
+        softStop();
     }
 
     @Override

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

@@ -12,7 +12,8 @@ import frc4388.robot.Constants.FieldConstants;
 public class ReefPositionHelper {
     public enum Side {
         LEFT,
-        RIGHT
+        RIGHT,
+        CENTER
     }
 
     public static final Pose2d[] RED_TAGS = {
@@ -74,10 +75,22 @@ public class ReefPositionHelper {
 
     public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
         return offset(getNearestTag(position), 
-        (side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim,
+        getSide(side) + xtrim,
         ydistance); 
     }
 
+    public static double getSide(Side side){
+        switch(side) {
+            case LEFT:
+                return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
+            case RIGHT:
+                return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
+            case CENTER:
+                return 0;
+        }
+        return 0;
+    }
+
 
     public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
         Translation2d oldTranslation = oldPose.getTranslation();