Merge branch 'full-robot-test' into Climber

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

@@ -260,11 +260,27 @@ 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 TURRET_kP = 0.1;
+
+    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

@@ -61,6 +61,7 @@ import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Claws.ClawType;
 import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.commands.Shoot;
+import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.Intake;
@@ -70,6 +71,7 @@ import frc4388.robot.subsystems.Storage;
 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;
@@ -102,9 +104,9 @@ public class RobotContainer {
   private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
   //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 Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
   private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
-  private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom);
+  private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
   
   
   /* Controllers */
@@ -139,8 +141,8 @@ public class RobotContainer {
             getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
     
     // Turret default command
-    // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret,
-    // m_robotSwerveDrive));
+    //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
+    m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret));
 
     //Swerve Drive
     m_robotSwerveDrive.setDefaultCommand(
@@ -242,6 +244,13 @@ public class RobotContainer {
     new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
         .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))
         .whenReleased(() -> m_robotStorage.runStorage(0.0));
+
+    //Shooter
+    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+        .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+        .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
   }
 
   /**

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

@@ -16,6 +16,7 @@ import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.CAN;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -38,16 +39,15 @@ import frc4388.robot.subsystems.SwerveModule;
 public class RobotMap {
 
   public RobotMap() {
-    // configureLEDMotorControllers();
+    configureLEDMotorControllers();
     configureSwerveMotorControllers();
     configureShooterMotorControllers();
   }
 
   /* LED Subsystem */
-  // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
-  void configureLEDMotorControllers() {
+  public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
-  }
+  void configureLEDMotorControllers() {}
 
   /* Swerve Subsystem */
   public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
@@ -179,8 +179,12 @@ 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);
 
+  // turret subsystem
   public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
 
