Finished refactor and added center correction

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

@@ -288,6 +288,8 @@ public final class Constants {
     public static final double LIME_VIXELS = 960;
     public static final double LIME_HIXELS = 720;
 
+    public static final double LIMELIGHT_RADIUS = 1.d;
+    public static final double SHOOTER_CORRECTION = 1.d;
   }
 
   public static final class OIConstants {

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

@@ -73,7 +73,7 @@ import frc4388.robot.subsystems.LED;
 // import frc4388.robot.subsystems.ShooterHood_1;
 // import frc4388.robot.subsystems.Storage;
 // import frc4388.robot.subsystems.Vision;
-import frc4388.robot.subsystems.VisionOdomotry;
+import frc4388.robot.subsystems.VisionOdometry;
 // import frc4388.robot.subsystems.Storage.StorageMode;
 import frc4388.utility.controller.ButtonFox;
 import frc4388.utility.controller.IHandController;
@@ -102,7 +102,7 @@ public class RobotContainer {
   private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
   private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
   // public final LimeLight m_robotLime = new LimeLight();
-  public final VisionOdomotry m_robotVision = new VisionOdomotry();
+  public final VisionOdometry m_robotVision = new VisionOdometry();
 
   /* Controllers */
   public boolean isGS = false;

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

@@ -6,4 +6,8 @@ public class VOPShooter extends SubsystemBase {
     public VOPShooter() {
         
     }
+
+    public double getShooterRotation() {
+        return 0;
+    }
 }

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

@@ -107,12 +107,14 @@ public class VisionOdometry extends SubsystemBase {
       guess = iterateGuess(guess, points);
     }
 
-    // TODO rotate guess for shooter & gyro
+    guess = correctGuessForCenter(guess, m_shooter.getShooterRotation());
+    guess = correctGuessForGyro(guess, m_drive.getRotation());
 
     SmartDashboard.putNumber("Vision ODO x: ", guess.x);
     SmartDashboard.putNumber("Vision ODO y: ", guess.y);
 
-    Pose2d odometryPose = new Pose2d(0, 0, new Rotation2d(0));
+    Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRotation()));
+    Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation);
 
     return odometryPose;
   }
@@ -212,6 +214,47 @@ public class VisionOdometry extends SubsystemBase {
     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 gyroRotation 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 += VOPConstants.LIMELIGHT_RADIUS;
+
+    double dist = Math.hypot(guess.x, guess.y);
+    double angle = Math.tan(corrected.y / corrected.x);
+    angle += shooterRotation;
+
+    corrected.x = dist * Math.cos(angle);
+    corrected.y = dist * Math.sin(angle);
+
+    corrected.y += VOPConstants.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 += 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
    * 

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

@@ -4,18 +4,34 @@ package frc4388.utility;
  * @author Daniel Thomas McGrath
  */
 public class VisionObscuredException extends RuntimeException {
+    /**
+     * Creates new VisionObscuredException with error text 'null'
+     */
     public VisionObscuredException() {
         super();
     }
 
+    /** 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(cause);
     }