Shooter Tuning

src/main/deploy/Robot Data - Distances.csv

@@ -1,9 +1,13 @@
-Distance (in),Hood Ext. (u),Drum Velocity (u/ds)
-74,20,8000
-144,23,10000
-162,28,10000
-208,29,10000
-296,32,12500
-418,33,16000
-430,31,16000
-450,31,16000
+Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg)
+1,10,7000,2
+70,20,8000,2, double check
+100,24,9000,2
+145,28,10000,1,Add a 200
+230,31,12000,0,Add a 270
+320,32,17000,0,change 17000 and mark them lower
+331,33,17000,0
+397,33,16000,0
+415,33,16250,0
+436,31,18000,0
+500,33,18000,0
+501,33,18000,0

src/main/java/frc4388/robot/Constants.java

@@ -197,8 +197,8 @@ public final class Constants {
     
     public static final class VisionConstants {
         public static final double FOV = 29.8; //Field of view of limelight
-        public static final double TARGET_HEIGHT = 66.75;
-        public static final double LIME_ANGLE = 28.387;
+        public static final double TARGET_HEIGHT = 71.5;
+        public static final double LIME_ANGLE = 24.7;
         public static final double TURN_P_VALUE = 0.6;
         public static final double X_ANGLE_ERROR = 1.3;
         public static final double MOTOR_DEAD_ZONE = 0.3;

src/main/java/frc4388/robot/commands/shooter/TrackTarget.java

@@ -68,8 +68,14 @@ public class TrackTarget extends CommandBase {
     xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
     yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
 
+    // Finding Distance
+    distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
+    realDistance = (1.09 * distance) - 12.8;
+
+
     if (target == 1.0) { // If target in view
       // Aiming Left/Right
+      xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance);
       turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
       if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
         turnAmount = 0;
@@ -82,12 +88,8 @@ public class TrackTarget extends CommandBase {
       }
       m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
 
-      // Finding Distance
-      distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
-      realDistance = (1.13 * distance) - 14.3;
       SmartDashboard.putNumber("Distance to Target", realDistance);
-      SmartDashboard.putNumber("ts Skew or Rotation", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ts").getDouble(0));
-      SmartDashboard.putNumber("ta Area", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0));
+      SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance));
         //START Equation Code
       double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT);
       double xVel = (distance * VisionConstants.GRAV) / (yVel);

src/main/java/frc4388/utility/ShooterTables.java

@@ -20,12 +20,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  * Add your docs here.
  */
 public class ShooterTables {
-    double[][] m_distance = new double[50][3];
+    double[][] m_distance = new double[50][4];
     double[][] m_angle = new double[50][2];
 
     final int m_columnDistance = 0;
     final int m_columnHood = 1;
     final int m_columnVel = 2;
+    final int m_columnCenterDisplacement = 3;
     final int m_columnAngle = 0;
     final int m_columnDisplacement = 1;
 
@@ -55,6 +56,7 @@ public class ShooterTables {
                     m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]);
                     m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]);
                     m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]);
+                    m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]);
                     
                     lineNum ++;
                 }
@@ -123,6 +125,23 @@ public class ShooterTables {
         }
     }
 
+    public double getCenterDisplacement(double distance) { //Degrees of Lime FOV
+        int i = 0;
+        while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) {
+            i ++;
+        }
+        if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) {
+            return m_distance[i][m_columnCenterDisplacement];
+        } else {
+            if (i >= m_distanceLength) {
+                i = m_distanceLength - 1;
+            }
+            return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance);
+        }
+    }
+
+    
+
     public double getAngleDisplacement(double angle) {
         int i = 0;
         while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {