Updated vision code for 2022

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

@@ -175,11 +175,25 @@ public final class Constants {
 
   }
   public static final class VisionConstants {
-    public static final double TURN_P_VALUE = 0.8;
-    public static final double X_ANGLE_ERROR = 0.5;
-    public static final double GRAV = 385.83;
-    public static final double TARGET_HEIGHT = 67.5;
-    public static final double FOV = 29.8; //Field of view limelight
+    // public static final double TURN_P_VALUE = 0.8;
+    // public static final double X_ANGLE_ERROR = 0.5;
+    // public static final double GRAV = 385.83;
+    // public static final double TARGET_HEIGHT = 67.5;
+    // public static final double FOV = 29.8; //Field of view limelight
     public static final double LIME_ANGLE = 24.7;
+
+    public static final String NAME = "photonCamera";
+
+    public static final double TARGET_HEIGHT = 8*12 + 8; // Convert to metric
+    public static final double TARGET_RADIUS = 4*12; // Convert to metric
+    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 double RANGE = 10;
+
+    public static final double LIMELIGHT_RADIUS = 1.d;
+    public static final double SHOOTER_CORRECTION = 1.d;
   }
 }

src/main/java/frc4388/robot/RobotContainer.java

@@ -64,6 +64,7 @@ import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
+import frc4388.robot.subsystems.VisionOdometry;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.ListeningSendableChooser;
 import frc4388.utility.PathPlannerUtil;
