moved repositionGamePiece to its own command (#250)

-called in whileFalse after intakeGamePiece

src/main/java/frc/robot/Constants.java

@@ -129,7 +129,7 @@ public static class pracBot {
   }
 
   public static class constIntake {
-    public static final double ABS_ENCODER_OFFSET = 0.420915;
+    public static final double ABS_ENCODER_OFFSET = 0.384;
     public static final boolean ABS_ENCODER_INVERT = true;
     public static final double GEAR_RATIO = 28.13;
     public static final NeutralModeValue PIVOT_NEUTRAL_MODE = NeutralModeValue.Brake;

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

@@ -24,6 +24,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.constControllers;
 import frc.robot.Constants.LockedLocation;
 import frc.robot.Constants.constLEDs;
@@ -48,6 +49,7 @@
 import frc.robot.commands.ManualHoodMovement;
 import frc.robot.commands.ManualTurretMovement;
 import frc.robot.commands.Panic;
+import frc.robot.commands.RepositionGamePiece;
 import frc.robot.commands.PrepAmp;
 import frc.robot.commands.SetLEDS;
 import frc.robot.commands.TransferGamePiece;
@@ -96,6 +98,7 @@ public class RobotContainer implements Logged {
   private final static Transfer subTransfer = new Transfer();
   private final static Vision subVision = new Vision();
 
+  Trigger repositionTrigger = new Trigger(() -> subTransfer.calcGamePieceCollected(false));
   SendableChooser<AutoInterface> autoChooser = new SendableChooser<>();
 
   @Log.NT
@@ -195,10 +198,13 @@ private void configureDriverBindings(SN_XboxController controller) {
   private void configureOperatorBindings(SN_XboxController controller) {
     // Left Trigger = Intake
     controller.btn_LeftTrigger
-        .whileTrue(new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber))
         .onTrue(Commands.runOnce(() -> RobotContainer.setLockedLocation(LockedLocation.NONE))
             .alongWith(Commands.runOnce(() -> subTransfer.hasGamePiece = false))
-            .unless(() -> RobotContainer.getLockedLocation() != LockedLocation.AMP));
+            .unless(() -> RobotContainer.getLockedLocation() != LockedLocation.AMP))
+        .whileTrue(new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber)
+            .unless(() -> subTransfer.hasGamePiece));
+
+    repositionTrigger.onTrue(new RepositionGamePiece(subTransfer, subShooter));
 
     // Left Bumper = Enable both Manuals
     // Left Stick = Manual Hood

src/main/java/frc/robot/commands/IntakeFromSource.java

@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj2.command.Command;
-
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.RobotPreferences.prefPitch;
 import frc.robot.RobotPreferences.prefShooter;
 import frc.robot.RobotPreferences.prefTransfer;
@@ -62,10 +62,7 @@ public void end(boolean interrupted) {
       subTransfer.setNeutralMode();
     }
     if (!interrupted) {
-      subTransfer.repositionGamePiece();
-
-      subShooter.setDesiredVelocities(prefShooter.leftShooterSubVelocity.getValue(),
-          prefShooter.rightShooterSubVelocity.getValue());
+      Commands.runOnce(() -> new RepositionGamePiece(subTransfer, subShooter));
     } else {
       subTransfer.setTransferNeutralOutput();
       subShooter.setDesiredVelocities(0, 0);

src/main/java/frc/robot/commands/IntakeGroundGamePiece.java

@@ -80,8 +80,6 @@ public void end(boolean interrupted) {
       subIntake.setRollerNeutralOutput();
     }
     if (!interrupted) {
-      subTransfer.repositionGamePiece();
-
       subShooter.setDesiredVelocities(prefShooter.leftShooterSubVelocity.getValue(),
           prefShooter.rightShooterSubVelocity.getValue());
       subShooter.getUpToSpeed();

src/main/java/frc/robot/commands/RepositionGamePiece.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.RobotPreferences.prefShooter;
+import frc.robot.RobotPreferences.prefTransfer;
+import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Transfer;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class RepositionGamePiece extends SequentialCommandGroup {
+  Transfer subTransfer;
+  Shooter subShooter;
+
+  /** Creates a new RepositionGamePiece. */
+  public RepositionGamePiece(Transfer subTransfer, Shooter subShooter) {
+    this.subTransfer = subTransfer;
+    this.subShooter = subShooter;
+
+    addRequirements(subTransfer, subShooter);
+
+    addCommands(
+        Commands.runOnce(() -> subTransfer.setTransferMotorSpeed(prefTransfer.transferRepositionSpeed.getValue())),
+        Commands.waitSeconds(prefTransfer.transferRepositionTime.getValue()),
+        Commands.runOnce(() -> subTransfer.setTransferMotorSpeed(-prefTransfer.transferRepositionSpeed.getValue())),
+        Commands.waitSeconds(prefTransfer.transferRepositionTime.getValue() / 2),
+        Commands.runOnce(() -> subTransfer.setTransferNeutralOutput()),
+        Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSubVelocity.getValue(),
+            prefShooter.rightShooterSubVelocity.getValue())),
+        Commands.runOnce(() -> subShooter.getUpToSpeed()));
+
+  }
+}

src/main/java/frc/robot/subsystems/Transfer.java

@@ -99,23 +99,6 @@ public boolean calcGamePieceCollected(boolean fromSource) {
     return hasGamePiece;
   }
 
-  public void repositionGamePiece() {
-    hasRepositioned = false;
-    double time = Timer.getFPGATimestamp();
-
-    while (Timer.getFPGATimestamp() <= time + prefTransfer.transferRepositionTime.getValue()) {
-      setTransferMotorSpeed(prefTransfer.transferRepositionSpeed.getValue());
-    }
-
-    time = Timer.getFPGATimestamp();
-
-    while (Timer.getFPGATimestamp() <= time + prefTransfer.transferRepositionTime.getValue() / 2) {
-      setTransferMotorSpeed(-prefTransfer.transferRepositionSpeed.getValue());
-    }
-    setTransferNeutralOutput();
-    hasRepositioned = true;
-  }
-
   public boolean calcGPShotAuto() {
     double currentPosition = Units.rotationsToDegrees(transferMotor.getPosition().getValueAsDouble());