Se/better handoff

src/main/java/com/stuypulse/robot/Robot.java

@@ -41,6 +41,7 @@ public void robotPeriodic() {
             && Arm.getInstance().atTarget() 
             && !Shooter.getInstance().hasNote()
             && Intake.getInstance().hasNote()
+            && Intake.getInstance().getState() != Intake.State.DEACQUIRING
         ) {
             CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake()
                                                     .andThen(new BuzzController(robot.driver)));

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -17,6 +17,8 @@
 import com.stuypulse.robot.commands.intake.IntakeStop;
 import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake;
 import com.stuypulse.robot.commands.shooter.ShooterAutoShoot;
+import com.stuypulse.robot.commands.shooter.ShooterFeederShoot;
+import com.stuypulse.robot.commands.shooter.ShooterFeederStop;
 import com.stuypulse.robot.commands.swerve.SwerveDriveAutoAlignment;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry;
@@ -108,7 +110,8 @@ private void configureButtonBindings() {
         driver.getRightTriggerButton()
             .whileTrue(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                 .andThen(new ShooterAutoShoot())
-            );
+            )
+            .onFalse(new ShooterFeederStop());
 
         driver.getTopButton()
             .onTrue(new ConditionalCommand(

src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java

@@ -15,12 +15,12 @@ public IntakeAcquire() {
 
     @Override
     public void initialize() {
-        intake.acquire();
+        intake.setState(Intake.State.ACQUIRING);
     }
 
     @Override
     public void end(boolean interrupted) {
-        intake.stop();
+        intake.setState(Intake.State.STOP);
     }
 
     @Override

src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java

@@ -15,6 +15,6 @@ public IntakeAcquireForever() {
 
     @Override
     public void initialize() {
-        intake.acquire();
+        intake.setState(Intake.State.ACQUIRING);
     }
 }

src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java

@@ -15,6 +15,6 @@ public IntakeDeacquire() {
 
     @Override
     public void initialize() {
-        intake.deacquire();
+        intake.setState(Intake.State.DEACQUIRING);
     }
 }

src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java

@@ -15,6 +15,6 @@ public IntakeStop() {
 
     @Override
     public void initialize() {
-        intake.stop();
+        intake.setState(Intake.State.STOP);
     }
 }

src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java

@@ -1,7 +1,9 @@
 package com.stuypulse.robot.commands.shooter;
 
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.intake.Intake;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
+import com.stuypulse.stuylib.util.StopWatch;
 
 import edu.wpi.first.wpilibj2.command.Command;
 
@@ -10,18 +12,43 @@ public class ShooterAcquireFromIntake extends Command {
     private final Shooter shooter;
     private final Intake intake;
 
+    private final StopWatch stopWatch;
+    private boolean isFeeding;
+
     public ShooterAcquireFromIntake() {
         shooter = Shooter.getInstance();
         intake = Intake.getInstance();
+
+        stopWatch = new StopWatch();
+        isFeeding = true;
+
         addRequirements(shooter, intake);
     }
 
     @Override
     public void initialize() {
-        intake.acquire();
+        intake.setState(Intake.State.ACQUIRING);
         shooter.feederIntake();
     }
 
+    @Override
+    public void execute() {
+        if (isFeeding) {
+            if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) {
+                intake.setState(Intake.State.DEACQUIRING);
+                isFeeding = false;
+                stopWatch.reset();
+            }
+        }
+        else {
+            if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) {
+                intake.setState(Intake.State.ACQUIRING);
+                isFeeding = true;
+                stopWatch.reset();
+            }
+        }
+    }
+
     @Override
     public boolean isFinished() {
         return shooter.hasNote();
@@ -30,7 +57,7 @@ public boolean isFinished() {
     @Override
     public void end(boolean interrupted) {
         shooter.feederStop();
-        intake.stop();
+        intake.setState(Intake.State.STOP);
     }
 
 }

src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java

@@ -0,0 +1,21 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ShooterFeederStop extends InstantCommand {
+
+    private final Shooter shooter;
+
+    public ShooterFeederStop() {
+        shooter = Shooter.getInstance();
+        addRequirements(shooter);
+    }
+
+    @Override
+    public void initialize() {
+        shooter.feederStop();
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java

@@ -3,7 +3,6 @@
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 
@@ -14,7 +13,7 @@ public ShooterScoreAmp() {
             new ShooterFeederDeacquire(),
             new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()),
             new WaitUntilCommand(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME.get()),
-            new InstantCommand(() -> Shooter.getInstance().feederStop())
+            new ShooterFeederStop()
         );
     }
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -61,13 +61,14 @@ public interface Arm {
 
         SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 100);
         SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25);
+
         SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2);
 
         SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 70);
         SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50);
         SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50);
         SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80);