@@ -88,8 +89,10 @@ public class RobotContainer {
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
   private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
   private final Hood m_robotHood = new Hood();
-  // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
+  private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
+  private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
   // private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
+
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -119,7 +122,7 @@ public class RobotContainer {
 
     //Turret default command
 
-    // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
+    m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
 
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(

src/main/java/frc4388/robot/RobotMap.java

@@ -136,7 +136,7 @@ public class RobotMap {
   public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
   public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
 
-  // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
+  public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
 
   // Create motor CANSparkMax
   void configureShooterMotorControllers() {

src/main/java/frc4388/robot/commands/AimToCenter.java

@@ -6,13 +6,16 @@ package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.VisionOdometry;
 
 public class AimToCenter extends CommandBase {
   /** Creates a new AimWithOdometry. */
   Turret m_turret;
   SwerveDrive m_drive;
+  VisionOdometry m_visionOdometry;
 
   // use odometry to find x and y later
   double x;
@@ -21,11 +24,12 @@ public class AimToCenter extends CommandBase {
 
   // public static Gains m_aimGains;
 
-  public AimToCenter(Turret turret, SwerveDrive drive) {
+  public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_turret = turret;
     m_drive = drive;
-    addRequirements(m_turret, m_drive);
+    m_visionOdometry = visionOdometry;
+    addRequirements(m_turret, m_drive, m_visionOdometry);
   }
 
   // Called when the command is initially scheduled.
@@ -40,6 +44,9 @@ public class AimToCenter extends CommandBase {
   public void execute() {
     m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
     m_turret.runshooterRotatePID(m_targetAngle);
+
+    // Check if limelight is within range
+    m_visionOdometry.setLEDs(Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE);
   }
 
   public static double angleToCenter(double x, double y, double gyro) {

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -17,12 +17,14 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.VisionObscuredException;
 
 public class SwerveDrive extends SubsystemBase {
 
@@ -51,6 +53,7 @@ public class SwerveDrive extends SubsystemBase {
   below are robot specific, and should be tuned. */
   public SwerveDrivePoseEstimator m_poseEstimator;
   public SwerveDriveOdometry m_odometry;
+  public VisionOdometry m_visionOdometry;
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
@@ -227,11 +230,14 @@ public class SwerveDrive extends SubsystemBase {
                             modules[2].getState(), 
                             modules[3].getState());
   
-      // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
-      // a real robot, this must be calculated based either on latency or timestamps.
-      // m_poseEstimator.addVisionMeasurement(
-      //         m_poseEstimator.getEstimatedPosition(),
-      //     Timer.getFPGATimestamp() - 0.1);
+      // Also apply vision measurements if the camera can get vision
+      try {
+        m_poseEstimator.addVisionMeasurement(
+                m_visionOdometry.getVisionOdometry(),
+                Timer.getFPGATimestamp() - m_visionOdometry.getLatency());
+      } catch (VisionObscuredException err) {
+        err.printStackTrace();
+      }
     }
   
   /**

src/main/java/frc4388/robot/subsystems/VisionOdometry.java

@@ -0,0 +1,279 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.subsystems;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.opencv.core.Point;
+import org.opencv.core.Point3;
+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.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.utility.VisionObscuredException;
+
+/** Represents the limelight and odometry related functionality
+ * @author Daniel McGrath
+ */
+public class VisionOdometry extends SubsystemBase {
+  // roborio ip & port: 10.43.88.2:1735
+  private PhotonCamera m_camera;
+
+  private SwerveDrive m_drive;
+  private Turret m_shooter;
+
+  private double latency = 0;
+
+  /** Creates a new VisionOdometry
+   * 
+   * @param drive The swerve drive subsystem
+   * @param shooter The turret subsystem
+   */
+  public VisionOdometry(SwerveDrive drive, Turret shooter) {
+    m_camera = new PhotonCamera(VisionConstants.NAME);
+    m_drive = drive;
+    m_shooter = shooter;
+  }
+
+  /** Gets the vision points from the limelight
+   * <p>
+   * Breaks down targets into 4 corners and uses the top 2 points
+   * 
+   * @return Vision points on the rim of the target in screen space
+   */
+  public ArrayList<Point> getTargetPoints() {
+    PhotonPipelineResult result = m_camera.getLatestResult();
+    latency = result.getLatencyMillis();
+  
+    if(!result.hasTargets())
+      return new ArrayList<Point>();
+    
+    ArrayList<Point> points = new ArrayList<>();
+
+    for(PhotonTrackedTarget target : result.getTargets()) {
+      List<TargetCorner> corners = target.getCorners();
+
+      double centerY = 0;
+      for(TargetCorner corner : corners) {
+        centerY += corner.y;
+      }
+      centerY /= corners.size();
+
+      for(TargetCorner corner : corners) {
+        if(corner.y <= centerY)
+          points.add(new Point(corner.x, VisionConstants.LIME_VIXELS - corner.y));
+      }
+    }
+
+    return points;
+  }
+
+  /** Sets LEDs on or off (duh)
+   * 
+   * @param on LED state
+   */
+  public void setLEDs(boolean on) {
+    m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
+
+  /** Gets estimated odometry based on limelight data
+   * 
+   * @return The estimated odometry pose, including gyro rotation
+   * @throws VisionObscuredException
+   */
+  public Pose2d getVisionOdometry() throws VisionObscuredException {
+    ArrayList<Point> screenPoints = getTargetPoints();
+
+    if(screenPoints.size() < 3)
+      throw new VisionObscuredException("Not enough vision points available");
+
+    ArrayList<Point3> points3d = get3dPoints(screenPoints);
+    ArrayList<Point> points = topView(points3d);
+
+    Point guess = averagePoint(points);
+
+    for(int i = 0; i < 30; i++) {
+      guess = iterateGuess(guess, points);
+    }
+
+    guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees());
+    guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
+
+    SmartDashboard.putNumber("Vision ODO x: ", guess.x);
+    SmartDashboard.putNumber("Vision ODO y: ", guess.y);
+
+    Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees()));
+    Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation);
+
+    return odometryPose;
+  }
+
+  public double getLatency() {
+    return latency;
+  }
+
+  /** Reverse 3d projects target points from screen coordinates to 3d space
+   * <p>
+   * Uses the known height of the target to project points
+   * 
+   * @param points2d Vision points on the rim of the target in screen space
+   * @return Reverse 3d projected points    
+   */
+  public static final ArrayList<Point3> get3dPoints(ArrayList<Point> points2d) {
+    ArrayList<Point3> points3d = new ArrayList<>();
+
+    for(Point point2d : points2d) {
+      double y_rot = point2d.y / VisionConstants.LIME_VIXELS;
+      y_rot *= Math.toRadians(VisionConstants.V_FOV);
+      y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2;
+      y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
+
+      double x_rot = point2d.x / VisionConstants.LIME_HIXELS;
+      x_rot *= Math.toRadians(VisionConstants.H_FOV);
+      x_rot -= Math.toRadians(VisionConstants.H_FOV) / 2;
+
+      double z = VisionConstants.TARGET_HEIGHT / Math.tan(y_rot);
+      double x = z * Math.tan(x_rot);
+      double y = VisionConstants.TARGET_HEIGHT;
+
+      points3d.add(new Point3(x, y, z));
+    }
+
+    return points3d;
+  }
+
+  /** Flattens 3d points from above
+   * 
+   * @param points3d 3d points along the target rim
+   * @return An array of flattened 3d points
+   */
+  public static final ArrayList<Point> topView(ArrayList<Point3> points3d) {
+    ArrayList<Point> points = new ArrayList<>();
+
+    for(Point3 point3d : points3d) {
+      points.add(new Point(point3d.x, point3d.z));
+    }
+
+    return points;
+  }
+
+  /** Finds the average point from an array of points
+   * 
+   * @param points The points the average will be taken from
+   * @return The average point
+   */
+  public static final Point averagePoint(ArrayList<Point> 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;
+  }
+
+  /** Iterates the current guess for the vision center (relative to the limelight)
+   * based on points on the rim of the target
+   * <p>
+   * The guess is iterated by finding the current average vector error between the guess
+   * and the circlePoints, assuming that the guess should be a constant radius from each point
+   * 
+   * @param guess The current estimate for the vision center
+   * @param circlePoints Vision points along the rim of the target
+   * @return The guess after iteration
+   */
+  public static final Point iterateGuess(Point guess, ArrayList<Point> 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 = VisionConstants.TARGET_RADIUS * Math.cos(angle) + guess.x;
+      estimate.y = VisionConstants.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);
+  }
+
+  /** Corrects odometry guess for shooter angle
+   * 
+   * @param guess The current guess for the vision center
+   * @param shooterRotation The rotation to correct for
+   * @return The corrected odometry point
+   */
+  public static final Point correctGuessForCenter(Point guess, double shooterRotation) {
+    Point corrected = new Point(guess.x, guess.y);
+    corrected.y += VisionConstants.LIMELIGHT_RADIUS;
+
+    double dist = Math.hypot(guess.x, guess.y);
+    double angle = Math.tan(corrected.y / corrected.x);
+    angle += Math.toRadians(shooterRotation);
+
+    corrected.x = dist * Math.cos(angle);
+    corrected.y = dist * Math.sin(angle);
+
+    corrected.y += VisionConstants.SHOOTER_CORRECTION;
+    
+    return corrected;
+  }
+
+  /** Corrects odometry guess for gyro angle
+   * 
+   * @param guess The current guess for the vision center
+   * @param gyroRotation The rotation to correct for
+   * @return The corrected odometry point
+   */
+  public static final Point correctGuessForGyro(Point guess, double gyroRotation) {
+    Point corrected = new Point(guess.x, guess.y);
+
+    double dist = Math.hypot(guess.x, guess.y);
+    double angle = Math.tan(corrected.y / corrected.x);
+    angle += Math.toRadians(gyroRotation);
+
+    corrected.x = dist * Math.cos(angle);
+    corrected.y = dist * Math.sin(angle);
+    
+    return corrected;
+  }
+
+  /** Corrects the angle from the current center estimate to a point on the target rim
+   * for multiple quadrents
+   * 
+   * @param angle The angle to be corrected
+   * @param guess The current guess for the vision center
+   * @param circlePoint A point along the target rim
+   * @return The angle corrected for the quadrent
+   */
+  public static final double correctQuadrent(double angle, Point guess, Point circlePoint) {
+    if(circlePoint.x - guess.x < 0) {
+      return angle - Math.PI;
+    }
+
+    return angle;
+  }
+}

src/main/java/frc4388/utility/VisionObscuredException.java

@@ -0,0 +1,38 @@
+package frc4388.utility;
+
+/** Exception that occurs if the limelight can't see enough points
+ * @author Daniel Thomas McGrath
+ */
+public class VisionObscuredException extends Exception {
+    /**
+     * Creates new VisionObscuredException with error text 'null'
+     */
+    public VisionObscuredException() {
+        super("Unable to see sufficient vision points");
+    }
+
+    /** Creates new VisionObscuredException with error text message
+     * 
+     * @param message Error text message
+     */
+    public VisionObscuredException(String message) {
+        super(message);
+    }
+
+    /** Creates new VisionObscuredException with error text message and detailed stack trace
+     * 
+     * @param message Error text message
+     * @param cause Root cause of error
+     */
+    public VisionObscuredException(String message, Throwable cause) {
+        super(message, cause);
+    }
+
+    /** Creates new VisionObscuredException with error text 'null' and detailed stack trace
+     * 
+     * @param cause Root cause of error
+     */
+    public VisionObscuredException(Throwable cause) {
+        super("Unable to see sufficient vision points", cause);
+    }
+}

vendordeps/photonlib.json

@@ -0,0 +1,41 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2022.1.5",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-cpp",
+            "version": "v2022.1.5",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxx86-64"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-java",
+            "version": "v2022.1.5"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonTargeting-java",
+            "version": "v2022.1.5"
+        }
+    ]
+}