Limelight

Robot2019/src/main/java/frc/robot/lib/Limelight.java

@@ -7,7 +7,6 @@
 
 package frc.robot.lib;
 
-import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -25,15 +24,48 @@ public enum Mode {
   */
   private double tv, tx, ty, ta;
   private double prev_tx = 1.0;
+
+  // Parameters for vision using linear algebra. 
+  private double[][] rotMat = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  private double[] translateVec = {0, 0, 0};
+  private double[] defaultValue = {0, 0, 0, 0};
+
+  /* Given what is currently seen, determine the entries rotMat and translateVec parameters
+    by solving a system of equations using Gaussian-elimination */
+  public void computeParams(double[] worldXs, double[] worldYs, double[] worldZs) {
+    double[] cornerXs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornx").getDoubleArray(defaultValue);
+    double[] cornerYs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcorny").getDoubleArray(defaultValue);
+    double[][] corners = {cornerXs, cornerYs, {1, 1, 1, 1}};
+
+    for (int i = 0; i < 3; i++) {
+      // Set up the matrix
+      double[][] matrix = new double[4][5];
+      for (int row = 0; row < 4; row++) {
+        matrix[row][0] = worldXs[row];
+        matrix[row][1] = worldYs[row];
+        matrix[row][2] = worldZs[row];
+        matrix[row][3] = 1;
+        matrix[row][4] = corners[i][row];
+      }
+
+      /* Row reduce and find solutions; assumed that echelon is of the form [I | x] 
+         where I is the identity matrix and x are the solutions. This has not been tested yet. */
+      double[][] echelon = Gaussian(matrix);
+      rotMat[i][0] = echelon[0][4];
+      rotMat[i][1] = echelon[1][4];
+      rotMat[i][2] = echelon[2][4];
+      translateVec[i] = echelon[3][4];
+    }
+  }
 
   // Adjusts the distance between a vision target and the robot. Uses basic PID with the ty value from the network table.
   public double distanceAssist() {
     tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0);
     ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0.0);
     SmartDashboard.putNumber("Crosshair Vertical Offset", ty);
     double adjustment = 0.0;
-    double area_threshold = 1.75;  // TODO: Set the desired area ratio. 0 to 100.
-    double Kp = 0.225;   // TODO: Set PID K value.
+    double area_threshold = 1.75;
+    double Kp = 0.225;
 
     if (tv == 1.0) {
       adjustment = (area_threshold - ta) * Kp;
@@ -52,7 +84,7 @@ public double steeringAssist() {
     SmartDashboard.putNumber("Prev_tx", prev_tx);
     double adjustment = 0.0;
     double steering_factor = 0.25;
-    double Kp = 0.025;   // TODO: Set PID K value.
+    double Kp = 0.025;
 
     if (tv == 1.0) {
       if (ta > 0.02) {
@@ -74,4 +106,121 @@ public double[] autoTarget() {
     double[] params = {dist_assist + steer_assist, dist_assist - steer_assist};
     return params;
   }
+
+  /* Given a desired straight-line distance targetDist away from the vision target, determine the distance 
+    in order to face the target from head-on. Returns the required distance at the current heading.
+  */
+  public double[] determineDist(double targetDist) {
+    // Get the x and y coordinates of the corners of the bounding box.
+    double[] cornerXs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornx").getDoubleArray(defaultValue);
+    double[] cornerYs = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcorny").getDoubleArray(defaultValue);
+
+    // Average the corners to get the center of the vision target as viewed by the camera.
+    double xSum = 0.0;
+    double ySum = 0.0;
+    for (int i = 0; i < 4; i += 1) {
+      xSum += cornerXs[i];
+      ySum += cornerYs[i];
+    }
+    double[] cameraPos = {xSum / 4, ySum / 4, 1};
+
+    // Calculate the position of the center in 3D space.
+    double[] worldPos = dot(transpose(rotMat), difference(cameraPos, translateVec));
+    double x = worldPos[0];
+    double y = worldPos[1];
+
+    // Use trigonometry to find the required distance.
+    double r = Math.sqrt(x * x + y * y);
+    double a1 = Math.atan2(y, x);
+    double a3 = Math.asin(r * Math.sin(a1) / targetDist);
+    double a2 = Math.PI - a1 - a3;
+    double[] params = {r * Math.sin(a2) / Math.sin(a3), a3};
+    return params;
+  }
+
+  // Returns the transpose of a matrix.
+  private double[][] transpose(double[][] matrix) {
+    int n = matrix.length;
+    int m = matrix[0].length;
+    double[][] matrixTranspose = new double[m][n];
+    for (int i = 0; i < n; i++) {
+      for (int j = 0; j < m; j++) {
+        matrixTranspose[m][n] = matrix[n][m];
+      }
+    }
+    return matrixTranspose;
+  }
+
+  // Returns matrix-vector product.
+  private double[] dot(double[][] matrix, double[] vector) {
+    int n = matrix.length;
+    int m = matrix[0].length;
+    assert m == vector.length;
+    double[] dotVector = new double[n];
+    for (int i = 0; i < n; i++) {
+      dotVector[i] = dot(matrix[i], vector);
+    }
+    return dotVector;
+  }
+
+  // Returns x * yT.
+  private double dot(double[] x, double[] y) {
+    int n = x.length;
+    assert x.length == y.length;
+    double dot = 0.0;
+    for (int i = 0; i < n; i++) { dot += x[i] * y[i]; }
+    return dot;
+  }
+
+  // Returns the difference of two vectors.
+  private double[] difference(double[] x, double[] y) {
+    double[] diff = new double[x.length];
+    for (int i = 0; i < x.length; i++) { diff[i] = x[i] - y[i]; }
+    return diff;
+  }
+
+  // Returns the reduced row-echelon form of a matrix.
+  private double[][] Gaussian(double[][] matrix) {
+    int n = matrix.length;
+    int m = matrix[0].length;
+    double[][] echelon = new double[n][];
+    for(int i = 0; i < matrix.length; i++) {
+      echelon[i] = matrix[i].clone();
+    }
+
+    while (!isEchelon(echelon)) {
+      for (int i = 0; i < Math.min(n, m); i++) {
+        double pivot = matrix[i][i];
+        if (pivot != 0) {
+          if (pivot != 1) {
+            for (int j = 0; j < m; j++) { echelon[i][j] /= pivot; }
+          }
+          for (int rowNum = 0; rowNum < n; rowNum++) {
+            for (int j = 0; j < m; j++) { echelon[rowNum][j] -= echelon[rowNum][i] * echelon[i][j]; }
+          }
+        }
+      }
+    }
+    return echelon;
+  }
+
+  // Tests whether a matrix is in reduced-row-echelon form
+  private boolean isEchelon(double[][] matrix) {
+    int n = matrix.length;
+    int m = matrix[0].length;
+    double[][] matrixTranspose = transpose(matrix);
+    for (int i = 0; i < n; i++) {
+      for (int j = 0; j < m; j++) {
+        double val = matrix[i][j];
+        if (Math.abs(val) > 0) {
+          int non_zero_count = 0;
+          for (int k = 0; k < n; k++) {
+            if (matrixTranspose[j][k] != 0) { non_zero_count += 1; }
+          }
+          if (non_zero_count != 1){ return false; }
+        }
+      }
+    }
+    return true;
+  }
 }