Merge pull request #229 from strykeforce/mwitcpalek/teleopFixes

Mwitcpalek/teleop fixes

Author: mwitcpalek

src/main/java/frc/team2767/deepspace/command/approach/sequences/AutoHatchPickupCommandGroup.java

@@ -32,7 +32,7 @@ public AutoHatchPickupCommandGroup() {
 
     addSequential(new PressureSetCommand(VacuumSubsystem.kHatchPressureInHg), 0.02);
     addSequential(new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.GAME_PIECE_PICKUP));
-    addParallel(new ReleaseKrakenCommand(true));
+    addParallel(new ReleaseKrakenCommand(false)); // Don't want kraken in teleop
     addParallel(new BiscuitPositionAboveCameraCommand());
     addParallel(new ElevatorExecutePlanCommand());
     addSequential(new VisionAutoAlignPickupCommand());

src/main/java/frc/team2767/deepspace/command/intake/WaitForBeamBreakCommand.java

@@ -20,6 +20,7 @@ protected void execute() {
     if (INTAKE.isBeamBroken() && !hasBroken) {
       hasBroken = true;
       breakTime = System.currentTimeMillis();
+      INTAKE.setPosition(105);
     }
   }
 

src/main/java/frc/team2767/deepspace/command/sequences/pickup/Biscuit270WrapConditionalCommand.java

@@ -0,0 +1,46 @@
+package frc.team2767.deepspace.command.sequences.pickup;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.ConditionalCommand;
+import frc.team2767.deepspace.Robot;
+import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
+import frc.team2767.deepspace.command.elevator.ElevatorExecutePlanCommand;
+import frc.team2767.deepspace.command.elevator.ElevatorSetPositionCommand;
+import frc.team2767.deepspace.subsystem.BiscuitSubsystem;
+import frc.team2767.deepspace.subsystem.FieldDirection;
+import frc.team2767.deepspace.subsystem.VisionSubsystem;
+
+public class Biscuit270WrapConditionalCommand extends ConditionalCommand {
+  private static final BiscuitSubsystem BISCUIT = Robot.BISCUIT;
+  private static final VisionSubsystem VISION = Robot.VISION;
+
+  private static double currentBiscuitPos;
+  private static final double LEFT_270 = 260;
+  private static final double RIGHT_270 = -260;
+
+  public Biscuit270WrapConditionalCommand() {
+    super(
+        new CommandGroup() {
+          {
+            addSequential(
+                new CommandGroup() {
+                  {
+                    addParallel(new ElevatorSetPositionCommand(22.0));
+                    addParallel(new BiscuitExecutePlanCommand());
+                  }
+                });
+
+            addSequential(new ElevatorExecutePlanCommand());
+          }
+        });
+  }
+
+  // If Biscuit is wrapped 270 and needs to switch sides, move elevator up, spin biscuit, and move
+  // back down
+  @Override
+  protected boolean condition() {
+    currentBiscuitPos = BISCUIT.getPosition();
+    return (currentBiscuitPos > LEFT_270 && VISION.direction == FieldDirection.RIGHT)
+        || (currentBiscuitPos < RIGHT_270 && VISION.direction == FieldDirection.LEFT);
+  }
+}

src/main/java/frc/team2767/deepspace/command/sequences/pickup/CoconutPickupAutoRetryCommand.java

@@ -73,6 +73,12 @@ protected void execute() {
           ELEVATOR.openLoopMove(DOWN_SPEED);
           state = PickupState.DOWN;
           logger.info("Moving Down");
+          break;
+        }
+        if (currentPressure - initialPressure > PRESSURE_DIFFERENTIAL) {
+          state = PickupState.DONE;
+          logger.info("Got Seal on RESET");
+          break;
         }
     }
   }

src/main/java/frc/team2767/deepspace/command/sequences/pickup/CoconutPickupCommandGroup.java

@@ -3,7 +3,6 @@
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import frc.team2767.deepspace.command.biscuit.BiscuitSetPositionCommand;
 import frc.team2767.deepspace.command.elevator.ElevatorSetPositionCommand;
-import frc.team2767.deepspace.command.intake.IntakePositionCommand;
 import frc.team2767.deepspace.command.intake.RollerStopCommand;
 import frc.team2767.deepspace.command.log.LogCommand;
 import frc.team2767.deepspace.command.states.SetActionCommand;
@@ -16,8 +15,7 @@ public class CoconutPickupCommandGroup extends CommandGroup {
 
   public CoconutPickupCommandGroup() {
     addSequential(new LogCommand("BEGIN COCONUT PICKUP"));
-
-    addParallel(new IntakePositionCommand(IntakeSubsystem.kMiddlePositionDeg));
+    // addParallel(new IntakePositionCommand(IntakeSubsystem.kMiddlePositionDeg));
     addSequential(
         new CommandGroup() {
           {
@@ -47,7 +45,7 @@ public CoconutPickupCommandGroup() {
       }
     });*/
 
-    addParallel(new IntakePositionCommand(105));
+    // addParallel(new IntakePositionCommand(105));
     addSequential(new BiscuitSetPositionCommand(BiscuitSubsystem.kDownPosition));
     // addSequential(new ElevatorDownFastOpenLoopCommand());
     // addSequential(new WaitForPressureCommand());

src/main/java/frc/team2767/deepspace/command/sequences/place/PositionExecuteCommandGroup.java

@@ -6,13 +6,15 @@
 import frc.team2767.deepspace.command.elevator.ElevatorMoveToSafePositionCommand;
 import frc.team2767.deepspace.command.log.LogCommand;
 import frc.team2767.deepspace.command.sequences.pickup.BallToHatchConditionalCommand;
+import frc.team2767.deepspace.command.sequences.pickup.Biscuit270WrapConditionalCommand;
 
 public class PositionExecuteCommandGroup extends CommandGroup {
 
   public PositionExecuteCommandGroup() {
     addSequential(new LogCommand("BEGIN POSITION EXECUTE"));
     addSequential(new ElevatorMoveToSafePositionCommand());
     addSequential(new BiscuitMoveSafeCommand());
+    addSequential(new Biscuit270WrapConditionalCommand());
     addSequential(
         new CommandGroup() {
           {

src/main/java/frc/team2767/deepspace/command/teleop/StowElevatorConditionalCommand.java

@@ -15,7 +15,7 @@ public StowElevatorConditionalCommand() {
     super(
         new CommandGroup() {
           {
-            addSequential(new ElevatorSetPositionCommand(23.0));
+            addSequential(new ElevatorSetPositionCommand(25.0));
             addSequential(new BiscuitSetPositionCommand(BiscuitSubsystem.kUpPositionDeg));
             addSequential(new ElevatorSetPositionCommand(12.0));
           }