Add a way to backtrack stuck notes

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

@@ -7,6 +7,9 @@ public class Constants {
     public static final int DRIVE_LEFT_CHILD_ID = 8;
     public static final int DRIVE_RIGHT_PARENT_ID = 11;
     public static final int DRIVE_RIGHT_CHILD_ID = 3;
+    public static final int SHOOTER_LEFT_FLYWHEEL_ID = 6;
+    public static final int SHOOTER_RIGHT_FLYWHEEL_ID = 7;
+    public static final int SHOOTER_FEEDER_ID = 9;
     public static final int INTAKE_ID = 6101;
 
     /*CLIMB MOTOR IDS */
@@ -19,6 +22,7 @@ public class Constants {
     public static final double SHOOTER_FLYWHEEL_STARTUP_TIME = 1.0;
     /*The amount of seconds that the feeder should wait before turning off after shooting */
     public static final double FEEDER_MOTOR_SHUTOFF_TIME = 0;
+    public static final double SHOOTING_VELOCITY_RPM = 4000;
 
     /*CLIMB CONFIG */
     public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5;
@@ -27,11 +31,16 @@ public class Constants {
     public static final double CLIMB_EXTENSION_SPEED = .25;
     /**the % of max power we give to the arm motors while retracting */
     public static final double CLIMB_RETRACTION_SPEED = -.25;
+
+    /*INTAKE CONFIG */
+    public static final double INTAKE_SPEED = 0;
+
     /*DRIVE TRAIN CONFIG */
     public static final double CONTROLLER_DEADZONE = 0.06;
 
     /*CONTROLLER PORT IDs*/
     public static final int DRIVER_CONTROLLER_ID = 0;
+    public static final int CODRIVER_CONTROLLER_ID = 1;
 
     /*START OF VISION CONSTANTS */
     public static final int CAMERA_HEIGHT = 480;
@@ -44,6 +53,7 @@ public class Constants {
     /*PID GAINS *///0.00001, 100
     public static final Gains ROTATION_GAINS = new Gains(0.011, 0.0001, 0.000935, 0, 0, 0.3);
     public static final Gains NORMAL_ROBOT_GAINS = new Gains(0.0005, 0.0, 0, 0, 0, 1.);
+    public static final double ROTATION_ERROR_DEGREES = 5.0;
 
     /*PHYSICAL ROBOT CONSTANTS */
     public static final double WHEEL_CIRCUMFERENCE_INCHES = 8 * Math.PI;
@@ -59,8 +69,7 @@ public class Constants {
     /** amount of time in ms to wait for confirmation */
     public static final int TIMEOUT_MS = 30;
     public static final double ENCODER_ROTATION = 4096.0;
-    /* ROTATION PID CONSTANTS */
 
-    public static final double ROTATION_ERROR_DEGREES = 5.;
-    public static final double SHOOTING_VELOCITY_RPM = 4000;
+    /*OTHER CONFIG */
+    public static final double MOTOR_BACKTRACK_SPEED_PERCENT = -0.5;
 }

src/main/java/frc/robot/InputtedCoDriverControls.java

@@ -14,7 +14,7 @@ public static void setCoDriverController(XboxController xboxController) {
     }
 
     public static void onEveryFrame() {
-        if (controller.getAButtonPressed()) {
+        if (controller.getYButtonPressed()) {
             Shooter.startFeedMotors();
         }
         if (controller.getXButton()) {
@@ -23,7 +23,7 @@ public static void onEveryFrame() {
             Shooter.stopShooterMotors();
         }
 
-        if (controller.getYButton()) {
+        if (controller.getLeftBumper()) {
             Intake.startFloorIntake();
         } else {
             Intake.stopFloorIntake();
@@ -36,5 +36,13 @@ public static void onEveryFrame() {
         } else {
             Climber.stopClimbMotors();
         }
+
+        if (controller.getAButtonPressed()) {
+            Intake.backtrack();
+            Shooter.backtrack();
+        } else if (controller.getAButtonReleased()) {
+            Intake.stopFloorIntake();
+            Shooter.stopShooterMotors();
+        }
     }
 }

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

@@ -39,14 +39,23 @@ public class Robot extends TimedRobot {
     MotorController driveLeftChild = RobotConfigs.getLeftChild();
     MotorController driveRightParent = RobotConfigs.getRightParent();
     MotorController driveRightChild = RobotConfigs.getRightChild();
-    MotorController leftFlywheel = MotorControllerFactory.create(6, MotorController.Type.SparkMax);
-    MotorController rightFlywheel = MotorControllerFactory.create(7, MotorController.Type.SparkMax);
-    MotorController feederMotor = MotorControllerFactory.create(9, MotorController.Type.SparkMax);
+    MotorController leftFlywheel = MotorControllerFactory.create(
+        Constants.SHOOTER_LEFT_FLYWHEEL_ID,
+        MotorController.Type.SparkMax
+    );
+    MotorController rightFlywheel = MotorControllerFactory.create(
+        Constants.SHOOTER_RIGHT_FLYWHEEL_ID,
+        MotorController.Type.SparkMax
+    );
+    MotorController feederMotor = MotorControllerFactory.create(
+        Constants.SHOOTER_FEEDER_ID,
+        MotorController.Type.SparkMax
+    );
     MotorController leftClimb = MotorControllerFactory.create(Constants.LEFT_CLIMB_ID, MotorController.Type.Talon);
     MotorController rightClimb = MotorControllerFactory.create(Constants.RIGHT_CLIMB_ID, MotorController.Type.Talon);
     MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMax);
     XboxController driverController = new XboxController(Constants.DRIVER_CONTROLLER_ID);
-    XboxController coDriverController = new XboxController(1);
+    XboxController coDriverController = new XboxController(Constants.CODRIVER_CONTROLLER_ID);
     static AHRS navX = new AHRS(SPI.Port.kMXP);
     SequentialActionRunner auton;
     static ParallelActionRunner teleopActionRunner = new ParallelActionRunner();

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

@@ -1,11 +1,17 @@
 package frc.robot.robotcomponents;
 
+import frc.robot.Constants;
 import frc.robot.Robot;
 
 public class Intake {
 
     public static void startFloorIntake() {
-        Robot.motors.getIntake().set(.5);
+        Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
+    }
+
+    /* This will spin the motor backwards in an attempt to eject a stuck note*/
+    public static void backtrack() {
+        Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
     }
 
     public static void stopFloorIntake() {

src/main/java/frc/robot/RobotComponents/Shooter.java

@@ -15,13 +15,22 @@ public static void startShooterMotors() {
     public static void stopShooterMotors() {
         Robot.motors.getLeftFlywheel().stopMotor();
         Robot.motors.getRightFlywheel().stopMotor();
+        Robot.motors.getFeeder().stopMotor();
     }
 
     public static void startFeedMotors() {
         Robot.getTeleopActionRunner().removeActionsOfType(AutonShooterFeed.class);
         Robot.getTeleopActionRunner().addActionToRun(new AutonShooterFeed());
     }
 
+    /* This will spin the motor backwards in an attempt to eject a stuck note*/
+    public static void backtrack() {
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonShooterFeed.class, AutonShoot.class);
+        Robot.motors.getLeftFlywheel().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
+        Robot.motors.getRightFlywheel().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
+        Robot.motors.getFeeder().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
+    }
+
     public static void autoShootIntoSpeaker() {
         Robot.getTeleopActionRunner().addActionToRun(new AutonShoot());
     }

src/main/java/frc/robot/auton/ParallelActionRunner.java

@@ -26,12 +26,14 @@ public void onEveryFrame() {
 
     // This method is important if you have code that could add two of the same action with opposite goals.
     // That would mean they would fight each other and never finish, which is bad news bears if you ask me.
-    public void removeActionsOfType(Class<?> clazz) {
+    public void removeActionsOfType(Class<?>... clazzez) {
         Iterator<AutonAction> it = actions.iterator();
         while (it.hasNext()) {
             AutonAction auction = it.next();
-            if (auction.getClass().equals(clazz)) {
-                actions.remove(auction);
+            for (Class<?> clazz : clazzez) {
+                if (auction.getClass().equals(clazz)) {
+                    actions.remove(auction);
+                }
             }
         }
     }