From 191a3c5ee743e6ec6ebeeaddd0d030d7b649ae06 Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Sat, 8 Feb 2020 11:34:19 -0600
Subject: [PATCH 01/10] Shooter Routine First Iteration

Shoots balls once motors are at desired speed, increments ball count accordingly. Will not run if ball count <1.
---
 .../java/frc/robot/Settings/Constants.java    |  6 +-
 .../robot/commands/DefaultIntakeRoutine.java  |  8 ++-
 .../frc/robot/commands/ShooterRoutine.java    | 60 +++++++++++++++++++
 .../java/frc/robot/subsystems/Indexer.java    | 10 +++-
 .../java/frc/robot/subsystems/Shooter.java    |  5 ++
 5 files changed, 83 insertions(+), 6 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ShooterRoutine.java

diff --git a/src/main/java/frc/robot/Settings/Constants.java b/src/main/java/frc/robot/Settings/Constants.java
index da93efe..c113c5e 100644
--- a/src/main/java/frc/robot/Settings/Constants.java
+++ b/src/main/java/frc/robot/Settings/Constants.java
@@ -64,6 +64,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 +80,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;
diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index cb60e4f..ad91ddf 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/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.
@@ -51,6 +52,9 @@ public void execute() {
     if(indexer.getSensorBallLeave()){
       Variables.Indexer.finalBallLoaded = true;
     }
+    else{
+      Variables.Indexer.finalBallLoaded = false;
+    }
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/ShooterRoutine.java b/src/main/java/frc/robot/commands/ShooterRoutine.java
new file mode 100644
index 0000000..f5c6496
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ShooterRoutine.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* 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 ShooterRoutine extends CommandBase {
+
+  private Indexer indexer;
+  private Shooter shooter;
+
+  /**
+   * Creates a new ShooterRoutine.
+   */
+  public ShooterRoutine(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() {
+    shooter.runLeftShooterVelocity(Constants.Shooter.shootSpeedRpm);
+
+    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1){
+      indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
+      Variables.Indexer.ballsLoaded --;
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    indexer.stop();
+    shooter.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java
index 492d078..d2ba8c1 100644
--- a/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/Indexer.java
@@ -17,7 +17,7 @@
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 
 public class Indexer extends SubsystemBase {
-  TalonSRX indexerBelt, index, funnel1, funnel2;
+  TalonSRX indexerBelt, shooterFeeder, funnel1, funnel2;
   Solenoid indexSolenoid;
   DigitalInput sensorBallEnter, sensorBallLeave, sensorPositionOne, sensorPositionTwo, sensorPositionThree, sensorPositionFour, sensorPositionFive, sensorPositionSix;
 
@@ -26,7 +26,7 @@ public class Indexer extends SubsystemBase {
    */
   public Indexer() {
     indexerBelt = new TalonSRX(Constants.Indexer.index1TalonID);
-    index = new TalonSRX(Constants.Indexer.index2TalonID);
+    shooterFeeder = new TalonSRX(Constants.Indexer.index2TalonID);
     funnel1 = new TalonSRX(Constants.Indexer.funnel1TalonID);
     funnel2 = new TalonSRX(Constants.Indexer.funnel2TalonID);
     indexSolenoid = new Solenoid(Constants.Indexer.indexSolenoidID);
@@ -41,6 +41,11 @@ public void runIndexMotor(double speed) {
     indexerBelt.set(ControlMode.PercentOutput, speed);
   }
 
+  // runs the shooter feeder using percent output
+  public void runShooterFeederMotor(double speed) {
+    shooterFeeder.set(ControlMode.PercentOutput, speed);
+  }
+
   // runs funnel motor using percent output
   public void runFunnelMotor1(double speed) {
     funnel1.set(ControlMode.PercentOutput, speed);
@@ -64,6 +69,7 @@ public void toggleIndexSolenoid() {
 
   public void stop() {
     runIndexMotor(0);
+    runShooterFeederMotor(0);
     runFunnelMotor1(0);
     runFunnelMotor2(0);
   }
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index b1bbd82..94e18f7 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -74,6 +74,11 @@ public void runLeftShooterVelocity(double speed) {
     shooterLeftMotor.set(ControlMode.Velocity, speed);
   }
 
+  //Stop the shooter motors
+  public void stop() {
+    shooterLeftMotor.set(ControlMode.PercentOutput, 0);
+  }
+
   //Getting encoder distance and rate
   public double getRightEncoderDistance() {
     return shooterRightMotor.getSelectedSensorPosition(0) * Constants.Shooter.shooterEncoderToInches;

From 082ff19225c91cbccceaaa4c60379cc35c29ae78 Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Thu, 13 Feb 2020 17:43:05 -0600
Subject: [PATCH 02/10] Updated Intake Routine for 4 Ball Index, 1 Funnel

---
 .../frc/robot/commands/DefaultIntakeRoutine.java    | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index ad91ddf..8f73462 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
@@ -37,19 +37,26 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    boolean sensorToggle = false;
 
     if(indexer.getSensorBallEnter() & !Variables.Indexer.finalBallLoaded){
       indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
       indexer.toggleIndexSolenoid();
-
-      Variables.Indexer.ballsLoaded ++;
+      
+      if(!sensorToggle){
+        Variables.Indexer.ballsLoaded ++;
+        sensorToggle = true;
+      }
+    }
+    else{
+      sensorToggle = false;
     }
 
     if(indexer.getSensorPositionOne() & !Variables.Indexer.finalBallLoaded){
       indexer.toggleIndexSolenoid();
     }
 
-    if(indexer.getSensorBallLeave()){
+    if(indexer.getSensorBallLeave() || Variables.Indexer.ballsLoaded <= 4){
       Variables.Indexer.finalBallLoaded = true;
     }
     else{

From 43de42282bf2b0f19d6b03af055488ef65db4e6c Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Sat, 15 Feb 2020 15:08:01 -0600
Subject: [PATCH 03/10] Modified Shooter Routine

TODO: Implement semiauto function for shooting balls
---
 src/main/java/frc/robot/RobotContainer.java   |  2 +
 .../java/frc/robot/Settings/Constants.java    |  5 ++
 .../java/frc/robot/commands/FeedShooter.java  | 59 +++++++++++++++++++
 .../java/frc/robot/commands/RevShooter.java   | 49 +++++++++++++++
 .../frc/robot/commands/ShooterRoutine.java    | 57 +++++-------------
 5 files changed, 131 insertions(+), 41 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/FeedShooter.java
 create mode 100644 src/main/java/frc/robot/commands/RevShooter.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1af2d57..ac851e7 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -54,6 +54,8 @@ public RobotContainer() {
         m_Indexer,
         m_Intake)
     );
+
+
   }
 
   /**
diff --git a/src/main/java/frc/robot/Settings/Constants.java b/src/main/java/frc/robot/Settings/Constants.java
index c113c5e..e6b8ac7 100644
--- a/src/main/java/frc/robot/Settings/Constants.java
+++ b/src/main/java/frc/robot/Settings/Constants.java
@@ -16,6 +16,11 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+
+    public final class General {
+        //Shooter Routine
+        public final static boolean semiAutoShoot = true;
+    }
     
     public final class Drivetrain {
         //controllers
diff --git a/src/main/java/frc/robot/commands/FeedShooter.java b/src/main/java/frc/robot/commands/FeedShooter.java
new file mode 100644
index 0000000..87f1eac
--- /dev/null
+++ b/src/main/java/frc/robot/commands/FeedShooter.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1){
+      indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
+      Variables.Indexer.ballsLoaded --;
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    indexer.stop();
+    shooter.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/RevShooter.java b/src/main/java/frc/robot/commands/RevShooter.java
new file mode 100644
index 0000000..8bc2b06
--- /dev/null
+++ b/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;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ShooterRoutine.java b/src/main/java/frc/robot/commands/ShooterRoutine.java
index f5c6496..ac8082d 100644
--- a/src/main/java/frc/robot/commands/ShooterRoutine.java
+++ b/src/main/java/frc/robot/commands/ShooterRoutine.java
@@ -7,54 +7,29 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Settings.Constants;
-import frc.robot.Settings.Variables;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Indexer;
+import frc.robot.Settings.Constants;
+import frc.robot.Settings.Constants.General;
 
-public class ShooterRoutine extends CommandBase {
-
-  private Indexer indexer;
-  private Shooter 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 {
   /**
    * Creates a new ShooterRoutine.
    */
   public ShooterRoutine(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() {
-    shooter.runLeftShooterVelocity(Constants.Shooter.shootSpeedRpm);
-
-    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1){
-      indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
-      Variables.Indexer.ballsLoaded --;
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());super();
+    if(Constants.General.semiAutoShoot){
+      
     }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    indexer.stop();
-    shooter.stop();
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
+    else {
+      new RevShooter(shooter);
+      new FeedShooter(shooter, indexer);
+    }
+    //TODO: Create if statement to decare wether shooter routine should be full or semi auto
   }
 }

From 9c1eaaedda5bbc009377aac96e5f20633d400c1f Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Sat, 15 Feb 2020 16:49:09 -0600
Subject: [PATCH 04/10] Re-organized shoot routine vars + more work on routine

Bools for sensor values are now in index vars
---
 src/main/java/frc/robot/Settings/Constants.java       |  8 ++------
 src/main/java/frc/robot/Settings/Variables.java       |  2 ++
 .../java/frc/robot/commands/DefaultIntakeRoutine.java | 11 +++++++----
 src/main/java/frc/robot/commands/FeedShooter.java     |  3 +++
 src/main/java/frc/robot/commands/ShooterRoutine.java  |  7 ++++---
 5 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/Settings/Constants.java b/src/main/java/frc/robot/Settings/Constants.java
index e6b8ac7..8e7435d 100644
--- a/src/main/java/frc/robot/Settings/Constants.java
+++ b/src/main/java/frc/robot/Settings/Constants.java
@@ -16,11 +16,6 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-
-    public final class General {
-        //Shooter Routine
-        public final static boolean semiAutoShoot = true;
-    }
     
     public final class Drivetrain {
         //controllers
@@ -103,9 +98,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 {
diff --git a/src/main/java/frc/robot/Settings/Variables.java b/src/main/java/frc/robot/Settings/Variables.java
index 7764cda..6ff0574 100644
--- a/src/main/java/frc/robot/Settings/Variables.java
+++ b/src/main/java/frc/robot/Settings/Variables.java
@@ -15,5 +15,7 @@ public class Variables {
     public static class Indexer {
         public static int ballsLoaded = 0;
         public static boolean finalBallLoaded = false;
+        public static boolean enterSensorToggle = false;
+        public static boolean exitSensorToggle = false;
     }
 }
diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index 8f73462..bfcde06 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
@@ -37,19 +37,18 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    boolean sensorToggle = false;
 
     if(indexer.getSensorBallEnter() & !Variables.Indexer.finalBallLoaded){
       indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
       indexer.toggleIndexSolenoid();
       
-      if(!sensorToggle){
+      if(!Variables.Indexer.enterSensorToggle){
         Variables.Indexer.ballsLoaded ++;
-        sensorToggle = true;
+        Variables.Indexer.enterSensorToggle = true;
       }
     }
     else{
-      sensorToggle = false;
+      Variables.Indexer.enterSensorToggle = false;
     }
 
     if(indexer.getSensorPositionOne() & !Variables.Indexer.finalBallLoaded){
@@ -58,9 +57,13 @@ public void execute() {
 
     if(indexer.getSensorBallLeave() || Variables.Indexer.ballsLoaded <= 4){
       Variables.Indexer.finalBallLoaded = true;
+      if(!Variables.Indexer.exitSensorToggle){
+        Variables.Indexer.exitSensorToggle = true;
+      }
     }
     else{
       Variables.Indexer.finalBallLoaded = false;
+      Variables.Indexer.exitSensorToggle = false;
     }
   }
 
diff --git a/src/main/java/frc/robot/commands/FeedShooter.java b/src/main/java/frc/robot/commands/FeedShooter.java
index 87f1eac..f899e40 100644
--- a/src/main/java/frc/robot/commands/FeedShooter.java
+++ b/src/main/java/frc/robot/commands/FeedShooter.java
@@ -40,6 +40,9 @@ public void execute() {
 
     if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1){
       indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
+      if(indexer.getSensorBallLeave()){
+        indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
+      }
       Variables.Indexer.ballsLoaded --;
     }
   }
diff --git a/src/main/java/frc/robot/commands/ShooterRoutine.java b/src/main/java/frc/robot/commands/ShooterRoutine.java
index ac8082d..f54a0c3 100644
--- a/src/main/java/frc/robot/commands/ShooterRoutine.java
+++ b/src/main/java/frc/robot/commands/ShooterRoutine.java
@@ -11,7 +11,7 @@
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Indexer;
 import frc.robot.Settings.Constants;
-import frc.robot.Settings.Constants.General;
+import frc.robot.Settings.Variables;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -23,8 +23,9 @@ public class ShooterRoutine extends ParallelCommandGroup {
   public ShooterRoutine(Shooter shooter, Indexer indexer) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());super();
-    if(Constants.General.semiAutoShoot){
-      
+    if(Constants.Shooter.semiAutoShoot){
+      new RevShooter(shooter);
+      new FeedShooter(shooter, indexer);
     }
     else {
       new RevShooter(shooter);

From f78239e113487ed7bb0641548c658ea4ee7d4d33 Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Sat, 15 Feb 2020 18:02:50 -0600
Subject: [PATCH 05/10] Automatic and semi auto shot modes configured

TODO: Configure default intake routine logic to account for 4 ball index 1 ball funnel system.
---
 src/main/java/frc/robot/Settings/Variables.java       |  1 +
 .../java/frc/robot/commands/DefaultIntakeRoutine.java |  7 +++++--
 src/main/java/frc/robot/commands/FeedShooter.java     | 10 +++++-----
 src/main/java/frc/robot/commands/ShooterRoutine.java  | 11 ++---------
 src/main/java/frc/robot/subsystems/Indexer.java       |  7 +++++++
 5 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/Settings/Variables.java b/src/main/java/frc/robot/Settings/Variables.java
index 6ff0574..da7f7f4 100644
--- a/src/main/java/frc/robot/Settings/Variables.java
+++ b/src/main/java/frc/robot/Settings/Variables.java
@@ -17,5 +17,6 @@ public static class Indexer {
         public static boolean finalBallLoaded = false;
         public static boolean enterSensorToggle = false;
         public static boolean exitSensorToggle = false;
+        public static boolean semiAutoShotComplete = false;
     }
 }
diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index bfcde06..150f9da 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
@@ -40,8 +40,11 @@ public void execute() {
 
     if(indexer.getSensorBallEnter() & !Variables.Indexer.finalBallLoaded){
       indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
-      indexer.toggleIndexSolenoid();
       
+      if(!indexer.getSensorPositionOne()){
+        indexer.setIndexSolenoid(false);
+      }
+
       if(!Variables.Indexer.enterSensorToggle){
         Variables.Indexer.ballsLoaded ++;
         Variables.Indexer.enterSensorToggle = true;
@@ -52,7 +55,7 @@ public void execute() {
     }
 
     if(indexer.getSensorPositionOne() & !Variables.Indexer.finalBallLoaded){
-      indexer.toggleIndexSolenoid();
+      indexer.setIndexSolenoid(true);
     }
 
     if(indexer.getSensorBallLeave() || Variables.Indexer.ballsLoaded <= 4){
diff --git a/src/main/java/frc/robot/commands/FeedShooter.java b/src/main/java/frc/robot/commands/FeedShooter.java
index f899e40..0794bf9 100644
--- a/src/main/java/frc/robot/commands/FeedShooter.java
+++ b/src/main/java/frc/robot/commands/FeedShooter.java
@@ -32,18 +32,18 @@ public FeedShooter(Shooter shooter, Indexer indexer) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    Variables.Indexer.semiAutoShotComplete = false;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-
-    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1){
+    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1 & !Variables.Indexer.semiAutoShotComplete){
       indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
-      if(indexer.getSensorBallLeave()){
-        indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
-      }
       Variables.Indexer.ballsLoaded --;
+      if(Constants.Shooter.semiAutoShoot) {
+        Variables.Indexer.semiAutoShotComplete = true;
+      }
     }
   }
 
diff --git a/src/main/java/frc/robot/commands/ShooterRoutine.java b/src/main/java/frc/robot/commands/ShooterRoutine.java
index f54a0c3..fae778e 100644
--- a/src/main/java/frc/robot/commands/ShooterRoutine.java
+++ b/src/main/java/frc/robot/commands/ShooterRoutine.java
@@ -23,14 +23,7 @@ public class ShooterRoutine extends ParallelCommandGroup {
   public ShooterRoutine(Shooter shooter, Indexer indexer) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());super();
-    if(Constants.Shooter.semiAutoShoot){
-      new RevShooter(shooter);
-      new FeedShooter(shooter, indexer);
-    }
-    else {
-      new RevShooter(shooter);
-      new FeedShooter(shooter, indexer);
-    }
-    //TODO: Create if statement to decare wether shooter routine should be full or semi auto
+    new RevShooter(shooter);
+    new FeedShooter(shooter, indexer);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java
index d2ba8c1..c6eb32a 100644
--- a/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/Indexer.java
@@ -67,6 +67,13 @@ public void toggleIndexSolenoid() {
     indexSolenoid.set(state);
   }
 
+  //Opens or closes index solenoid
+  public void setIndexSolenoid(boolean state) {
+    if(indexSolenoid.get() != state){
+      indexSolenoid.set(state);
+    }
+  }
+
   public void stop() {
     runIndexMotor(0);
     runShooterFeederMotor(0);

From 9ea81fde739227f10bf180485a71d86f2e6ed5ba Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Thu, 20 Feb 2020 16:38:24 -0600
Subject: [PATCH 06/10] General Work

---
 src/main/java/frc/robot/ControlBoard.java        |  5 +++++
 .../robot/Controllers/IOperatorController.java   |  1 +
 src/main/java/frc/robot/RobotContainer.java      | 11 ++++++++++-
 src/main/java/frc/robot/Settings/Constants.java  |  3 +++
 src/main/java/frc/robot/Settings/Variables.java  |  1 -
 .../java/frc/robot/commands/FeedShooter.java     | 16 +++++++++-------
 .../java/frc/robot/commands/ShooterRoutine.java  | 14 ++++++++------
 .../java/frc/robot/manipulators/ButtonBoard.java |  9 +++++++++
 8 files changed, 45 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/ControlBoard.java b/src/main/java/frc/robot/ControlBoard.java
index 1d05062..c52901c 100644
--- a/src/main/java/frc/robot/ControlBoard.java
+++ b/src/main/java/frc/robot/ControlBoard.java
@@ -51,4 +51,9 @@ public JoystickButton returnIntakeButton() {
         // TODO Auto-generated method stub
         return mOperatorController.returnIntakeButton();
     }
+
+    @Override
+    public JoystickButton returnRevShooterButton() {
+        return mOperatorController.returnRevShooterButton();
+    }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Controllers/IOperatorController.java b/src/main/java/frc/robot/Controllers/IOperatorController.java
index 04ed5db..6897f2b 100644
--- a/src/main/java/frc/robot/Controllers/IOperatorController.java
+++ b/src/main/java/frc/robot/Controllers/IOperatorController.java
@@ -16,6 +16,7 @@
 public interface IOperatorController {
 
      public JoystickButton returnIntakeButton();
+     public JoystickButton returnRevShooterButton();
 
 }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ac851e7..69759f5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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.
@@ -62,10 +69,12 @@ public RobotContainer() {
    * 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);
   }
 
 
diff --git a/src/main/java/frc/robot/Settings/Constants.java b/src/main/java/frc/robot/Settings/Constants.java
index 8e7435d..5f4872e 100644
--- a/src/main/java/frc/robot/Settings/Constants.java
+++ b/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;
diff --git a/src/main/java/frc/robot/Settings/Variables.java b/src/main/java/frc/robot/Settings/Variables.java
index da7f7f4..6ff0574 100644
--- a/src/main/java/frc/robot/Settings/Variables.java
+++ b/src/main/java/frc/robot/Settings/Variables.java
@@ -17,6 +17,5 @@ public static class Indexer {
         public static boolean finalBallLoaded = false;
         public static boolean enterSensorToggle = false;
         public static boolean exitSensorToggle = false;
-        public static boolean semiAutoShotComplete = false;
     }
 }
diff --git a/src/main/java/frc/robot/commands/FeedShooter.java b/src/main/java/frc/robot/commands/FeedShooter.java
index 0794bf9..263c80b 100644
--- a/src/main/java/frc/robot/commands/FeedShooter.java
+++ b/src/main/java/frc/robot/commands/FeedShooter.java
@@ -32,18 +32,16 @@ public FeedShooter(Shooter shooter, Indexer indexer) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    Variables.Indexer.semiAutoShotComplete = false;
+
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1 & !Variables.Indexer.semiAutoShotComplete){
+    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1){
       indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
+      
       Variables.Indexer.ballsLoaded --;
-      if(Constants.Shooter.semiAutoShoot) {
-        Variables.Indexer.semiAutoShotComplete = true;
-      }
     }
   }
 
@@ -51,12 +49,16 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     indexer.stop();
-    shooter.stop();
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    if(indexer.getSensorBallLeave()){
+      return true;  
+    }
+    else{
+      return false;
+    }
   }
 }
diff --git a/src/main/java/frc/robot/commands/ShooterRoutine.java b/src/main/java/frc/robot/commands/ShooterRoutine.java
index fae778e..7cb0f6b 100644
--- a/src/main/java/frc/robot/commands/ShooterRoutine.java
+++ b/src/main/java/frc/robot/commands/ShooterRoutine.java
@@ -8,22 +8,24 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Indexer;
-import frc.robot.Settings.Constants;
-import frc.robot.Settings.Variables;
+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(Shooter shooter, Indexer indexer) {
+  public ShooterRoutine() {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());super();
-    new RevShooter(shooter);
-    new FeedShooter(shooter, indexer);
+    addCommands(
+      new RevShooter(m_Shooter),
+      new FeedShooter(m_Shooter, m_Indexer));
   }
 }
diff --git a/src/main/java/frc/robot/manipulators/ButtonBoard.java b/src/main/java/frc/robot/manipulators/ButtonBoard.java
index bd2d666..8542182 100644
--- a/src/main/java/frc/robot/manipulators/ButtonBoard.java
+++ b/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,6 +34,7 @@ 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
@@ -38,4 +42,9 @@ public JoystickButton returnIntakeButton() {
         // TODO Auto-generated method stub
         return intakeButton;
     }
+
+    @Override
+    public JoystickButton returnRevShooterButton() {
+        return revShooterButton;
+    }
 }

From 13a4a7fec4d59c7460fd4f20ae5b211081894f8e Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Thu, 20 Feb 2020 19:14:59 -0600
Subject: [PATCH 07/10] PS4 controller added, I think the intake routine is
 good now

---
 src/main/java/frc/robot/ControlBoard.java     |  7 ++-
 .../java/frc/robot/Settings/Variables.java    |  2 -
 .../robot/commands/DefaultIntakeRoutine.java  | 28 +++++------
 src/main/java/frc/robot/manipulators/PS4.java | 50 +++++++++++++++++++
 4 files changed, 68 insertions(+), 19 deletions(-)
 create mode 100644 src/main/java/frc/robot/manipulators/PS4.java

diff --git a/src/main/java/frc/robot/ControlBoard.java b/src/main/java/frc/robot/ControlBoard.java
index c52901c..f3dff76 100644
--- a/src/main/java/frc/robot/ControlBoard.java
+++ b/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
diff --git a/src/main/java/frc/robot/Settings/Variables.java b/src/main/java/frc/robot/Settings/Variables.java
index 6ff0574..7764cda 100644
--- a/src/main/java/frc/robot/Settings/Variables.java
+++ b/src/main/java/frc/robot/Settings/Variables.java
@@ -15,7 +15,5 @@ public class Variables {
     public static class Indexer {
         public static int ballsLoaded = 0;
         public static boolean finalBallLoaded = false;
-        public static boolean enterSensorToggle = false;
-        public static boolean exitSensorToggle = false;
     }
 }
diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index 150f9da..5d0df75 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
@@ -18,6 +18,8 @@ public class DefaultIntakeRoutine extends CommandBase {
   private Indexer indexer;
   private Intake intake;
 
+  boolean firstSensorTrigger = false;
+
   /**
    * Creates a new IntakeRoutine.
    */
@@ -40,33 +42,27 @@ public void execute() {
 
     if(indexer.getSensorBallEnter() & !Variables.Indexer.finalBallLoaded){
       indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
-      
-      if(!indexer.getSensorPositionOne()){
-        indexer.setIndexSolenoid(false);
-      }
-
-      if(!Variables.Indexer.enterSensorToggle){
-        Variables.Indexer.ballsLoaded ++;
-        Variables.Indexer.enterSensorToggle = true;
-      }
+      indexer.setIndexSolenoid(false);
     }
     else{
-      Variables.Indexer.enterSensorToggle = false;
+      indexer.setIndexSolenoid(true);
     }
 
-    if(indexer.getSensorPositionOne() & !Variables.Indexer.finalBallLoaded){
-      indexer.setIndexSolenoid(true);
+    if(indexer.getSensorPositionOne() & !firstSensorTrigger){
+      indexer.runIndexMotor(0);
+      firstSensorTrigger = true;
+      Variables.Indexer.ballsLoaded ++;
+    }
+    else{
+      firstSensorTrigger = false;
     }
 
     if(indexer.getSensorBallLeave() || Variables.Indexer.ballsLoaded <= 4){
       Variables.Indexer.finalBallLoaded = true;
-      if(!Variables.Indexer.exitSensorToggle){
-        Variables.Indexer.exitSensorToggle = true;
-      }
+      indexer.setIndexSolenoid(false);
     }
     else{
       Variables.Indexer.finalBallLoaded = false;
-      Variables.Indexer.exitSensorToggle = false;
     }
   }
 
diff --git a/src/main/java/frc/robot/manipulators/PS4.java b/src/main/java/frc/robot/manipulators/PS4.java
new file mode 100644
index 0000000..5279b6c
--- /dev/null
+++ b/src/main/java/frc/robot/manipulators/PS4.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.manipulators;
+
+import edu.wpi.first.wpilibj.Joystick;
+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.
+ */
+public class PS4 implements IOperatorController {
+
+    private Joystick m_Joystick;
+    private static PS4 m_Instance = null;
+    private JoystickButton intakeButton;
+    private JoystickButton revShooterButton;
+
+    public static PS4 getInstance(){
+        if (m_Instance == null){
+            m_Instance = new PS4();
+        }
+        return m_Instance;
+    }
+
+    private PS4(){
+        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;
+    }
+}

From 74a00fac8c36406b622ceb1acc763c87e4c651fa Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Thu, 20 Feb 2020 19:59:38 -0600
Subject: [PATCH 08/10] Everything is set up except semi-auto, vars
 reorganized.

---
 .../java/frc/robot/Settings/Variables.java    |  5 ---
 .../robot/commands/DefaultIntakeRoutine.java  | 17 ++------
 .../java/frc/robot/commands/FeedShooter.java  | 10 +++--
 .../java/frc/robot/subsystems/Indexer.java    | 41 +++++++++++++++++++
 4 files changed, 50 insertions(+), 23 deletions(-)

diff --git a/src/main/java/frc/robot/Settings/Variables.java b/src/main/java/frc/robot/Settings/Variables.java
index 7764cda..daa0274 100644
--- a/src/main/java/frc/robot/Settings/Variables.java
+++ b/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;
-    }
 }
diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index 5d0df75..f09956c 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
@@ -18,8 +18,6 @@ public class DefaultIntakeRoutine extends CommandBase {
   private Indexer indexer;
   private Intake intake;
 
-  boolean firstSensorTrigger = false;
-
   /**
    * Creates a new IntakeRoutine.
    */
@@ -40,7 +38,7 @@ public void initialize() {
   @Override
   public void execute() {
 
-    if(indexer.getSensorBallEnter() & !Variables.Indexer.finalBallLoaded){
+    if(indexer.getSensorBallEnter() & !indexer.getFinalBallLoaded()){
       indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
       indexer.setIndexSolenoid(false);
     }
@@ -48,22 +46,13 @@ public void execute() {
       indexer.setIndexSolenoid(true);
     }
 
-    if(indexer.getSensorPositionOne() & !firstSensorTrigger){
+    if(indexer.getSensorPositionOne()){
       indexer.runIndexMotor(0);
-      firstSensorTrigger = true;
-      Variables.Indexer.ballsLoaded ++;
-    }
-    else{
-      firstSensorTrigger = false;
     }
 
-    if(indexer.getSensorBallLeave() || Variables.Indexer.ballsLoaded <= 4){
-      Variables.Indexer.finalBallLoaded = true;
+    if(indexer.getSensorBallLeave() || indexer.getBallsLoaded() <= 4){
       indexer.setIndexSolenoid(false);
     }
-    else{
-      Variables.Indexer.finalBallLoaded = false;
-    }
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/FeedShooter.java b/src/main/java/frc/robot/commands/FeedShooter.java
index 263c80b..b97fd4f 100644
--- a/src/main/java/frc/robot/commands/FeedShooter.java
+++ b/src/main/java/frc/robot/commands/FeedShooter.java
@@ -32,16 +32,18 @@ public FeedShooter(Shooter shooter, Indexer indexer) {
   // 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(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & Variables.Indexer.ballsLoaded >= 1){
+
+    if(!indexer.getSensorBallLeave()){
+      indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
+    }
+
+    if(shooter.getEncoderRate() >= Constants.Shooter.shootSpeedRps & indexer.getBallsLoaded() >= 1){
       indexer.runShooterFeederMotor(Constants.Indexer.shooterFeederSpeed);
-      
-      Variables.Indexer.ballsLoaded --;
     }
   }
 
diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java
index c6eb32a..cd296a7 100644
--- a/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/Indexer.java
@@ -8,6 +8,7 @@
 package frc.robot.subsystems;
 
 import frc.robot.Settings.Constants;
+import frc.robot.Settings.Variables;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -20,6 +21,11 @@ public class Indexer extends SubsystemBase {
   TalonSRX indexerBelt, shooterFeeder, funnel1, funnel2;
   Solenoid indexSolenoid;
   DigitalInput sensorBallEnter, sensorBallLeave, sensorPositionOne, sensorPositionTwo, sensorPositionThree, sensorPositionFour, sensorPositionFive, sensorPositionSix;
+  
+  boolean firstSensorTrigger = false;
+  boolean lastSensorTrigger = false;
+  boolean finalBallLoaded = false;
+  int ballsLoaded = 0;
 
   /**
    * Creates a new Indexer.
@@ -115,8 +121,43 @@ public boolean getSensorPositionFive(){
   public boolean getSensorPositionSix(){
     return sensorPositionSix.get();
   }
+
+  public int getBallsLoaded() {
+    return ballsLoaded;
+  }
+
+  public boolean getFinalBallLoaded() {
+    return finalBallLoaded;
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    if(getSensorPositionOne()){
+      if(!firstSensorTrigger){
+        firstSensorTrigger = true;
+        ballsLoaded ++;
+      }
+    }
+    else{
+      firstSensorTrigger = false;
+    }
+
+    if(!getSensorBallLeave()){
+      if(!lastSensorTrigger){
+        lastSensorTrigger = true;
+        ballsLoaded--;
+      }
+    }
+    else{
+      lastSensorTrigger = false;
+    }
+
+    if(getSensorBallLeave() || ballsLoaded <= 4){
+      finalBallLoaded = true;
+    }
+    else{
+      finalBallLoaded = false;
+    }
   }
 }
\ No newline at end of file

From 97ee90b03d6144d5098121b75785f425672cf768 Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Tue, 25 Feb 2020 16:44:30 -0600
Subject: [PATCH 09/10] EVERYTHING IS BROKEN I HATE BUILD

it dosent work anymore.
---
 src/main/java/frc/robot/commands/DefaultIntakeRoutine.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index f09956c..e034094 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
@@ -46,7 +46,7 @@ public void execute() {
       indexer.setIndexSolenoid(true);
     }
 
-    if(indexer.getSensorPositionOne()){
+    if(indexer.getSensorPositionOne() & !indexer.getSensorBallEnter()){
       indexer.runIndexMotor(0);
     }
 

From af8671eda81be2e909c91f95b6ed44db8cf4e375 Mon Sep 17 00:00:00 2001
From: subearthling <hmaclaughlin@greatriverschool.org>
Date: Thu, 27 Feb 2020 17:11:35 -0600
Subject: [PATCH 10/10] Door gone
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

⠄⠄⠄⠄⠄⠄⠄⠄⠄⢀⣀⣀⣄⣶⡶⣦⣀⠄⠄⠄⠄⠄⠄⠄
⠄⠄⠄⠄⠄⢠⡦⡟⠻⠛⠙⠉⠈⠄⠄⠈⠻⠛⣾⣦⣤⣀⠄⠄
⠄⠄⠄⣰⡿⠟⠃⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠘⠋⠽⢿⣧⠄
⠄⢀⣴⠞⠂⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⢼⠆⠄
⠄⣼⠇⠄⠄⠄⠄⠄⠄⠄⠄⣀⣠⣤⣶⣿⣶⣦⣤⣀⠄⣻⡃⠄
⠄⡿⠄⠄⠄⠄⠄⠄⠄⠄⠄⣿⣿⣿⣿⣿⣿⣿⣿⣿⣦⢸⣧⠄
⠄⢿⡀⠄⠄⠄⠄⠄⠄⠄⢠⣾⣿⣿⣋⣩⣭⣝⣿⣿⠛⢰⡇⠄
⠄⢸⡇⠄⠄⢀⠄⠄⠄⠄⣾⣿⣿⣿⣟⣯⠉⢉⣿⠋⣟⢻⡇⠄
⠄⠄⢹⡀⢳⡗⠂⣠⠄⠄⣿⣿⣿⣿⣿⣭⣽⣿⣿⣿⣉⣸⠇⠄
⠄⠄⠈⣷⠄⢳⣷⣿⠄⠄⢹⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡇⠄
⠄⠄⠄⠘⣧⠄⠈⠙⠄⠄⠄⠉⠙⠛⠛⣿⣿⣷⣤⣄⢿⡿⠃⠄
⠄⠄⠄⠄⠉⠳⣄⡀⠄⠄⠄⢢⣦⣾⣿⠿⠿⠛⠉⢉⣽⠇⠄⠄
⠄⠄⠄⠄⠄⠄⠘⠿⣄⢀⠄⣀⣝⢻⣿⡿⠒⣀⣀⣸⠁⠄⠄⠄
⠄⠄⠄⠄⠄⠄⠄⠄⠈⠳⣤⠁⠙⠎⢻⣄⠄⠄⣸⠋⠄⠄⠄⠄
⠄⠄⠄⠄⠄⠄⠄⠄⠄⠄⠈⠙⠶⢦⣄⣀⣣⠴⠃⠄⠄⠄⠄⠄
---
 .../java/frc/robot/commands/DefaultIntakeRoutine.java | 11 ++---------
 1 file changed, 2 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
index e034094..384c091 100644
--- a/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
+++ b/src/main/java/frc/robot/commands/DefaultIntakeRoutine.java
@@ -40,18 +40,11 @@ public void execute() {
 
     if(indexer.getSensorBallEnter() & !indexer.getFinalBallLoaded()){
       indexer.runIndexMotor(Constants.Indexer.indexIntakeSpeed);
-      indexer.setIndexSolenoid(false);
-    }
-    else{
-      indexer.setIndexSolenoid(true);
-    }
 
-    if(indexer.getSensorPositionOne() & !indexer.getSensorBallEnter()){
-      indexer.runIndexMotor(0);
     }
 
-    if(indexer.getSensorBallLeave() || indexer.getBallsLoaded() <= 4){
-      indexer.setIndexSolenoid(false);
+    if(indexer.getSensorPositionOne() & !indexer.getSensorBallEnter() & !indexer.getFinalBallLoaded()){
+      indexer.runIndexMotor(0);
     }
   }