Ball Tracker

src/main/java/com/pigmice/frc/robot/BallTracker.java

@@ -1,43 +1,39 @@
 package com.pigmice.frc.robot;
 
-import java.util.LinkedList;
+import java.util.Deque;
+import java.util.ArrayDeque;
 
-// this here class is to find where yo ballz at
+//this here class is to find where yo ballz at
 public class BallTracker {
+    Deque<BallColor> balls = new ArrayDeque<BallColor>();
 
-    private static final int SIZE = 2;
-
-    LinkedList<BallType> balls = new LinkedList<>();
-
-    /**
-     * Adds a ball to the queue. Used when a ball has been collected.
-     */
-    public void newBallStored(BallType color) {
-        if (!isFull()) {
-            balls.add(color);
-        }
+    public void newBall(BallColor color) {
+        balls.add(color);
     }
 
-    /**
-     * Removes the ball at the head of the queue. Used when a ball is shot.
-     */
-    public BallType ballLaunched() {
-        return balls.isEmpty() ? BallType.NONE : balls.poll();
+    public void ballLaunched() {
+        balls.removeFirst();
     }
 
-    /**
-     * Returns the ball in a specific position in the indexer.
-     */
-    public BallType getBallInPosition(int slot) {
-        // BallType.NONE should never be in the queue
-        return balls.size() > slot ? balls.get(slot) : BallType.NONE;
+    // return the color of ball in a spot. 0 is the next one to be shot, 1 is the
+    // second one to be shot
+    public BallColor getCompartment(int spotNumber) {
+        if (spotNumber == 0) {
+            return balls.getFirst();
+        } else {
+            if (balls.size() == 2) {
+                return balls.getLast();
+            } else {
+                return BallColor.NONE;
+            }
+        }
     }
 
-    public boolean isFull() {
-        return balls.size() >= SIZE;
+    public boolean holdingColor(BallColor color) {
+        return balls.contains(color);
     }
 
-    enum BallType {
+    public enum BallColor {
         RED, BLUE, NONE
     }
 }

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

@@ -19,8 +19,9 @@
  */
 public final class Constants {
     public static class ShooterConfig {
-        public static final int topMotorPort = 10;
-        public static final int bottomMotorPort = 9;
+        public static final int topMotorPort = 1;
+        public static final int bottomMotorPort = 2;
+        public static final int indexerport = 3;
 
         public static final double topMotorSpeed = 0.62;
         public static final double bottomMotorSpeed = 0.62;

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

@@ -7,26 +7,28 @@
 import java.util.ArrayList;
 import java.util.List;
 
-import com.pigmice.frc.robot.commands.Indexer.SpinIndexerToAngle;
 import com.pigmice.frc.robot.commands.Indexer.SpinIndexerToAngleOld;
 import com.pigmice.frc.robot.commands.drivetrain.ArcadeDrive;
 import com.pigmice.frc.robot.commands.drivetrain.DriveDistance;
 import com.pigmice.frc.robot.commands.shooter.ShootBallCommand;
 import com.pigmice.frc.robot.subsystems.Drivetrain;
 import com.pigmice.frc.robot.subsystems.Indexer;
 import com.pigmice.frc.robot.subsystems.Intake;
+/* import com.pigmice.frc.robot.subsystems.Climber;
+import com.pigmice.frc.robot.subsystems.Drivetrain;
+import com.pigmice.frc.robot.subsystems.Indexer;
+import com.pigmice.frc.robot.subsystems.Intake;
+import com.pigmice.frc.robot.subsystems.Lights; */
 import com.pigmice.frc.robot.subsystems.Shooter;
 import com.pigmice.frc.robot.subsystems.climber.Lifty;
 import com.pigmice.frc.robot.subsystems.climber.Rotato;
 import com.pigmice.frc.robot.testmode.Testable;
+/* import com.pigmice.frc.robot.Controls;
+import com.pigmice.frc.robot.commands.ShootBallCommand; */
 
-import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.XboxController.*;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
@@ -113,36 +115,37 @@ private void configureButtonBindings(XboxController driver, XboxController opera
     // .whenPressed(this.indexer::resetEncoder)
     // .whenPressed(new SpinIndexerToAngle(indexer, 90, false));
 
-    /*new JoystickButton(driver, Button.kA.value)
-        .whenPressed(new SequentialCommandGroup(
-            new InstantCommand(() -> this.shooter.enable()),
-            new WaitUntilCommand(this.shooter::isAtTargetVelocity),
-            new InstantCommand(() -> this.indexer.resetEncoder()),
-            new InstantCommand(() -> this.indexer.enable()),
-            new SpinIndexerToAngle(indexer, 200, false),
-            new WaitUntilCommand(this.shooter::isAtTargetVelocity),
-            new InstantCommand(() -> this.indexer.resetEncoder()),
-            new InstantCommand(() -> this.indexer.enable()),
-            new SpinIndexerToAngle(indexer, 200, false)))
-
-        .whenReleased(() -> {
-          this.shooter.disable();
-          this.indexer.disable();
-        });*/
+    /*
+     * new JoystickButton(driver, Button.kA.value)
+     * .whenPressed(new SequentialCommandGroup(
+     * new InstantCommand(() -> this.shooter.enable()),
+     * new WaitUntilCommand(this.shooter::isAtTargetVelocity),
+     * new InstantCommand(() -> this.indexer.resetEncoder()),
+     * new InstantCommand(() -> this.indexer.enable()),
+     * new SpinIndexerToAngle(indexer, 200, false),
+     * new WaitUntilCommand(this.shooter::isAtTargetVelocity),
+     * new InstantCommand(() -> this.indexer.resetEncoder()),
+     * new InstantCommand(() -> this.indexer.enable()),
+     * new SpinIndexerToAngle(indexer, 200, false)))
+     * 
+     * .whenReleased(() -> {
+     * this.shooter.disable();
+     * this.indexer.disable();
+     * });
+     */
 
-        new JoystickButton(driver, Button.kA.value)
+    new JoystickButton(driver, Button.kA.value)
         .whileHeld(
-          new ShootBallCommand(shooter, indexer)
-        )
+            new ShootBallCommand(shooter, indexer))
         .whenReleased(() -> {
           this.shooter.disable();
           this.indexer.disable();
         });
 
     new JoystickButton(driver, Button.kX.value)
-      .whenPressed(indexer::resetEncoder)
-      .whenPressed(indexer::enable)
-      .whenPressed(new SpinIndexerToAngleOld(indexer, 360, false));
+        .whenPressed(indexer::resetEncoder)
+        .whenPressed(indexer::enable)
+        .whenPressed(new SpinIndexerToAngleOld(indexer, 360, false));
 
     // TODO remove these or move them to operator controls
 

src/main/java/com/pigmice/frc/robot/subsystems/Indexer.java

@@ -25,7 +25,7 @@ public class Indexer extends SubsystemBase {
   private ShuffleboardTab indexerTab;
   private NetworkTableEntry enabledEntry;
   private NetworkTableEntry motorOutputEntry;
-  //private NetworkTableEntry encoderPositionEntry;
+  // private NetworkTableEntry encoderPositionEntry;
   private NetworkTableEntry rotateAngleEntry;
 
   private NetworkTableEntry rEntry;
@@ -46,7 +46,7 @@ public Indexer() {
     this.indexerTab = Shuffleboard.getTab("Indexer");
     this.enabledEntry = indexerTab.add("Enabled", enabled).getEntry();
     this.motorOutputEntry = indexerTab.add("Motor Output", 0).getEntry();
-    //this.encoderPositionEntry = indexerTab.add("Encoder Position", 0).getEntry();
+    // this.encoderPositionEntry = indexerTab.add("Encoder Position", 0).getEntry();
     this.rotateAngleEntry = indexerTab.add("Rotate Angle", 0).getEntry();
 
     this.rEntry = indexerTab.add("Color R", 0.0).getEntry();
@@ -86,13 +86,15 @@ public void periodic() {
     irEntry.setDouble(colorSensor.getIR());
     proximityEntry.setDouble(colorSensor.getProximity());
 
-    /*if (enabled) {
-      setMotorOutput(0.25);
-      encoderPositionEntry.setDouble(getEncoderPosition());
-    } else {
-      stopMotor();
-    }*/
-    //motor.set(ControlMode.PercentOutput, 0.5);
+    /*
+     * if (enabled) {
+     * setMotorOutput(0.25);
+     * encoderPositionEntry.setDouble(getEncoderPosition());
+     * } else {
+     * stopMotor();
+     * }
+     */
+    // motor.set(ControlMode.PercentOutput, 0.5);
   }
 
   public void setMotorOutput(double output) {