From 54783f762afafc6afb26088ebf6bb720017aef09 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Feb 2022 20:27:30 -0700 Subject: [PATCH] Updated points and new photon vision stuffs --- src/main/java/frc4388/robot/Constants.java | 10 +- .../commands/drive/VisionUpdateOdometry.java | 129 ++++++++++++++---- .../frc4388/robot/subsystems/VOPDrive.java | 19 +++ .../frc4388/robot/subsystems/VOPShooter.java | 0 .../java/frc4388/robot/subsystems/Vision.java | 42 +++++- 5 files changed, 166 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/VOPDrive.java create mode 100644 src/main/java/frc4388/robot/subsystems/VOPShooter.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a40e9c9..3b7e827 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -279,7 +279,13 @@ public final class Constants { // for vision odo proto public static final class VOPConstants { - public static final double TARGET_HEIGHT = 1.d; + public static final double TARGET_HEIGHT = 8*12 + 8 - 28; + public static final double TARGET_RADIUS = 4*12; + public static final double H_FOV = 59.6; + public static final double V_FOV = 49.7; + public static final double LIME_VIXELS = 960; + public static final double LIME_HIXELS = 720; + } public static final class OIConstants { @@ -287,4 +293,4 @@ public final class Constants { public static final int XBOX_OPERATOR_ID = 1; public static final int BUTTON_FOX_ID = 2; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java index 602a56f..93e0a25 100644 --- a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java +++ b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java @@ -8,6 +8,7 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import org.ejml.simple.SimpleMatrix; import org.opencv.core.Mat; import org.opencv.core.Point; +import org.opencv.core.Point3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.geometry.Pose2d; @@ -59,52 +60,122 @@ public class VisionUpdateOdometry extends CommandBase { @Override public void initialize() { // Vision Processing Mode - m_limeLight.setLEDs(true); - // m_limeLight.changePipeline(0); + // m_limeLight.setLEDs(true); + // m_limeLight.changePipeline(5); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - ArrayList points = m_limeLight.getTargetPoints(); - if(points.size() < 5) - System.out.println("not enough points"); + m_limeLight.setLEDs(true); + m_limeLight.changePipeline(5); - double[] xPoints = new double[5]; - double[] yPoints = new double[5]; + ArrayList screenPoints = m_limeLight.getTargetPoints(); - for(int i = 0; i < 5; i++) { - xPoints[i] = points.get(i).x; - yPoints[i] = points.get(i).y; + // Debug power off + m_limeLight.setLEDs(false); + + if(screenPoints.size() < 3) { + System.err.println("Vision Update Odometry Error: Not enough points"); + m_limeLight.setLEDs(false); + return; } - double[] ellipseRadii = getEllipseRadii(xPoints, yPoints); + ArrayList points3d = get3dPoints(screenPoints); + ArrayList points = topView(points3d); - // https://www.desmos.com/calculator/qu0pe4rmiv - // https://math.stackexchange.com/questions/2388747/formula-for-ellipse-formed-from-projecting-a-tilted-circle-onto-the-xy-plane - // PI - acos((R_y / R_x)^2) - double viewAngle = Math.PI - Math.acos(Math.pow(ellipseRadii[1] / ellipseRadii[0], 2)); // TODO account for limelight angle of rotation + Point guess = averagePoint(points); - double distance = 1.d / Math.tan(viewAngle); - distance *= VOPConstants.TARGET_HEIGHT; // replace with VisionConstants for 2022 - System.out.println("Never gonna give you up: " + distance); + for(int i = 0; i < 30; i++) { + guess = iterateGuess(guess, points); + } - double[] ypr = new double[3]; - // Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022 - double relativeAngle = 0;//Math.toDegrees(/*m_shooterAim.getShooterAngleDegrees()*/ - ypr[0]); - rotation = new Rotation2d(0); + // TODO rotate guess for shooter & gyro - xPos = Math.cos(relativeAngle) * distance; - yPos = Math.sin(relativeAngle) * distance; - translate = new Translation2d(xPos, yPos); + SmartDashboard.putNumber("Vision ODO x: ", guess.x); + SmartDashboard.putNumber("Vision ODO y: ", guess.y); - Pose2d pose = new Pose2d(translate, rotation); - // m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter - SmartDashboard.putNumber("x:", pose.getX()); - SmartDashboard.putNumber("y:", pose.getY()); m_limeLight.setLEDs(false); } + public static ArrayList get3dPoints(ArrayList points2d) { + ArrayList points3d = new ArrayList<>(); + + for(Point point2d : points2d) { + double y_rot = point2d.y / VOPConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VOPConstants.V_FOV); + y_rot -= Math.toRadians(VOPConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double x_rot = point2d.x / VOPConstants.LIME_HIXELS; + x_rot *= Math.toRadians(VOPConstants.H_FOV); + x_rot -= Math.toRadians(VOPConstants.H_FOV) / 2; + + double z = VOPConstants.TARGET_HEIGHT / Math.tan(y_rot); + double x = z * Math.tan(x_rot); + double y = VOPConstants.TARGET_HEIGHT; + + points3d.add(new Point3(x, y, z)); + } + + return points3d; + } + + // Flattens 3d points from above + public static ArrayList topView(ArrayList points3d) { + ArrayList points = new ArrayList<>(); + + for(Point3 point3d : points3d) { + points.add(new Point(point3d.x, point3d.z)); + } + + return points; + } + + public static Point averagePoint(ArrayList points) { + Point average = new Point(0, 0); + for(Point point : points) { + average.x += point.x; + average.y += point.y; + } + + average.x /= points.size(); + average.y /= points.size(); + + return average; + } + + // Fits center of circle to projected points + public static Point iterateGuess(Point guess, ArrayList circlePoints) { + Point totalDiff = new Point(0, 0); + + for(Point circlePoint : circlePoints) { + double angle = Math.atan((guess.y - circlePoint.y) / (guess.x - circlePoint.x)); + angle = correctQuadrent(angle, guess, circlePoint); + + Point estimate = new Point(); + estimate.x = VOPConstants.TARGET_RADIUS * Math.cos(angle) + guess.x; + estimate.y = VOPConstants.TARGET_RADIUS * Math.sin(angle) + guess.y; + + Point diff = new Point(estimate.x - circlePoint.x, estimate.y - circlePoint.y); + totalDiff.x += diff.x; + totalDiff.y += diff.y; + } + + totalDiff.x /= circlePoints.size(); + totalDiff.y /= circlePoints.size(); + + return new Point(guess.x - totalDiff.x, guess.y - totalDiff.y); + } + + public static double correctQuadrent(double angle, Point guess, Point circlePoint) { + if(circlePoint.x - guess.x < 0) { + return angle - Math.PI; + } + + 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 diff --git a/src/main/java/frc4388/robot/subsystems/VOPDrive.java b/src/main/java/frc4388/robot/subsystems/VOPDrive.java new file mode 100644 index 0000000..fbd95f8 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/VOPDrive.java @@ -0,0 +1,19 @@ +package frc4388.robot.subsystems; + +import com.ctre.phoenix.sensors.PigeonIMU; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class VOPDrive extends SubsystemBase { + private PigeonIMU m_pigeon; + + public VOPDrive(PigeonIMU pigeon) { + m_pigeon = pigeon; + } + + public double getRotation() { + double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); + return Math.toRadians(ypr[0]); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/VOPShooter.java b/src/main/java/frc4388/robot/subsystems/VOPShooter.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index cd4f63e..997348b 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -11,7 +11,9 @@ import java.util.ArrayList; import org.opencv.core.Point; 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; public class Vision extends SubsystemBase { @@ -25,14 +27,48 @@ public class Vision extends SubsystemBase { ArrayList points = new ArrayList<>(); - double[] corners = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornxy").getDoubleArray(new double[0]); - for(int i = 0; i < corners.length; i += 2*2) { - points.add(new Point(corners[i], corners[i+1])); + 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]); + + double[] hSide = NetworkTableInstance.getDefault().getTable("limelight").getEntry("thor").getDoubleArray(new double[0]); + double[] vSide = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tvert").getDoubleArray(new double[0]); + + // 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); } 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);