-        SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 17);
+        SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.getAsDouble() + 16);
         SmartNumber PODIUM_SHOT_ANGLE = new SmartNumber("Arm/Podium Shot Angle", -60);
 
         SmartNumber SHOULD_RETURN_TO_FEED_TIME = new SmartNumber("Arm/Return To Feed Time", 1.0);
@@ -97,17 +98,21 @@ public interface Encoder {
     }
 
     public interface Intake {
-        double INTAKE_ACQUIRE_SPEED = 0.75;
+        double INTAKE_ACQUIRE_SPEED = 0.8;
         double INTAKE_DEACQUIRE_SPEED = 1.0;
         double FUNNEL_ACQUIRE = 1.0;
         double FUNNEL_DEACQUIRE = 1.0;
+
         double IR_DEBOUNCE = .005;
+
+        double HANDOFF_TIMEOUT = 1.8;
+        double MINIMUM_DEACQUIRE_TIME_WHEN_STUCK = 0.5;
     }
 
     public interface Shooter {
-        double FEEDER_INTAKE_SPEED = 0.22;
+        double FEEDER_INTAKE_SPEED = 0.25;
         double FEEDER_DEAQUIRE_SPEED = 0.5;
-        double FEEDER_SHOOT_SPEED = 0.22;
+        double FEEDER_SHOOT_SPEED = 0.25;
 
         double TARGET_RPM_THRESHOLD = 250;
         double MAX_WAIT_TO_REACH_TARGET = 1.5;

src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java

@@ -10,24 +10,32 @@ public abstract class Intake extends SubsystemBase {
     private static final Intake instance;
 
     static {
-        if (Robot.isReal()) {
-            instance = new IntakeImpl();
-        } else {
-            instance = new IntakeSim();
-        }
+        instance = new IntakeImpl();
     }
 
     public static Intake getInstance() {
         return instance;
     }
 
-    protected Intake() {}
+    public enum State {
+        DEACQUIRING,
+        ACQUIRING,
+        STOP
+    }
 
-    public abstract void acquire();
+    private State state;
 
-    public abstract void deacquire();
+    protected Intake() {
+        this.state = State.STOP;
+    }
 
-    public abstract void stop();
+    public void setState(State state) {
+        this.state = state;
+    }
+
+    public State getState() {
+        return this.state;
+    }
 
     public abstract boolean hasNote();
 }

src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java

@@ -23,6 +23,7 @@ public class IntakeImpl extends Intake {
     private final BStream hasNote;
 
     public IntakeImpl() {
+        super();
         funnelMotorLeft = new CANSparkMax(Ports.Intake.FUNNEL_LEFT, MotorType.kBrushless);
         funnelMotorRight = new CANSparkMax(Ports.Intake.FUNNEL_RIGHT, MotorType.kBrushless);        
         intakeMotor = new CANSparkMax(Ports.Intake.INTAKE_MOTOR, MotorType.kBrushless);
@@ -37,22 +38,19 @@ public IntakeImpl() {
             .filtered(new BDebounce.Both(Settings.Intake.IR_DEBOUNCE));
     }
 
-    @Override
-    public void acquire() {
+    private void acquire() {
         intakeMotor.set(+Settings.Intake.INTAKE_ACQUIRE_SPEED);
         funnelMotorLeft.set(+Settings.Intake.FUNNEL_ACQUIRE);
         funnelMotorRight.set(+Settings.Intake.FUNNEL_ACQUIRE);
     }
 
-    @Override
-    public void deacquire() {
+    private void deacquire() {
         intakeMotor.set(-Settings.Intake.INTAKE_DEACQUIRE_SPEED);
         funnelMotorLeft.set(-Settings.Intake.FUNNEL_DEACQUIRE);
         funnelMotorRight.set(-Settings.Intake.FUNNEL_DEACQUIRE);
     }
 
-    @Override
-    public void stop() {
+    private void stop() {
         intakeMotor.stopMotor();
         funnelMotorLeft.stopMotor();
         funnelMotorRight.stopMotor();
@@ -67,6 +65,21 @@ public boolean hasNote() {
     public void periodic() {
         super.periodic();
 
+        switch (getState()) {
+            case ACQUIRING:
+                acquire();
+                break;
+            case DEACQUIRING:
+                deacquire();
+                break;
+            case STOP:
+                stop();
+                break;
+            default:
+                stop();
+                break;
+        }
+
         SmartDashboard.putNumber("Intake/Intake Speed", intakeMotor.get());
         SmartDashboard.putNumber("Intake/Funnel/Right Funnel Speed", funnelMotorLeft.get());
         SmartDashboard.putNumber("Intake/Funnel/Left Funnel Speed", funnelMotorRight.get());