+  // hood subsystem
+  public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+
   // Create motor CANSparkMax
   void configureShooterMotorControllers() {
 
@@ -221,6 +225,9 @@ public class RobotMap {
     // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull
     // numbers out of our ass anymore
 
+    // hood subsystem
+    angleAdjusterMotor.restoreFactoryDefaults();
+    angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
   }
 
        

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,15 @@ 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 (comment out to disable vision odo)
+    if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
+      m_visionOdometry.updateOdometryWithVision();
+      m_visionOdometry.setLEDs(true);
+    }
+    else{
+      m_visionOdometry.setLEDs(false);
+    }
   }
 
   public static double angleToCenter(double x, double y, double gyro) {

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

@@ -7,6 +7,7 @@ package frc4388.robot.commands;
 import edu.wpi.first.hal.simulation.SimulatorJNI;
 import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
 import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.subsystems.BoomBoom;
@@ -44,7 +45,8 @@ public class Shoot extends CommandBase {
   public double proportional, integral, derivative;
   public double time;
   public double output;
-  public double tolerance = 5.0;
+  public double tolerance;
+  public boolean isAimedInTolerance;
 
   // testing
   public DummySensor dummy = new DummySensor(0);
@@ -75,6 +77,9 @@ public class Shoot extends CommandBase {
     derivative = 0;
     time = 0.02;
 
+    tolerance = 5.0;
+    isAimedInTolerance = false;
+
     DummySensor.resetAll();
   }
 
@@ -83,6 +88,8 @@ public class Shoot extends CommandBase {
    */
   public void updateError() {
     error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
+    isAimedInTolerance = (Math.abs(error) <= tolerance);
+    SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
   }
 
   // Called when the command is initially scheduled.
@@ -154,7 +161,6 @@ public class Shoot extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    updateError();
-    return Math.abs(error) <= tolerance;
+    return false;
   }
 }

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

@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+
+import org.opencv.core.Point;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Hood;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.VisionOdometry;
+
+public class TrackTarget extends CommandBase {
+  /** Creates a new TrackTarget. */
+  Turret m_turret;
+  SwerveDrive m_drive;
+  VisionOdometry m_visionOdometry;
+  BoomBoom m_boomBoom;
+  Hood m_hood;
+
+  // use odometry to find x and y later
+  double x;
+  double y;
+  double distance;
+  double vel;
+  double hood;
+  double average;
+  double output;
+  Pose2d pos = new Pose2d();
+  ArrayList<Point> points = new ArrayList<>();
+
+  // public static Gains m_aimGains;
+
+  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_turret = turret;
+    m_drive = drive;
+    m_boomBoom = boomBoom;
+    m_hood = hood;
+    m_visionOdometry = visionOdometry;
+    addRequirements(m_turret, m_drive, m_visionOdometry);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    x = 0;
+    y = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
+    m_visionOdometry.setLEDs(true);
+    points = m_visionOdometry.getTargetPoints();
+    double pointTotal = 0;
+    for(Point point : points)
+    {
+      pointTotal = pointTotal + point.x;
+    }
+    average = pointTotal/points.size();
+    output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP;
+    m_turret.runTurretWithInput(output);
+    try{
+      pos = m_visionOdometry.getVisionOdometry();
+      distance = Math.hypot(pos.getX(), pos.getY());
+    }
+    catch (Exception e){
+    }
+    vel = m_boomBoom.getVelocity(distance);
+    hood = m_boomBoom.getHood(distance);
+    m_boomBoom.runDrumShooterVelocityPID(vel);
+    m_hood.runAngleAdjustPID(hood);
+    //m_turret.runshooterRotatePID(m_targetAngle);
+  }
+
+ /* public static double angleToCenter(double x, double y, double gyro) {
+    double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
+    return angle;
+  }*/
+
+  /**
+   * Checks if in hardware deadzone (due to mechanical limitations).
+   * @param angle Angle to check.
+   * @return True if in hardware deadzone.
+   */
+  public static boolean isHardwareDeadzone(double angle) {
+    return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
+  }
+
+  /**
+   * Checks if in digital deadzone (due to climber).
+   * @param angle Angle to check.
+   * @return True if in digital deadzone.
+   */
+  public static boolean isDigitalDeadzone(double angle) {
+    return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -24,13 +24,13 @@ import frc4388.utility.Gains;
 public class Hood extends SubsystemBase {
   public BoomBoom m_shooterSubsystem;
   
-  public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  public CANSparkMax m_angleAdjusterMotor;
   public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
   public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
-  public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
-  public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
+  public static Gains m_angleAdjusterGains;
+  public RelativeEncoder m_angleEncoder;
 
-  public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
+  public SparkMaxPIDController m_angleAdjusterPIDController;
   
   
   public boolean m_isHoodReady = false;
@@ -39,8 +39,12 @@ public double m_fireAngle;
   
 
   /** Creates a new Hood. */
-  public Hood() {
-     m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
+  public Hood(CANSparkMax angleAdjusterMotor) {
+
+    m_angleAdjusterMotor = angleAdjusterMotor;
+    m_angleEncoder = m_angleAdjusterMotor.getEncoder();
+    m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
+    m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
 
     m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
     m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);

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 {
 
@@ -241,20 +243,14 @@ public class SwerveDrive extends SubsystemBase {
    * Updates the field relative position of the robot.
    */
   public void updateOdometry() {
-    m_poseEstimator.update(getRegGyro(),
-        modules[0].getState(),
-        modules[1].getState(),
-        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);
+    m_poseEstimator.update( getRegGyro(),
+                            modules[0].getState(), 
+                            modules[1].getState(), 
+                            modules[2].getState(), 
+                            modules[3].getState());
   }
-
+  
+  
   /**
    * Resets pigeon.
    */

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

@@ -0,0 +1,293 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Timer;
+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;
+  }
+  public boolean getLEDs() {
+    if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false;
+    return true;
+  }
+
+  public void updateOdometryWithVision(){
+    try {
+      m_drive.m_poseEstimator.addVisionMeasurement(
+              getVisionOdometry(),
+              Timer.getFPGATimestamp() - getLatency());
+    } catch (VisionObscuredException err) {
+      err.printStackTrace();
+    } 
+  }
+  /** 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;
+  }
+}