Auto Placing Logic

src/main/java/frc/robot/Robot.java

@@ -52,7 +52,9 @@ public void robotPeriodic() {
   }
 
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+    m_robotContainer.stopTagAlign();
+  }
 
   @Override
   public void disabledPeriodic() {}

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

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.algae.IntakeAlgaeCommand;
@@ -28,6 +29,7 @@
 import frc.robot.commands.elevator.JogElevatorCommand;
 import frc.robot.commands.elevator.SetElevatorPositionCommand;
 import frc.robot.commands.elevator.ZeroElevatorCommand;
+import frc.robot.commands.robotState.AutoReefCycleCommand;
 import frc.robot.commands.robotState.FloorAlgaeCommand;
 import frc.robot.commands.robotState.HPAlgaeCommand;
 import frc.robot.commands.robotState.InterruptAutoCommand;
@@ -182,10 +184,18 @@ private void configureDriverBindings() {
     new JoystickButton(driveJoystick, Button.SWD.id)
         .onTrue(
             new StowCommand(
-                robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
+                robotStateSubsystem,
+                elevatorSubsystem,
+                coralSubsystem,
+                biscuitSubsystem,
+                algaeSubsystem))
         .onFalse(
             new StowCommand(
-                robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
+                robotStateSubsystem,
+                elevatorSubsystem,
+                coralSubsystem,
+                biscuitSubsystem,
+                algaeSubsystem));
     new JoystickButton(driveJoystick, Button.SWA.id)
         .onTrue(new InterruptAutoCommand(robotStateSubsystem))
         .onFalse(new InterruptAutoCommand(robotStateSubsystem));
@@ -197,14 +207,26 @@ private void configureDriverBindings() {
         .onFalse(new ZeroElevatorCommand(elevatorSubsystem));
 
     // other stuff
+
     new JoystickButton(driveJoystick, Button.M_SWH.id)
         .onTrue(
-            new ReefCycleCommand(
-                robotStateSubsystem,
-                elevatorSubsystem,
-                coralSubsystem,
-                biscuitSubsystem,
-                algaeSubsystem));
+            new ConditionalCommand(
+                new AutoReefCycleCommand(
+                    robotStateSubsystem,
+                    elevatorSubsystem,
+                    coralSubsystem,
+                    driveSubsystem,
+                    tagAlignSubsystem,
+                    biscuitSubsystem,
+                    algaeSubsystem),
+                new ReefCycleCommand(
+                    robotStateSubsystem,
+                    elevatorSubsystem,
+                    coralSubsystem,
+                    biscuitSubsystem,
+                    algaeSubsystem),
+                () -> robotStateSubsystem.getIsAutoPlacing()));
+
     new JoystickButton(driveJoystick, Button.M_SWE.id)
         .onTrue(
             new ScoreAlgaeCommand(
@@ -226,6 +248,22 @@ private void configureDriverBindings() {
   }
 
   private void configureOperatorBindings() {
+    // Set Levels
+    new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
+        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
+    new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
+        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L2));
+    new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
+        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L3));
+    new Trigger(() -> xboxController.getRightTriggerAxis() > RobotConstants.kTriggerDeadband)
+        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4));
+
+    // Set scoring side
+    // new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
+    //     .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.LEFT));
+    // new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
+    //     .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.RIGHT));
+
     // Move biscuit
     new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
         .onTrue(
@@ -250,21 +288,15 @@ private void configureOperatorBindings() {
                 elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
         .onFalse(new HoldElevatorCommand(elevatorSubsystem));
 
-    // Set Levels
-    new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
-        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
-    new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
-        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L2));
-    new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
-        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L3));
-    new Trigger(() -> xboxController.getRightTriggerAxis() > RobotConstants.kTriggerDeadband)
-        .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4));
-
     // Stow
     new JoystickButton(xboxController, XboxController.Button.kBack.value)
         .onTrue(
             new StowCommand(
-                robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
+                robotStateSubsystem,
+                elevatorSubsystem,
+                coralSubsystem,
+                biscuitSubsystem,
+                algaeSubsystem));
 
     // Algae
     new JoystickButton(xboxController, XboxController.Button.kY.value)
