Merge pull request #64 from strykeforce/Level1NewMech

Level1 new mech

Author: mwitcpalek

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

@@ -48,6 +48,7 @@
 import frc.robot.commands.robotState.HPAlgaeCommand;
 import frc.robot.commands.robotState.InterruptAutoCommand;
 import frc.robot.commands.robotState.LockWheelsCommand;
+import frc.robot.commands.robotState.OperatorRumbleCommand;
 import frc.robot.commands.robotState.ReefCycleCommand;
 import frc.robot.commands.robotState.ScoreAlgaeCommand;
 import frc.robot.commands.robotState.SetScoreSideCommand;
@@ -423,6 +424,33 @@ private void configureOperatorBindings() {
         .onTrue(
             new ClimbPrepCommand(
                 robotStateSubsystem, climbSubsystem, elevatorSubsystem, biscuitSubsystem));
+
+    new Trigger(() -> robotStateSubsystem.isStuckAndMisaligned())
+        .onTrue(new OperatorRumbleCommand(robotStateSubsystem, xboxController));
+
+    // Move biscuit
+    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+
+    // Move elevator
+    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
   }
 
   private void configureTestOperatorBindings() {
@@ -603,6 +631,7 @@ private void configureMatchDashboard() {
                 () -> {
                   funnelSubsystem.clearCoral();
                   coralSubsystem.setState(CoralSubsystem.CoralState.EMPTY);
+                  algaeSubsystem.setState(AlgaeSubsystem.AlgaeStates.EMPTY);
                 }))
         .withSize(1, 1)
         .withPosition(9, 0);

src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java

@@ -12,6 +12,6 @@ public IntakeAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
 
   @Override
   public void initialize() {
-    algaeSubsystem.intake();
+    algaeSubsystem.intakeAlgae();
   }
 }

src/main/java/frc/robot/commands/auton/DriveAutonServoCommand.java

@@ -174,8 +174,7 @@ public void execute() {
       hasStaged = true;
       robotStateSubsystem.toAutonPrestage();
     }
-    if (tagAlignSubsystem.getCurRadius(robotStateSubsystem.getAllianceColor())
-            <= AutonConstants.kElevatorStageRadiusPathOne
+    if (tagAlignSubsystem.getCurRadius() <= AutonConstants.kElevatorStageRadiusPathOne
         && !hasPreppedCoral) {
       hasPreppedCoral = true;
       robotStateSubsystem.toPrepCoral();
@@ -189,6 +188,7 @@ public void execute() {
           isServoing = true;
           tagAlignSubsystem.startAuto(
               mirrorTrajectory ? Alliance.Red : Alliance.Blue,
+              robotStateSubsystem.getCoralLevel(),
               mirrorToProcessor ? !scoreLeft : scoreLeft,
               false);
         }
@@ -199,8 +199,7 @@ public void execute() {
   }
 
   private boolean shouldTransitionToServoing() {
-    return tagAlignSubsystem.getCurRadius(mirrorTrajectory ? Alliance.Red : Alliance.Blue)
-        < PathHandlerConstants.kServoRadius;
+    return tagAlignSubsystem.getCurRadius() < PathHandlerConstants.kServoRadius;
   }
 
   @Override

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

@@ -0,0 +1,47 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+
+public class OperatorRumbleCommand extends Command {
+
+  private RobotStateSubsystem robotStateSubsystem;
+  private Timer timer;
+  private XboxController xboxController;
+  private boolean hasStartedTimer = false;
+
+  public OperatorRumbleCommand(
+      RobotStateSubsystem robotStateSubsystem, XboxController xboxController) {
+    this.xboxController = xboxController;
+    timer = new Timer();
+  }
+
+  @Override
+  public void initialize() {
+    timer.reset();
+    timer.start();
+  }
+
+  @Override
+  public void execute() {
+    xboxController.setRumble(RumbleType.kBothRumble, 1);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return timer.hasElapsed(0.5);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    xboxController.setRumble(RumbleType.kBothRumble, 0);
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

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

@@ -44,11 +44,14 @@ public boolean isFinished() {
       return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD;
     } else {
       if (startingRobotState == RobotStates.PRESTAGE || startingRobotState == RobotStates.STOW) {
-        return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL;
+        return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL
+            || (!robotStateSubsystem.hasCoral() && !robotStateSubsystem.getGetAlgaeOnCycle());
       }
       if (startingRobotState == RobotStates.REEF_ALIGN_CORAL) {
         return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD
             || robotStateSubsystem.getState() == RobotStates.LOADING_CORAL
+            || robotStateSubsystem.getState() == RobotStates.TO_ALGAE_CORAL_LOAD
+            || robotStateSubsystem.getState() == RobotStates.ALGAE_CORAL_LOAD
             || !startingElevatorFinished;
       }
     }

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

@@ -33,7 +33,12 @@ public class AlgaeConstants {
 
   public static final double kHasAlgaeVelThreshold = 10;
   public static final double kSuperCycleHasAlgaeVelThres = 40;
-  public static final double kHasAlgaeCounts = 2;
+  public static final int kHasAlgaeCounts = 2;
+
+  public static final double kHasCoralVelThreshold = 70; // FIXME
+  public static final int kHasCoralCounts = 10; // FIXME
+
+  public static final double kCoralScoringTime = 1; // FIXME
 
   // Example Talon FX Config
   public static TalonFXSConfiguration getFXConfig() {

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

@@ -49,7 +49,7 @@ public class ElevatorConstants {
   public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint;
 
   // Coral score
-  public static final Angle kL1CoralSetpoint = Rotations.of(15.2); // 13.04053
+  public static final Angle kL1CoralSetpoint = Rotations.of(3.931); // 13.04053
   public static final Angle kL2CoralSetpoint = Rotations.of(19.62793); // 19.62793 -> 21.0786 ->
   public static final Angle kL3CoralSetpoint =
       Rotations.of(31.0901); // was 30.42969 -> 31.7505 -> 31.0901