mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 08:48:01 -06:00
i dunno i gotta switch branches ~ryan
This commit is contained in:
aarav18
@@ -279,7 +279,9 @@ public final class Constants {
|
||||
|
||||
// for vision odo proto
|
||||
public static final class VOPConstants {
|
||||
public static final double TARGET_HEIGHT = 8*12 + 8 - 28;
|
||||
public static final String NAME = "VOPCam";
|
||||
|
||||
public static final double TARGET_HEIGHT = 8*12 + 8 - (4*12 +7.5);
|
||||
public static final double TARGET_RADIUS = 4*12;
|
||||
public static final double H_FOV = 59.6;
|
||||
public static final double V_FOV = 49.7;
|
||||
|
||||
@@ -68,14 +68,14 @@ public class VisionUpdateOdometry extends CommandBase {
|
||||
@Override
|
||||
public void execute() {
|
||||
m_limeLight.setLEDs(true);
|
||||
m_limeLight.changePipeline(5);
|
||||
// m_limeLight.changePipeline(5);
|
||||
|
||||
ArrayList<Point> screenPoints = m_limeLight.getTargetPoints();
|
||||
|
||||
// Debug power off
|
||||
m_limeLight.setLEDs(false);
|
||||
|
||||
if(screenPoints.size() < 3) {
|
||||
if(!(screenPoints != null && screenPoints.size() >= 3)) {
|
||||
System.err.println("Vision Update Odometry Error: Not enough points");
|
||||
m_limeLight.setLEDs(false);
|
||||
return;
|
||||
@@ -176,117 +176,117 @@ public class VisionUpdateOdometry extends CommandBase {
|
||||
return angle;
|
||||
}
|
||||
|
||||
// http://www.lee-mac.com/5pointellipse.html
|
||||
// https://math.stackexchange.com/questions/163920/how-to-find-an-ellipse-given-five-points
|
||||
// https://towardsdatascience.com/understanding-singular-value-decomposition-and-its-application-in-data-science-388a54be95d
|
||||
// https://www.desmos.com/calculatoroe_points_determine_a_conic
|
||||
// // http://www.lee-mac.com/5pointellipse.html
|
||||
// // https://math.stackexchange.com/questions/163920/how-to-find-an-ellipse-given-five-points
|
||||
// // https://towardsdatascience.com/understanding-singular-value-decomposition-and-its-application-in-data-science-388a54be95d
|
||||
// // https://www.desmos.com/calculatoroe_points_determine_a_conic
|
||||
|
||||
/* solves the determinant of the following matrix
|
||||
* | x0^2 x0y0 y0^2 x0 y0 1 |
|
||||
* | x1^2 x1y1 y1^2 x1 y1 1 |
|
||||
* | x2^2 x2y2 y2^2 x2 y2 1 | = 0
|
||||
* | x3^2 x3y3 y3^2 x3 y3 1 |
|
||||
* | x4^2 x4y4 y4^2 x4 y4 1 |
|
||||
* | x5^2 x5y5 y5^2 x5 y5 1 |
|
||||
* for conic equation
|
||||
* ax^2 - bxy + cy^2 - dx + fy - g = 0
|
||||
*/
|
||||
public static double[] getEllipseRadii(double[] xPoints, double[] yPoints) {
|
||||
double[][] matrix = new double[6][5];
|
||||
// /* solves the determinant of the following matrix
|
||||
// * | x0^2 x0y0 y0^2 x0 y0 1 |
|
||||
// * | x1^2 x1y1 y1^2 x1 y1 1 |
|
||||
// * | x2^2 x2y2 y2^2 x2 y2 1 | = 0
|
||||
// * | x3^2 x3y3 y3^2 x3 y3 1 |
|
||||
// * | x4^2 x4y4 y4^2 x4 y4 1 |
|
||||
// * | x5^2 x5y5 y5^2 x5 y5 1 |
|
||||
// * for conic equation
|
||||
// * ax^2 - bxy + cy^2 - dx + fy - g = 0
|
||||
// */
|
||||
// public static double[] getEllipseRadii(double[] xPoints, double[] yPoints) {
|
||||
// double[][] matrix = new double[6][5];
|
||||
|
||||
// Generate matrix
|
||||
for(int i = 0; i < 5; i++) {
|
||||
matrix[i][0] = xPoints[i] * xPoints[i];
|
||||
matrix[i][1] = xPoints[i] * yPoints[i];
|
||||
matrix[i][2] = yPoints[i] * yPoints[i];
|
||||
matrix[i][3] = xPoints[i] * 1.d;
|
||||
matrix[i][4] = 1.d * yPoints[i];
|
||||
matrix[i][5] = 1.d;
|
||||
}
|
||||
// // Generate matrix
|
||||
// for(int i = 0; i < 5; i++) {
|
||||
// matrix[i][0] = xPoints[i] * xPoints[i];
|
||||
// matrix[i][1] = xPoints[i] * yPoints[i];
|
||||
// matrix[i][2] = yPoints[i] * yPoints[i];
|
||||
// matrix[i][3] = xPoints[i] * 1.d;
|
||||
// matrix[i][4] = 1.d * yPoints[i];
|
||||
// matrix[i][5] = 1.d;
|
||||
// }
|
||||
|
||||
double[] coeficients = new double[6];
|
||||
int pos = 1;
|
||||
for(int i = 0; i < 6; i++) {
|
||||
double[][] cofactor = cofactor(matrix, -1, i);
|
||||
coeficients[i] = pos * determinant(cofactor);
|
||||
// double[] coeficients = new double[6];
|
||||
// int pos = 1;
|
||||
// for(int i = 0; i < 6; i++) {
|
||||
// double[][] cofactor = cofactor(matrix, -1, i);
|
||||
// coeficients[i] = pos * determinant(cofactor);
|
||||
|
||||
pos *= -1;
|
||||
}
|
||||
// pos *= -1;
|
||||
// }
|
||||
|
||||
double[] radii = new double[2];
|
||||
// double[] radii = new double[2];
|
||||
|
||||
// https://math.stackexchange.com/questions/280937/finding-the-angle-of-rotation-of-an-ellipse-from-its-general-equation-and-the-ot
|
||||
double angle = Math.atan(coeficients[1] / (coeficients[0] - coeficients[2]));
|
||||
angle /= 2.d;
|
||||
// // https://math.stackexchange.com/questions/280937/finding-the-angle-of-rotation-of-an-ellipse-from-its-general-equation-and-the-ot
|
||||
// double angle = Math.atan(coeficients[1] / (coeficients[0] - coeficients[2]));
|
||||
// angle /= 2.d;
|
||||
|
||||
// A' = Acos^2(angle) + Bcos(angle)sin(angle) + Csin^2(angle)
|
||||
// B' = 0
|
||||
// C' = Asin^2(angle) - Bcos(angle)sin(angle) + Ccos^2(angle)
|
||||
// D' = Dcos(angle) + Esin(angle)
|
||||
// E' = -Dsin(angle) + Ecos(angle)
|
||||
// F' = F
|
||||
double A_prime = coeficients[0] * Math.pow(Math.cos(angle), 2) + coeficients[1] * Math.cos(angle) * Math.sin(angle) + coeficients[2] * Math.pow(Math.sin(angle), 2);
|
||||
double B_prime = 0;
|
||||
double C_prime = coeficients[0] * Math.pow(Math.sin(angle), 2) + coeficients[1] * Math.cos(angle) * Math.sin(angle) + coeficients[2] * Math.pow(Math.cos(angle), 2);
|
||||
double D_prime = coeficients[3] * Math.cos(angle) + coeficients[4] * Math.sin(angle);
|
||||
double E_prime = -coeficients[3] * Math.sin(angle) + coeficients[4] * Math.cos(angle);
|
||||
double F_prime = coeficients[5];
|
||||
// // A' = Acos^2(angle) + Bcos(angle)sin(angle) + Csin^2(angle)
|
||||
// // B' = 0
|
||||
// // C' = Asin^2(angle) - Bcos(angle)sin(angle) + Ccos^2(angle)
|
||||
// // D' = Dcos(angle) + Esin(angle)
|
||||
// // E' = -Dsin(angle) + Ecos(angle)
|
||||
// // F' = F
|
||||
// double A_prime = coeficients[0] * Math.pow(Math.cos(angle), 2) + coeficients[1] * Math.cos(angle) * Math.sin(angle) + coeficients[2] * Math.pow(Math.sin(angle), 2);
|
||||
// double B_prime = 0;
|
||||
// double C_prime = coeficients[0] * Math.pow(Math.sin(angle), 2) + coeficients[1] * Math.cos(angle) * Math.sin(angle) + coeficients[2] * Math.pow(Math.cos(angle), 2);
|
||||
// double D_prime = coeficients[3] * Math.cos(angle) + coeficients[4] * Math.sin(angle);
|
||||
// double E_prime = -coeficients[3] * Math.sin(angle) + coeficients[4] * Math.cos(angle);
|
||||
// double F_prime = coeficients[5];
|
||||
|
||||
// r1^2 = (-4F'A'C'+C'D'^2+A'E'^2) / (4A'^2C')
|
||||
radii[0] = -4 * F_prime * A_prime * C_prime + C_prime * Math.pow(D_prime, 2) + A_prime * Math.pow(E_prime, 2);
|
||||
radii[0] /= 4 * Math.pow(A_prime, 2) * C_prime;
|
||||
radii[0] = Math.sqrt(radii[0]);
|
||||
// r2^2 = (-4F'A'C'+C'D'^2+A'E'^2) / (4A'C'^2)
|
||||
radii[1] = -4 * F_prime * A_prime * C_prime + C_prime * Math.pow(D_prime, 2) + A_prime * Math.pow(E_prime, 2);
|
||||
radii[1] = 4 * A_prime * Math.pow(C_prime, 2);
|
||||
radii[1] = Math.sqrt(radii[1]);
|
||||
// // r1^2 = (-4F'A'C'+C'D'^2+A'E'^2) / (4A'^2C')
|
||||
// radii[0] = -4 * F_prime * A_prime * C_prime + C_prime * Math.pow(D_prime, 2) + A_prime * Math.pow(E_prime, 2);
|
||||
// radii[0] /= 4 * Math.pow(A_prime, 2) * C_prime;
|
||||
// radii[0] = Math.sqrt(radii[0]);
|
||||
// // r2^2 = (-4F'A'C'+C'D'^2+A'E'^2) / (4A'C'^2)
|
||||
// radii[1] = -4 * F_prime * A_prime * C_prime + C_prime * Math.pow(D_prime, 2) + A_prime * Math.pow(E_prime, 2);
|
||||
// radii[1] = 4 * A_prime * Math.pow(C_prime, 2);
|
||||
// radii[1] = Math.sqrt(radii[1]);
|
||||
|
||||
return radii;
|
||||
}
|
||||
// return radii;
|
||||
// }
|
||||
|
||||
public static double determinant(double[][] matrix) {
|
||||
if(matrix.length == 2) {
|
||||
return (matrix[0][0] * matrix[1][1]) - (matrix[0][1] * matrix[1][0]);
|
||||
} else {
|
||||
double sum = 0;
|
||||
int pos = 1;
|
||||
// public static double determinant(double[][] matrix) {
|
||||
// if(matrix.length == 2) {
|
||||
// return (matrix[0][0] * matrix[1][1]) - (matrix[0][1] * matrix[1][0]);
|
||||
// } else {
|
||||
// double sum = 0;
|
||||
// int pos = 1;
|
||||
|
||||
for(int i = 0; i < matrix.length; i++) {
|
||||
double[][] cofactor = cofactor(matrix, 0, i);
|
||||
sum += pos * determinant(cofactor);
|
||||
// for(int i = 0; i < matrix.length; i++) {
|
||||
// double[][] cofactor = cofactor(matrix, 0, i);
|
||||
// sum += pos * determinant(cofactor);
|
||||
|
||||
pos *= -1;
|
||||
}
|
||||
// pos *= -1;
|
||||
// }
|
||||
|
||||
return sum;
|
||||
}
|
||||
}
|
||||
// return sum;
|
||||
// }
|
||||
// }
|
||||
|
||||
public static double[][] cofactor(double[][] matrix, int row, int col) {
|
||||
double[][] cofactor = new double[matrix.length - 1][matrix.length - 1];
|
||||
// public static double[][] cofactor(double[][] matrix, int row, int col) {
|
||||
// double[][] cofactor = new double[matrix.length - 1][matrix.length - 1];
|
||||
|
||||
// comments mostly for decoration
|
||||
// // comments mostly for decoration
|
||||
|
||||
// row count without the excluded row
|
||||
int y = 0;
|
||||
for(int r = 0; r < matrix.length; r++) {
|
||||
// column count without the excluded column
|
||||
int x = 0;
|
||||
// doesn't add excluded row
|
||||
if(r != row) {
|
||||
for(int c = 0; c < matrix.length; c++) {
|
||||
// doesn't add excluded column
|
||||
if(c != col) {
|
||||
cofactor[y][x] = matrix[r][c];
|
||||
x++;
|
||||
}
|
||||
}
|
||||
y++;
|
||||
}
|
||||
}
|
||||
// // row count without the excluded row
|
||||
// int y = 0;
|
||||
// for(int r = 0; r < matrix.length; r++) {
|
||||
// // column count without the excluded column
|
||||
// int x = 0;
|
||||
// // doesn't add excluded row
|
||||
// if(r != row) {
|
||||
// for(int c = 0; c < matrix.length; c++) {
|
||||
// // doesn't add excluded column
|
||||
// if(c != col) {
|
||||
// cofactor[y][x] = matrix[r][c];
|
||||
// x++;
|
||||
// }
|
||||
// }
|
||||
// y++;
|
||||
// }
|
||||
// }
|
||||
|
||||
return cofactor;
|
||||
}
|
||||
// return cofactor;
|
||||
// }
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class VOPShooter extends SubsystemBase {
|
||||
public VOPShooter() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -7,70 +7,54 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.common.hardware.VisionLEDMode;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
import org.photonvision.targeting.TargetCorner;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.VOPConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
|
||||
public class Vision extends SubsystemBase {
|
||||
private PhotonCamera m_camera;
|
||||
// roborio ip & port: 10.43.88.2:1735
|
||||
|
||||
public Vision() {
|
||||
// TODO
|
||||
m_camera = new PhotonCamera(VOPConstants.NAME);
|
||||
}
|
||||
|
||||
public ArrayList<Point> getTargetPoints() {
|
||||
if(NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0) != 1)
|
||||
PhotonPipelineResult result = m_camera.getLatestResult();
|
||||
|
||||
if(!result.hasTargets())
|
||||
return null;
|
||||
|
||||
ArrayList<Point> points = new ArrayList<>();
|
||||
|
||||
double[] xCoord = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDoubleArray(new double[0]);
|
||||
double[] yCoord = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDoubleArray(new double[0]);
|
||||
double[] skew = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ts").getDoubleArray(new double[0]);
|
||||
for(PhotonTrackedTarget target : result.getTargets()) {
|
||||
List<TargetCorner> corners = target.getCorners();
|
||||
|
||||
double[] hSide = NetworkTableInstance.getDefault().getTable("limelight").getEntry("thor").getDoubleArray(new double[0]);
|
||||
double[] vSide = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tvert").getDoubleArray(new double[0]);
|
||||
double centerY = 0;
|
||||
for(TargetCorner corner : corners) {
|
||||
centerY += corner.y;
|
||||
}
|
||||
centerY /= corners.size();
|
||||
|
||||
// SmartDashboard.putNumberArray("Point array", corners);
|
||||
for(int i = 0; i < xCoord.length; i += 2*4) {
|
||||
Point center = new Point(xCoord[i], yCoord[i]);
|
||||
|
||||
center.x /= VOPConstants.H_FOV;
|
||||
center.x += .5;
|
||||
center.x *= VOPConstants.LIME_HIXELS;
|
||||
|
||||
center.y /= VOPConstants.V_FOV;
|
||||
center.y += .5;
|
||||
center.y *= VOPConstants.LIME_VIXELS;
|
||||
|
||||
double mag = Math.hypot(hSide[i]/2, vSide[i]/2);
|
||||
|
||||
Point tLeft = new Point(center.x - hSide[i]/2, center.y + vSide[i]/2);
|
||||
double lAngle = Math.atan(-(vSide[i]/2) / (hSide[i]/2));
|
||||
lAngle -= Math.toRadians(skew[i]);
|
||||
tLeft = new Point(mag * Math.cos(lAngle), mag * Math.sin(lAngle));
|
||||
|
||||
Point tRight = new Point(center.x + hSide[i]/2, center.y + vSide[i]/2);
|
||||
double rAngle = Math.atan((vSide[i]/2) / (hSide[i]/2));
|
||||
rAngle -= Math.toRadians(skew[i]);
|
||||
tRight = new Point(mag * Math.cos(rAngle), mag * Math.sin(rAngle));
|
||||
|
||||
points.add(tLeft);
|
||||
points.add(tRight);
|
||||
for(TargetCorner corner : corners) {
|
||||
if(corner.y <= centerY)
|
||||
points.add(new Point(corner.x, VOPConstants.LIME_VIXELS - corner.y));
|
||||
}
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
public void changePipeline(int pipelineId) {
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
|
||||
}
|
||||
|
||||
public void setLEDs(boolean on) {
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(on ? 0 : 1);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(on ? 3 : 1);
|
||||
m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user