@@ -400,4 +432,8 @@ public void configurePitDashboard() {
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }
+
+  public void stopTagAlign() {
+    tagAlignSubsystem.terminate();
+  }
 }

src/main/java/frc/robot/commands/drive/ResetOdometryCommand.java

@@ -0,0 +1,20 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class ResetOdometryCommand extends InstantCommand {
+  private DriveSubsystem driveSubsystem;
+  private Pose2d pose;
+
+  public ResetOdometryCommand(DriveSubsystem driveSubsystem, Pose2d pose) {
+    this.driveSubsystem = driveSubsystem;
+    this.pose = pose;
+  }
+
+  @Override
+  public void initialize() {
+    driveSubsystem.resetOdometry(pose);
+  }
+}

src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java

@@ -0,0 +1,51 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+
+public class AutoReefCycleCommand extends Command {
+  private RobotStateSubsystem robotStateSubsystem;
+  private TagAlignSubsystem tagAlignSubsystem;
+  private DriveSubsystem driveSubsystem;
+  private boolean scoringCoral;
+
+  public AutoReefCycleCommand(
+      RobotStateSubsystem robotStateSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      CoralSubsystem coralSubsystem,
+      DriveSubsystem driveSubsystem,
+      TagAlignSubsystem tagAlignSubsystem,
+      BiscuitSubsystem biscuitSubsystem,
+      AlgaeSubsystem algaeSubsystem) {
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.tagAlignSubsystem = tagAlignSubsystem;
+    this.driveSubsystem = driveSubsystem;
+
+    addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    driveSubsystem.setIgnoreSticks(true);
+    robotStateSubsystem.toReefAlign();
+    scoringCoral = robotStateSubsystem.hasCoral();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    tagAlignSubsystem.terminate();
+    driveSubsystem.setIgnoreSticks(false);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return scoringCoral && !robotStateSubsystem.hasCoral()
+        || !scoringCoral && robotStateSubsystem.hasAlgae();
+  }
+}

src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java

@@ -4,13 +4,15 @@
 import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
 
 public class ReefCycleCommand extends Command {
   private RobotStateSubsystem robotStateSubsystem;
   private ElevatorSubsystem elevatorSubsystem;
+  private DriveSubsystem driveSubsystem;
   private RobotStates startingRobotState;
   private boolean startingElevatorFinished;
   private boolean isAutoPlacing;
@@ -23,14 +25,16 @@ public ReefCycleCommand(
       AlgaeSubsystem algaeSubsystem) {
     this.robotStateSubsystem = robotStateSubsystem;
     this.elevatorSubsystem = elevatorSubsystem;
+
     addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem);
   }
 
   @Override
   public void initialize() {
     startingRobotState = robotStateSubsystem.getState();
     startingElevatorFinished = elevatorSubsystem.isFinished();
-    isAutoPlacing = robotStateSubsystem.getAutoPlaceOnCycle();
+    isAutoPlacing = robotStateSubsystem.getIsAutoPlacing();
+
     robotStateSubsystem.toPrepCoral();
   }
 

src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java

@@ -43,7 +43,7 @@ public boolean isFinished() {
     } else {
       return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
           || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
-          || !robotStateSubsystem.isBargeSafe;
+          || !robotStateSubsystem.getIsBargeSafe();
     }
   }
 }

src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java

@@ -25,7 +25,7 @@ public ScoreReefManualCommand(
   public void initialize() {
     startingRobotState = robotStateSubsystem.getState();
     startingElevatorFinished = elevatorSubsystem.isFinished();
-    robotStateSubsystem.setAutoPlaceOnCycle(false);
+    robotStateSubsystem.setIsAutoPlacing(false);
     robotStateSubsystem.setGetAlgaeOnCycle(false);
     robotStateSubsystem.toPrepCoral();
   }

src/main/java/frc/robot/commands/robotState/SetScoreSideCommand.java

@@ -0,0 +1,20 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
+
+public class SetScoreSideCommand extends InstantCommand {
+  private RobotStateSubsystem robotState;
+  private ScoreSide side;
+
+  public SetScoreSideCommand(RobotStateSubsystem robotState, ScoreSide side) {
+    this.robotState = robotState;
+    this.side = side;
+  }
+
+  @Override
+  public void initialize() {
+    robotState.setScoreSide(side);
+  }
+}

src/main/java/frc/robot/commands/robotState/StowCommand.java

@@ -1,6 +1,7 @@
 package frc.robot.commands.robotState;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
@@ -14,9 +15,10 @@ public StowCommand(
       RobotStateSubsystem robotState,
       ElevatorSubsystem elevatorSubsystem,
       CoralSubsystem coralSubsystem,
-      BiscuitSubsystem biscuitSubsystem) {
+      BiscuitSubsystem biscuitSubsystem,
+      AlgaeSubsystem algaeSubsystem) {
     this.robotState = robotState;
-    addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem);
+    addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem);
   }
 
   @Override

