From 86cc9b6ff598607bcdfceb7c8ac78adcc83f37a3 Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Sun, 13 Mar 2022 21:43:14 -0600
Subject: [PATCH] Speedy regression + circle fitting for target track

---
 .../frc4388/robot/commands/TrackTarget.java   | 19 ++++++++++
 .../robot/subsystems/VisionOdometry.java      | 35 +++++++++++--------
 2 files changed, 39 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java
index 8cb25c2..0acbfd6 100644
--- a/src/main/java/frc4388/robot/commands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/TrackTarget.java
@@ -39,6 +39,8 @@ public class TrackTarget extends CommandBase {
   Pose2d pos = new Pose2d();
   ArrayList<Point> points = new ArrayList<>();
 
+  double m=0;
+  double b=0;
   boolean isExecuted = false;
 
   // public static Gains m_aimGains;
@@ -88,6 +90,14 @@ public class TrackTarget extends CommandBase {
       double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot);
       DesmosServer.putDouble("distance", distance);
 
+      updateRegressionDesmos();
+      double regressedDistance = distanceRegression(distance);
+      DesmosServer.putDouble("distanceReg", regressedDistance);
+
+      //Vision odemetry circle fit based pose estimate
+      Point targetOffset = m_visionOdometry.getTargetOffset();
+      DesmosServer.putPoint("targetOff", targetOffset);
+
       // isExecuted = true;
     }
     catch (Exception e){
@@ -101,6 +111,15 @@ public class TrackTarget extends CommandBase {
     // m_turret.runshooterRotatePID(m_targetAngle);
   }
 
+  public final double distanceRegression(double distance) {
+    return m * distance + b;
+  }
+
+  public void updateRegressionDesmos() {
+    m = DesmosServer.readDouble("m");
+    b = DesmosServer.readDouble("b");
+  }
+
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
index 91c435c..f1048b0 100644
--- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
@@ -91,12 +91,7 @@ public class VisionOdometry extends SubsystemBase {
     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 {
+  public Point getTargetOffset() throws VisionObscuredException {
     ArrayList<Point> screenPoints = getTargetPoints();
 
     if(screenPoints.size() < 3)
@@ -111,14 +106,25 @@ public class VisionOdometry extends SubsystemBase {
       guess = iterateGuess(guess, points);
     }
 
-    guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees());
-    // guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
+    return guess;
+  }
 
-    SmartDashboard.putNumber("Vision ODO x: ", guess.x);
-    SmartDashboard.putNumber("Vision ODO y: ", guess.y);
+  /** Gets estimated odometry based on limelight data
+   * 
+   * @return The estimated odometry pose, including gyro rotation
+   * @throws VisionObscuredException
+   */
+  public Pose2d getVisionOdometry() throws VisionObscuredException {
+    Point targetOffset = getTargetOffset();
+
+    targetOffset = correctGuessForCenter(targetOffset, m_shooter.getBoomBoomAngleDegrees());
+    targetOffset = correctGuessForGyro(targetOffset, m_drive.getRegGyro().getDegrees());
+
+    SmartDashboard.putNumber("Vision ODO x: ", targetOffset.x);
+    SmartDashboard.putNumber("Vision ODO y: ", targetOffset.y);
 
     Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees()));
-    Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation);
+    Pose2d odometryPose = new Pose2d(targetOffset.x, targetOffset.y, rotation);
 
     return odometryPose;
   }
@@ -128,8 +134,7 @@ public class VisionOdometry extends SubsystemBase {
   }
 
   public boolean getLEDs() {
-    if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false;
-    return true;
+    return m_camera.getLEDMode() != VisionLEDMode.kOff;
   }
 
   public void updateOdometryWithVision(){
@@ -141,6 +146,7 @@ public class VisionOdometry extends SubsystemBase {
       err.printStackTrace();
     } 
   }
+
   /** Reverse 3d projects target points from screen coordinates to 3d space
    * <p>
    * Uses the known height of the target to project points
@@ -286,9 +292,8 @@ public class VisionOdometry extends SubsystemBase {
    * @return The angle corrected for the quadrent
    */
   public static final double correctQuadrent(double angle, Point guess, Point circlePoint) {
-    if(circlePoint.x - guess.x < 0) {
+    if(circlePoint.x - guess.x < 0)
       return angle - Math.PI;
-    }
 
     return angle;
   }