Intake control changes and automated shooter/indexer firing

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

@@ -18,6 +18,7 @@ import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -26,8 +27,10 @@ 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.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.constants.IntakeConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.constants.PhotonConstants;
 import frc.robot.constants.ShooterConstants;
@@ -55,6 +58,7 @@ public class RobotContainer {
 
     private Trigger isTeleopTrigger;
     private Trigger isEnabledTrigger;
+    private Trigger indexerBeamBreakWatcher;
 
     private SendableChooser<Command> autoChooser;
 
@@ -67,6 +71,7 @@ public class RobotContainer {
     private int speakerTag;
 
     private boolean setAmpAngle;
+    private boolean intakeDown;
     private boolean invertXYDrive;
 
     public RobotContainer() {
@@ -111,8 +116,10 @@ public class RobotContainer {
 
         isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
         isEnabledTrigger = new Trigger(DriverStation::isEnabled);
+        indexerBeamBreakWatcher = new Trigger(indexer::getBeamBreak);
 
         setAmpAngle = false;
+        intakeDown = false;
 
         configureBindings();
         shuffleboardSetup();
@@ -157,7 +164,35 @@ public class RobotContainer {
         );
 
         //intake.setDefaultCommand(intake.intakeUpCommand());
-        intake.setDefaultCommand(intake.manualPivot(() -> -operator.getLeftY(), operator::getRightY));
+        //intake.setDefaultCommand(intake.manualPivot(() -> -operator.getLeftY(), operator::getRightY));
+        intake.setDefaultCommand(
+            intake.intakeControl(
+                () -> {
+                    if (intakeDown) {
+                        return IntakeConstants.kDownAngle;
+                    } else {
+                        return IntakeConstants.kUpAngle;
+                    }
+                }, 
+                operator::getRightY
+            )
+        );
+
+        operator.povUp().onTrue(
+            new InstantCommand(
+                () -> {
+                    intakeDown = false;
+                }
+            )
+        );
+
+        operator.povDown().onTrue(
+            new InstantCommand(
+                () -> {
+                    intakeDown = true;
+                }
+            )
+        );
 
         shooter.setDefaultCommand(
             shooter.angleSpeedsSetpoints(
@@ -179,6 +214,20 @@ public class RobotContainer {
         );
         //shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
 
+        driver.leftBumper().onTrue(
+            shooter.angleSpeedsSetpoints(
+                () -> ShooterConstants.kShooterLoadAngle, 
+                () -> 1.0
+            ).withTimeout(.5).andThen(
+                shooter.angleSpeedsSetpoints(
+                    () -> ShooterConstants.kShooterLoadAngle, 
+                    () -> 1.0
+                ).alongWith(
+                    indexer.shootNote(() -> 1.0)
+                ).withTimeout(2)
+            )
+        );
+
         climber.setDefaultCommand(climber.stop());
 
         indexer.setDefaultCommand(indexer.shootNote(
@@ -191,6 +240,16 @@ public class RobotContainer {
             }
         ));
 
+        indexerBeamBreakWatcher.onFalse(
+            new RunCommand(() -> {
+                driver.getHID().setRumble(RumbleType.kBothRumble, 1);
+            }).withTimeout(.5).andThen(
+                new InstantCommand(() -> {
+                    driver.getHID().setRumble(RumbleType.kBothRumble, 0);
+                })
+            )
+        );
+
         /*
         driver.povCenter().onFalse(
             drivetrain.driveCardinal(
@@ -284,8 +343,8 @@ public class RobotContainer {
 
         operator.start().onTrue(shooter.zeroEncoder());
 
-        operator.povDown().whileTrue(intake.intakeDownCommand());
+        //operator.povDown().whileTrue(intake.intakeDownCommand());
-        operator.povUp().whileTrue(intake.intakeUpCommand());
+        //operator.povUp().whileTrue(intake.intakeUpCommand());
 
         driver.leftTrigger().whileTrue(Commands.parallel( intake.manualPivot(() -> 0.0, () -> 1.0), indexer.advanceNote()));
         
@@ -312,7 +371,7 @@ public class RobotContainer {
     teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
     teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
     teleopTab.addDouble("intake angle", intake::getIntakeDegrees);
-    teleopTab.addDouble("intake pid", intake::getIntakePID);
+    //teleopTab.addDouble("intake pid", intake::getIntakePID);
     teleopTab.addDouble("heading swerve", drivetrain::getHeading);
     teleopTab.addDouble("steering encoder", drivetrain::getEncoderSteering);
   }

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -8,13 +8,13 @@ public class IntakeConstants {
 
     public static final int kPivotCurrentLimit = 40;
 
-    public static final double kPIntake = 2.5;
+    public static final double kPIntake = 3;//4; //2.5;
     public static final double kIIntake = 0;
-    public static final double kDIntake = 0;
+    public static final double kDIntake = 0.01;
 
     public static final double kSFeedForward = 0;
-    public static final double kGFeedForward = 0;//1.11;
+    public static final double kGFeedForward = 1;//1.11;
-    public static final double kVFeedForward = 0;//0.73;
+    public static final double kVFeedForward = .5;//0.73;
 
     public static final double kStartingAngle = Math.toRadians(105.0);
     public static final double kUpAngle = Math.toRadians(90.0);

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

@@ -61,6 +61,22 @@ public class Intake extends SubsystemBase{
         armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle;
     }
 
+    public Command intakeControl(DoubleSupplier pivotAngle, DoubleSupplier intakeSpeed) {
+        return run(() -> {
+            intakeRoller.set(intakeSpeed.getAsDouble());
+
+            intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(),
+                    pivotAngle.getAsDouble()
+                ) + intakeFeedForward.calculate(
+                    pivotAngle.getAsDouble(), 
+                    0.0
+                )
+            );
+        });
+    }
+
     public Command intakeDownCommand() {
         return run(() -> {
             intakeRoller.set(0.0);