2022_03_10 vision shot and flywheel controller

Author: jlbabilino

.vscode/launch.json

@@ -4,6 +4,13 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
+    {
+      "type": "java",
+      "name": "Launch InterpolationTable",
+      "request": "launch",
+      "mainClass": "frc.lib.util.InterpolationTable",
+      "projectName": "RapidReact"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",

src/main/java/frc/lib/util/InterpolationTable.java

@@ -5,6 +5,15 @@
 
 public class InterpolationTable {
 
+    public static void main(String... args) {
+        InterpolationTable table = new InterpolationTable(new double[][]{
+            {1.7, 1800, 78.25},
+            {2.4, 1875, 73.4},
+            {2.95, 1980, 73.25}
+        });
+        System.out.println(Arrays.toString(table.sample(2)));
+    }
+
     private static final Comparator<double[]> INPUT_SORTER = (point1, point2) -> {return point1[0] > point2[0] ? 1 : -1;};
 
     private final double[][] referencePoints;
@@ -16,14 +25,22 @@ public InterpolationTable(double[][] referencePoints) {
         propertyCount = referencePoints[0].length - 1;
     }
 
-    public double[] sample(double input) throws IllegalArgumentException {
+    public double[] sample(double input) {
         int leftBoundIndex = 0;
         int rightBoundIndex = referencePoints.length - 1;
         if (input < referencePoints[leftBoundIndex][0]) {
-            throw new IllegalArgumentException();
+            double[] output = new double[propertyCount];
+            for (int i = 0; i < propertyCount; i++) {
+                output[i] = referencePoints[leftBoundIndex][1 + i];
+            }
+            return output;
         }
         if (input > referencePoints[rightBoundIndex][0]) {
-            throw new IllegalArgumentException();
+            double[] output = new double[propertyCount];
+            for (int i = 0; i < propertyCount; i++) {
+                output[i] = referencePoints[rightBoundIndex][1 + i];
+            }
+            return output;
         }
         boolean found = false;
         while (!found) {

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

@@ -17,6 +17,7 @@
 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.util.InterpolationTable;
 import frc.robot.Constants.ElectricalConstants;
 import frc.robot.Constants.ShooterConstants;
 
@@ -25,6 +26,12 @@ public class Shooter extends SubsystemBase {
   public static boolean UP = true;
   public static boolean DOWN = false;
 
+  public static final InterpolationTable INTERPOLATION_TABLE = new InterpolationTable(new double[][]{
+    {1.7, 1800, 78.25},
+    {2.4, 1875, 73.25},
+    {2.95, 1980, 73.25}
+  });
+
   private boolean hoodDirection;
   int highCurrentCount = 0;
   int lowCurrentCount = 0;
@@ -74,6 +81,8 @@ public Shooter() {
     hoodEncoder = hoodMotor.getEncoder();
     masterEncoder = shooterLeader.getEncoder();
 
+    shooterLeader.setOpenLoopRampRate(0.1);
+
     hoodEncoder.setPositionConversionFactor(ShooterConstants.kHoodGearingRatio);
 
     hoodController = hoodMotor.getPIDController();
@@ -173,7 +182,8 @@ public double getEncoderPosition() {
 
   public void setHoodPosition(double degrees) {
     // degrees = Math.min(Math.max(degrees, ShooterConstants.kHoodMinAngle), ShooterConstants.kHoodMaxAngle);
-    hoodController.setReference(degrees, ControlType.kPosition, 0, 0.0, ArbFFUnits.kPercentOut);
+    double reference = Math.max(60, Math.min(100, degrees));
+    hoodController.setReference(reference, ControlType.kPosition, 0, 0.0, ArbFFUnits.kPercentOut);
   }
 
   public void setShooterVelocity(double velocity) {
@@ -234,4 +244,21 @@ private void runTrigger() {
       triggerMotor.stopMotor();
     }
   }
+
+  public static class ShooterState {
+    public final double hoodAngle;
+    public final double velocity;
+    public ShooterState(double hoodAngle, double velocity) {
+      this.hoodAngle = hoodAngle;
+      this.velocity = velocity;
+    }
+  }
+
+  public static double lookupVelocity(double distance) {
+    return INTERPOLATION_TABLE.sample(distance)[0];
+  }
+
+  public static double lookupHoodAngle(double distance) {
+    return INTERPOLATION_TABLE.sample(distance)[1];
+  }
 }

src/main/java/frc/robot/shooter/ShooterTable.java

@@ -19,8 +19,8 @@ public class FlywheelController extends CommandBase {
     double rpm;
     private Timer timer = new Timer();
     Notifier controller = new Notifier(this::controller);
-    private DCMotor flywheel = new DCMotor(0.002081897, 0, 0.317466);
-    private PIDController flywheelController = new PIDController(0.02, 0, 0);
+    private DCMotor flywheel = new DCMotor(0.002081897, 0, 0.37);
+    private PIDController flywheelController = new PIDController(0.0175, 0, 0);
     // private PIDController flywheelController = new PIDController(0.05, 0, 0);
     boolean closeToTarget = false;
     double velocity = 0;
@@ -63,6 +63,8 @@ void controller() {
         double voltage = flywheel.solveFeedforward(rpm, 0) + flywheelController.calculate(velocity, dt);
         // double voltage = flywheel.solveFeedforward(rpm, 0);
 
+        voltage = Math.min(voltage, 11.5);
+
         shooter.setShooterVoltage(voltage);
 
         lastPosition = position;

src/main/java/frc/robot/shooter/commands/FlywheelController.java

@@ -17,8 +17,8 @@ public class VisionShooter extends CommandBase {
     double rpm;
     private Timer timer = new Timer();
     Notifier controller = new Notifier(this::controller);
-    private DCMotor flywheel = new DCMotor(0.002081897, 0, 0.317466);
-    private PIDController flywheelController = new PIDController(0.02, 0, 0);
+    private DCMotor flywheel = new DCMotor(0.002081897, 0, 0.302);
+    private PIDController flywheelController = new PIDController(0.0175, 0, 0);
     boolean closeToTarget = false;
     double velocity = 0;
 
@@ -53,9 +53,9 @@ void controller() {
         }
         velocity = (position - lastPosition) / (dt) * 60;
 
-        double distancePercent = (limelight.getState().distance - 1.7) / (2.9 - 1.7);
-        double rpm = distancePercent * (180) + 1800;
-        double hoodAngle = distancePercent * -5 + 78.25;
+        double distance = limelight.getState().distance;
+        double rpm = Shooter.lookupVelocity(distance);
+        double hoodAngle = Shooter.lookupHoodAngle(distance);
 
         shooter.setHoodPosition(hoodAngle);
         flywheelController.setReference(rpm);