Work from 3/19 Meeting

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 
 import java.util.Optional;
 import java.util.OptionalDouble;
@@ -27,6 +28,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 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.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -89,6 +91,7 @@ public class RobotContainer {
                 vp.visualPose()
             );
         });
+
         vision.addPoseEstimateConsumer((vp) -> {
             if(resetOdometryToVisualPose) {
                 drivetrain.resetOdometry(vp.visualPose());
@@ -96,7 +99,6 @@ public class RobotContainer {
             }
         });
         
-
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
         secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
@@ -104,8 +106,9 @@ public class RobotContainer {
         shiftTimer.reset();
 
         configureBindings();
-        //testConfigureBindings();
         configureShiftDisplay();
+        //testConfigureBindings();
+        
 
         if(AutoConstants.kAutoConfigOk) {
             autoChooser = AutoBuilder.buildAutoChooser();
@@ -285,7 +288,17 @@ public class RobotContainer {
         driver.leftBumper().whileTrue(intakeRoller.runOut());
         driver.rightBumper().whileTrue(intakeRoller.runIn());
 
-        driver.rightTrigger().whileTrue(spindexer.spinToShooter());
+        driver.rightTrigger().whileTrue(
+            spindexer.spinToShooter().alongWith(
+                intakeRoller.runIn(),
+                intakePivot.jimmy(.5)
+            )
+        );
+
+        driver.rightTrigger().onFalse(
+            intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
+        );
+
         driver.b().whileTrue(spindexer.spinToIntake());
        /*  driver.b().whileTrue(
             drivetrain.lockToYaw(
@@ -309,13 +322,6 @@ public class RobotContainer {
         //30 degrees good for shooter far near outpost
         secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
         //10 degrees good for shooting ~33in away from hub
-
-        secondary.y().whileTrue(
-            intakePivot.jimmy(.2)
-            .andThen(
-                intakePivot.manualSpeed(() -> -0.5)
-            )
-        );
         
         hood.setDefaultCommand(hood.trackToAngle(() -> {
             Pose2d drivetrainPose = drivetrain.getPose();
@@ -340,6 +346,24 @@ public class RobotContainer {
                 return 0;
             }
         }));
+
+        new Trigger(() -> MathUtil.isNear(
+            shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(), 
+            shooter.getAverageActualSpeeds(), 
+            150)).onTrue(
+            new FunctionalCommand(
+                () -> {}, 
+                () -> {
+                    driver.setRumble(RumbleType.kBothRumble, .75);
+                    secondary.setRumble(RumbleType.kBothRumble, .75);
+                }, 
+                (b) -> {
+                    driver.setRumble(RumbleType.kBothRumble, 0);
+                    secondary.setRumble(RumbleType.kBothRumble, 0);
+                }, 
+                () -> false
+            ).withTimeout(5)
+        );
     }
 
     private void configureNamedCommands() {