diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3b7e827..85a1bb1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -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; diff --git a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java index 93e0a25..fe6c646 100644 --- a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java +++ b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java @@ -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 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 diff --git a/src/main/java/frc4388/robot/subsystems/VOPShooter.java b/src/main/java/frc4388/robot/subsystems/VOPShooter.java index e69de29..2dbb556 100644 --- a/src/main/java/frc4388/robot/subsystems/VOPShooter.java +++ b/src/main/java/frc4388/robot/subsystems/VOPShooter.java @@ -0,0 +1,9 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class VOPShooter extends SubsystemBase { + public VOPShooter() { + + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 997348b..2c67e37 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -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 getTargetPoints() { - if(NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0) != 1) + PhotonPipelineResult result = m_camera.getLatestResult(); + + if(!result.hasTargets()) return null; ArrayList 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 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); } } \ No newline at end of file