Prepare for Intake spin on shoot.

Added "Safe shot".

Author: jasondickert

src/main/java/frc/robot/Robot.java

@@ -76,6 +76,9 @@ public void robotInit() {
     Preferences.initDouble("BLP.Velocity", 500.0);
     Preferences.initDouble("BLP.Angle", 60.0);
 
+    Preferences.initDouble("SAF.Velocity", 2100.0);
+    Preferences.initDouble("SAF.Angle", 75.0);
+
     String pathname = "None";
     try {
       File deployDir = Filesystem.getDeployDirectory();

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

@@ -236,7 +236,9 @@ public void configureButtonBindings() {
                                                       .alongWith(new TurnOnLEDs(mLimelight)));
 
       new JoystickButton(driver, RMZ_D_IN).whenPressed(new PullTrigger(mShooter));
+                                                     
       new JoystickButton(driver, RMZ_D_IN).whenReleased(new StopTrigger(mShooter));
+                                                        
 
 //      new JoystickButton(driver, RMZ_H_IN).whenPressed(new ResetHood(mShooter));
       new JoystickButton(driver, RMZ_G_IN).and(new JoystickButton(driver, RMZ_H_IN).whenPressed(new ZeroHeading(mDrive)));
@@ -352,10 +354,10 @@ public void configureButtonBindings() {
       //                       .alongWith(new TurnOffLEDs(mLimelight))
       //                       .alongWith(new IdleCommand()));
 
-      // xBoxX.whenHeld(new VisionShooter(mShooter, mLimelight)
-                          // .alongWith(new TurnOnLEDs(mLimelight))
-                          // .alongWith(new XBoxButtonCommand(X_BOX_X)));
-      xBoxX.whenHeld(new MotionProfileTurn(mDrive, Math.PI/2));
+      xBoxX.whenHeld(new VisionShooter(mShooter, mLimelight)
+                          .alongWith(new TurnOnLEDs(mLimelight))
+                          .alongWith(new XBoxButtonCommand(X_BOX_X)));
+      // xBoxX.whenHeld(new MotionProfileTurn(mDrive, Math.PI/2));
 
       xBoxX.whenReleased(new StopShooter(mShooter)
                           .alongWith(new TurnOffLEDs(mLimelight))
@@ -388,8 +390,20 @@ public boolean get() {
 
         // .and(new JoystickButton(operator, X_BOX_LOGO_LEFT))
         // .whenActive(new ToggleClimber(mClimber));
-    }
 
+        new Button() {
+          @Override
+          public boolean get() {
+            return (operator.getPOV() == 180);
+          }
+        }.whenPressed(new PresetFlywheelController(mShooter, "SAF").alongWith(new XBoxButtonCommand(-1)))
+          .whenReleased(new StopShooter(mShooter).alongWith(new IdleCommand()));
+  
+          // .and(new JoystickButton(operator, X_BOX_LOGO_LEFT))
+          // .whenActive(new ToggleClimber(mClimber));
+  
+    }
+    
   }
 }
 

src/main/java/frc/robot/intake/Intake.java

@@ -52,6 +52,11 @@ public void rollerStop() {
     motor.set(ControlMode.PercentOutput, 0);
   }
 
+  // Push a ball into the trigger
+  // public void rollerPush() {
+  //   motor.set(ControlMode.PercentOutput, 0.30);
+  // }
+
   // Status of the intake arm's extended state.
   public boolean isExtended() {
     // return solenoid.get() == Value.kForward;

src/main/java/frc/robot/intake/commands/SpinIntake.java

@@ -0,0 +1,26 @@
+package frc.robot.intake.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.intake.Intake;
+
+public class SpinIntake extends CommandBase {
+  private Intake intake;
+
+  public SpinIntake(Intake intake) {
+    this.intake = intake; 
+    addRequirements(intake);
+  }
+
+  @Override
+  public void initialize() {
+//    intake.deploy();
+//    Timer.delay(0.2);
+    // intake.rollerPush();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/shooter/commands/PresetFlywheelController.java

@@ -35,7 +35,11 @@ public void initialize() {
                 break;
             case "BLP":
                 this.rpm = Preferences.getInt("BLP.Velocity", 500);
-                this.hoodAngle = Preferences.getDouble("BLP.Angle", 60.0); // tarmac, upper goal, rear shot    
+                this.hoodAngle = Preferences.getDouble("BLP.Angle", 60.0); // Bloop shot    
+                break;
+            case "SAF":
+                this.rpm = Preferences.getInt("SAF.Velocity", 500);
+                this.hoodAngle = Preferences.getDouble("SAF.Angle", 60.0); // safe loading zone shot    
                 break;
             default:
                 this.rpm = 500;