Hugh shooter routine

src/main/java/frc/robot/ControlBoard.java

@@ -4,6 +4,7 @@
 import frc.robot.Controllers.IDriveController;
 import frc.robot.Controllers.IOperatorController;
 import frc.robot.manipulators.ButtonBoard;
+import frc.robot.manipulators.PS4;
 import frc.robot.manipulators.TM;
 
 public class ControlBoard implements IControlBoard {
@@ -24,8 +25,12 @@ public static ControlBoard getInstance() {
 
     private ControlBoard() {
         mDriveController = TM.getInstance();
+
+        //Buttonboard is currently disabled
+        //mOperatorController = ButtonBoard.getInstance();
 
-        mOperatorController = ButtonBoard.getInstance();
+        //PS4 controller is currently enabled
+        mOperatorController = PS4.getInstance();
     }
 
     @Override
@@ -51,4 +56,9 @@ public JoystickButton returnIntakeButton() {
         // TODO Auto-generated method stub
         return mOperatorController.returnIntakeButton();
     }
+
+    @Override
+    public JoystickButton returnRevShooterButton() {
+        return mOperatorController.returnRevShooterButton();
+    }
 }

src/main/java/frc/robot/Controllers/IOperatorController.java

@@ -16,6 +16,7 @@
 public interface IOperatorController {
 
      public JoystickButton returnIntakeButton();
+     public JoystickButton returnRevShooterButton();
 
 }
 

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

@@ -11,13 +11,18 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.drivetrain.Drive;
+import frc.robot.commands.intake.AutoIntake;
 import frc.robot.commands.DefaultIntakeRoutine;
+import frc.robot.commands.FeedShooter;
+import frc.robot.commands.RevShooter;
+import frc.robot.commands.ShooterRoutine;
 import frc.robot.commands.shooter.RunShooterAtSpeedPID;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Intake;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -35,6 +40,8 @@ public class RobotContainer {
   private final ControlBoard m_ControlBoard = ControlBoard.getInstance();
 
   private final RunShooterAtSpeedPID shooterAtSpeedPID = new RunShooterAtSpeedPID(m_Shooter);
+  private final ShooterRoutine shooterRoutine = new ShooterRoutine();
+  private final AutoIntake autoIntake = new AutoIntake(m_Intake);
 
   /**
    * The container for the robot.  Contains subsystems, OI devices, and commands.
@@ -54,16 +61,20 @@ public RobotContainer() {
         m_Indexer,
         m_Intake)
     );
+
+
   }
 
   /**
    * Use this method to define your button->command mappings.  Buttons can be created by
    * instantiating a {@link GenericHID} or one of its subclasses ({@link
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * {@link edu.wpi.first.wpilibj2.command.buttson.JoystickButton}.
    */
   private void configureButtonBindings() {
     SmartDashboard.putData(shooterAtSpeedPID);
+    m_ControlBoard.returnIntakeButton().whenPressed(autoIntake);
+    m_ControlBoard.returnRevShooterButton().whenPressed(shooterRoutine, false);
   }
 
 

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

@@ -47,6 +47,8 @@ public final class Controller{
 
         public final class ButtonBoard{
             public static final int intakeButtonID = 0; //TODO change to actual value
+            public static final int revShooterButtonID = 0; //TODO change to actual value
+            public static final int feedShooterButtonID = 0; //TODO change to actual value
         }
     }
 
@@ -55,6 +57,7 @@ public final class Intake {
         public static final int intakeSolenoidPort = 0;
         public static final int autoIntakeSpeed = 0;
     }
+
     public final class Indexer {
         //Motors
         public static final int index1TalonID = 0;
@@ -64,6 +67,7 @@ public final class Indexer {
         public static final int indexSolenoidID = 0;
         //Motor Speeds
         public static final double indexIntakeSpeed = 0;
+        public static final double shooterFeederSpeed = 0;
         //Sensors
         public static final int sensorOnePin = 1;
         public static final int sensorTwoPin = 2;
@@ -79,13 +83,14 @@ public final class Shooter {
         //shooter
         public static final int shooterTalonLeftMotor = 0;
         public static final int shooterTalonRightMotor = 0;
-
         //encoders
         public final static double shooterEncoderTicks = 2048.0; //Encoder ticks per wheel rotation is 2048
         public final static double shooterWheelDiameter = 4.0; //Inches
         public final static double shooterEncoderToInches = shooterWheelDiameter * Math.PI / shooterEncoderTicks; //Makes number inches
         public final static double shooterEncoderVelocityToRPS = 1.0 / shooterEncoderTicks * 10;
-
+        //Speeds
+        public static final double shootSpeedRpm = 0; //Rpm
+        public static final double shootSpeedRps = shootSpeedRpm / 60;
         //PID
         public final static int SlotIdx = 0;
         public final static int PIDLoopIdx = 0;
@@ -96,9 +101,10 @@ public final class Shooter {
 	    public final static double kF = 0;
 	    public final static int kIzone = 0;
         public final static double PeakOutput = 0;
-
         //Values
         public final static double testSpeed = 0;
+        //Modes
+        public final static boolean semiAutoShoot = true;
     }
 
     public final class Climber {

src/main/java/frc/robot/Settings/Variables.java

@@ -11,9 +11,4 @@
  * Add your docs here.
  */
 public class Variables {
-
-    public static class Indexer {
-        public static int ballsLoaded = 0;
-        public static boolean finalBallLoaded = false;
-    }
 }

src/main/java/frc/robot/commands/DefaultIntakeRoutine.java

@@ -15,8 +15,8 @@
 
 public class DefaultIntakeRoutine extends CommandBase {
 
-private Indexer indexer;
-private Intake intake;
+  private Indexer indexer;
+  private Intake intake;
 
   /**
    * Creates a new IntakeRoutine.
@@ -26,6 +26,7 @@ public DefaultIntakeRoutine(Indexer indexer, Intake intake) {
     this.indexer = indexer;
     this.intake = intake;
     addRequirements(indexer);
+    addRequirements(intake);
   }
 
   // Called when the command is initially scheduled.
@@ -37,19 +38,13 @@ public void initialize() {
   @Override
   public void execute() {
 
-    if(indexer.getSensorBallEnter() & !Variables.Indexer.finalBallLoaded){
+    if(indexer.getSensorBallEnter() & !indexer.getFinalBallLoaded()){
       indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
-      indexer.toggleIndexSolenoid();
 
-      Variables.Indexer.ballsLoaded ++;
     }
 
-    if(indexer.getSensorPositionOne() & !Variables.Indexer.finalBallLoaded){
-      indexer.toggleIndexSolenoid();
-    }
-
-    if(indexer.getSensorBallLeave()){
-      Variables.Indexer.finalBallLoaded = true;
+    if(indexer.getSensorPositionOne() & !indexer.getSensorBallEnter() & !indexer.getFinalBallLoaded()){
+      indexer.runIndexMotor(0);
     }
   }
 

src/main/java/frc/robot/commands/FeedShooter.java

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Settings.Constants;
+import frc.robot.Settings.Variables;
+import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Indexer;
+
+public class FeedShooter extends CommandBase {
+
+  private Indexer indexer;
+  private Shooter shooter;
+
+  /**
+   * Creates a new ShooterRoutine.
+   */
+  public FeedShooter(Shooter shooter, Indexer indexer) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.indexer = indexer;
+    this.shooter = shooter;
+    addRequirements(indexer);
+    addRequirements(shooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    if(!indexer.getSensorBallLeave()){
+      indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
+    }
+
+    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & indexer.getBallsLoaded() >= 1){
+      indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    indexer.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if(indexer.getSensorBallLeave()){
+      return true;  
+    }
+    else{
+      return false;
+    }
+  }
+}

src/main/java/frc/robot/commands/RevShooter.java

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Shooter;
+import frc.robot.Settings.Constants;
+
+public class RevShooter extends CommandBase {
+
+  public Shooter shooter;
+
+  /**
+   * Creates a new RevShooter.
+   */
+  public RevShooter(Shooter shooter) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.shooter = shooter;
+    addRequirements(shooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    shooter.runLeftShooterVelocity(Constants.Shooter.shootSpeedRpm);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    shooter.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/ShooterRoutine.java

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.subsystems.Indexer;
+import frc.robot.subsystems.Shooter;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class ShooterRoutine extends ParallelCommandGroup {
+
+  Shooter m_Shooter = new Shooter();
+  Indexer m_Indexer = new Indexer();
+  /**
+   * Creates a new ShooterRoutine.
+   */
+  public ShooterRoutine() {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());super();
+    addCommands(
+      new RevShooter(m_Shooter),
+      new FeedShooter(m_Shooter, m_Indexer));
+  }
+}

src/main/java/frc/robot/manipulators/ButtonBoard.java

@@ -11,6 +11,8 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.Controllers.IOperatorController;
 import frc.robot.Settings.Constants;
+import frc.robot.commands.RevShooter;
+import frc.robot.commands.FeedShooter;;
 
 /**
  * Add your docs here.
@@ -20,6 +22,7 @@ public class ButtonBoard implements IOperatorController {
     private Joystick m_Joystick;
     private static ButtonBoard m_Instance = null;
     private JoystickButton intakeButton;
+    private JoystickButton revShooterButton;
 
     public static ButtonBoard getInstance(){
         if (m_Instance == null){
@@ -31,11 +34,17 @@ public static ButtonBoard getInstance(){
     private ButtonBoard(){
         m_Joystick = new Joystick(Constants.Controller.opertatorControllerID);
         intakeButton = new JoystickButton(m_Joystick, Constants.Controller.ButtonBoard.intakeButtonID);
+        revShooterButton = new JoystickButton(m_Joystick, Constants.Controller.ButtonBoard.revShooterButtonID);
     }
 
     @Override
     public JoystickButton returnIntakeButton() {
         // TODO Auto-generated method stub
         return intakeButton;
     }
+
+    @Override
+    public JoystickButton returnRevShooterButton() {
+        return revShooterButton;
+    }
 }