src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java

@@ -4,18 +4,14 @@
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
 
 public class ToggleAutoCommand extends InstantCommand {
-  RobotStateSubsystem robotState;
+  private RobotStateSubsystem robotStateSubsystem;
 
   public ToggleAutoCommand(RobotStateSubsystem robotState) {
-    this.robotState = robotState;
+    this.robotStateSubsystem = robotState;
   }
 
   @Override
   public void initialize() {
-    if (robotState.getAutoPlaceOnCycle() == false) {
-      robotState.setAutoPlaceOnCycle(true);
-    } else {
-      robotState.setAutoPlaceOnCycle(false);
-    }
+    robotStateSubsystem.setIsAutoPlacing(!robotStateSubsystem.getIsAutoPlacing());
   }
 }

src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java

@@ -12,6 +12,6 @@ public ToggleGetAlgaeCommand(RobotStateSubsystem robotStateSubsystem) {
 
   @Override
   public void initialize() {
-    robotStateSubsystem.ToggleGetAlgaeOnCycle();
+    robotStateSubsystem.toggleGetAlgaeOnCycle();
   }
 }

src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java

@@ -0,0 +1,26 @@
+package frc.robot.commands.tagAlign;
+
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates;
+
+public class TagAlignCommand extends Command {
+  private TagAlignSubsystem tagAlignSubsystem;
+
+  public TagAlignCommand(TagAlignSubsystem tagAlignSubsystem, DriveSubsystem driveSubsystem) {
+    this.tagAlignSubsystem = tagAlignSubsystem;
+    addRequirements(driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    tagAlignSubsystem.start(Alliance.Blue, true);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return tagAlignSubsystem.getState() == TagAlignStates.DONE;
+  }
+}

src/main/java/frc/robot/constants/BiscuitConstants.java

@@ -28,8 +28,6 @@ public class BiscuitConstants {
   public static final double kTicksPerRot = 160;
   public static final int talonID = 25;
   public static final double kCloseEnough = 0.05;
-  public static final Angle kMaxFwd = Rotations.of(51.04735 + 5);
-  public static final Angle kMaxRev = Rotations.of(-12.3489 - 5);
   public static final double kSafeToStowUpper = 40;
   public static final double kSafeToStowLower = -5;
 
@@ -40,11 +38,11 @@ public class BiscuitConstants {
   public static final Angle kPrestageSetpoint = kStowSetpoint;
 
   // Algae removal
-  public static final Angle kL2AlgaeSetpoint = Rotations.of(20.848);
+  public static final Angle kL2AlgaeSetpoint = Rotations.of(24.104);
   public static final Angle kL3AlgaeSetpoint = Rotations.of(24.562);
 
-  public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0);
-  public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0);
+  public static final Angle kL2AlgaeRemovalSetpoint = kStowSetpoint;
+  public static final Angle kL3AlgaeRemovalSetpoint = kStowSetpoint;
 
   // Coral score
   public static final Angle kL1CoralSetpoint = kStowSetpoint;
@@ -66,6 +64,10 @@ public class BiscuitConstants {
   public static final double kJogAmountUp = 10;
   public static final double kJogAmountDown = -10;
 
+  // Soft Limits
+  public static final Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5));
+  public static final Angle kMaxRev = kBargeBackwardSetpoint.minus(Rotations.of(5));
+
   // Disables the TalonFXS by setting its voltage to zero.
   public static VoltageConfigs disableTalon() {
     VoltageConfigs voltage =