shooter distances

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

@@ -23,6 +23,7 @@
 import com.pigmice.frc.robot.subsystems.Indexer;
 import com.pigmice.frc.robot.subsystems.Intake;
 import com.pigmice.frc.robot.subsystems.Shooter;
+import com.pigmice.frc.robot.subsystems.Subsystem;
 import com.pigmice.frc.robot.subsystems.climber.Lifty;
 import com.pigmice.frc.robot.subsystems.climber.Rotato;
 import com.pigmice.frc.robot.testmode.Testable;
@@ -122,7 +123,7 @@ public RobotContainer() {
 	 * it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
 	 */
 	private void configureButtonBindings(XboxController driver, XboxController operator) {
-
+		
 		// DRIVER CONTROLS
 
 		new JoystickButton(driver, Button.kY.value)

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

@@ -0,0 +1,43 @@
+package com.pigmice.frc.robot;
+
+public class ShooterDistances {
+    private static int[] topSpeeds = {};
+    private static int[] bottomSpeeds = {};
+    private static int[] distances = new int[31]; { // distances in inches
+    for(int a = 0; a < 31; a++) {
+        distances[a] = a*6;
+    }}
+
+    private static int find(int[] a, double i) {
+        int k = (int) i;
+        for(int j = 0; j < a.length; j++) {
+            if(a[j] == k) {return j;}
+        }
+        return -1;
+    }
+
+    public static double getTopSpeed(double distance) {
+        if(distance % 6 == 0) {
+            return topSpeeds[find(distances, distance)];
+        } else {
+            double remainder = distance % 6;
+            int lowerBound = topSpeeds[find(distances, distance - remainder)];
+            int upperBound = topSpeeds[find(distances, distance + 6 - remainder)];
+            return ((lowerBound * (6 - remainder)) + (upperBound * remainder)) / 6;
+        }
+    }
+
+    public static double getBottomSpeed(double distance) {
+        if(distance % 6 == 0) {
+            return bottomSpeeds[find(distances, distance)];
+        } else {
+            double remainder = distance % 6;
+            int lowerBound = bottomSpeeds[find(distances, distance - remainder)];
+            int upperBound = bottomSpeeds[find(distances, distance + 6 - remainder)];
+            return ((lowerBound * (6 - remainder)) + (upperBound * remainder)) / 6;
+        }
+    }
+
+    public static int[] listTopSpeeds() {return topSpeeds;}
+    public static int[] listBottomSpeeds() {return bottomSpeeds;}
+}

src/main/java/com/pigmice/frc/robot/commands/ShootBallCommand.java

@@ -8,9 +8,7 @@
 import com.pigmice.frc.robot.subsystems.Indexer;
 import com.pigmice.frc.robot.subsystems.Shooter;
 
-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.*;
 
 public class ShootBallCommand extends SequentialCommandGroup {
   public ShootBallCommand(Shooter shooter, Indexer indexer) {

src/main/java/com/pigmice/frc/robot/commands/shooter/StartShooterCommand.java

@@ -21,10 +21,6 @@ public void initialize() {
         this.shooter.setMode(mode);
     }
 
-    @Override
-    public void execute() {
-    }
-
     @Override
     public boolean isFinished() {
         return this.shooter.isAtTargetVelocity();

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

@@ -1,5 +1,7 @@
 package com.pigmice.frc.robot.subsystems;
 
+import com.pigmice.frc.robot.ShooterDistances;
+import com.pigmice.frc.robot.Vision;
 import com.pigmice.frc.robot.Constants.ShooterConfig;
 import com.pigmice.frc.robot.Constants.ShooterConfig.ShooterMode;
 import com.revrobotics.CANSparkMax;
@@ -25,6 +27,7 @@ public class Shooter extends Subsystem {
 	private final RelativeEncoder topEncoder, botEncoder;
 
 	public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM, maxVel, minVel, maxAcc, allowedErr;
+	private double hubDistance;
 
 	private final ShuffleboardTab shooterTab;
 
@@ -128,6 +131,12 @@ public void periodic() {
 		double topRPM = this.mode.getTopRPM();
 		double botRPM = this.mode.getBottomRPM();
 
+		if(this.mode == ShooterMode.AUTO){
+			hubDistance = Vision.getDistanceFromTarget(Vision.getBestTarget());
+			topRPM = ShooterDistances.getTopSpeed(hubDistance);
+			botRPM = ShooterDistances.getBottomSpeed(hubDistance);
+		}
+
 		if (this.mode == ShooterMode.SHUFFLEBOARD || this.isTestMode()) {
 			topRPM = topRPMEntry.getDouble(topRPM);
 			botRPM = bottomRPMEntry.getDouble(botRPM);