From 627812e4fc0e72810757f656cccb3bec8800abde Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Wed, 16 Mar 2022 18:58:58 -0600
Subject: [PATCH] Added point filtering + added AaraVector2D

---
 src/main/java/frc4388/robot/Constants.java    |  2 +
 .../commands/ShooterCommands/TrackTarget.java | 18 ++++-
 src/main/java/frc4388/utility/Vector2D.java   | 67 +++++++++++++++++--
 3 files changed, 79 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 17d6f24..aa5b0c8 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -241,6 +241,8 @@ public final class Constants {
     public static final double LIME_VIXELS = 720;
     public static final double TURRET_kP = 0.1;
 
+    public static final double POINTS_THRESHOLD = 400;
+
     public static final double RANGE = 10;
 
     public static final double EDGE_TO_CENTER = 20;
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index b313fa8..bf524e6 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -19,6 +19,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.VisionOdometry;
+import frc4388.utility.Vector2D;
 import frc4388.utility.desmos.DesmosServer;
 
 public class TrackTarget extends CommandBase {
@@ -69,13 +70,24 @@ public class TrackTarget extends CommandBase {
     try {
       m_visionOdometry.setLEDs(true);
       points = m_visionOdometry.getTargetPoints();
+
+      Point average = VisionOdometry.averagePoint(points);
+
+      for(Point point : points) {
+        Vector2D difference = new Vector2D(point);
+        difference.subtract(new Vector2D(average));
+
+        if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD)
+          points.remove(point);
+      }
+
+      average = VisionOdometry.averagePoint(points);
+      DesmosServer.putPoint("average", average);
+
       for(int i = 0; i < points.size(); i++) {
         DesmosServer.putPoint("Point" + i, points.get(i));
       }
 
-      Point average = VisionOdometry.averagePoint(points);
-      DesmosServer.putPoint("average", average);
-
       output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
       output *= 4;
       // output *= 0.5;
diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java
index 8ce1968..cc04125 100644
--- a/src/main/java/frc4388/utility/Vector2D.java
+++ b/src/main/java/frc4388/utility/Vector2D.java
@@ -4,10 +4,16 @@
 
 package frc4388.utility;
 
+import java.security.InvalidParameterException;
+
+import org.opencv.core.Point;
+
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.drive.Vector2d;
 
-/** Aarav's good vector class (better than WPILib) */
+/** Aarav's good vector class (better than WPILib)
+ * @author Aarav Shah */
+
 public class Vector2D extends Vector2d {
 
   public double x;
@@ -15,8 +21,7 @@ public class Vector2D extends Vector2d {
   public double angle;
 
   public Vector2D() {
-    super();
-    this.angle = Math.atan2(this.y, this.x);
+    this(0, 0);
   }
 
   public Vector2D(double x, double y) {
@@ -27,6 +32,22 @@ public class Vector2D extends Vector2d {
     this.angle = Math.atan2(this.y, this.x);
   }
 
+  public Vector2D(double[] vec) {
+    this(vec[0], vec[1]);
+
+    if (vec.length != 2) {
+      throw new InvalidParameterException();
+    }
+  }
+
+  public Vector2D(Point p) {
+    this(p.x, p.y);
+  }
+
+  public Vector2D(Translation2d t) {
+    this(t.getX(), t.getY());
+  }
+
   /**
    * Add two vectors, component-wise.
    * @param v1 First vector in the addition.
@@ -37,6 +58,15 @@ public class Vector2D extends Vector2d {
     return new Vector2D(v1.x + v2.x, v1.y + v2.y);
   }
 
+  /**
+   * Adds vector to current object
+   * @param v Vector to add
+   */
+  public void add(Vector2D v) {
+    x += v.x;
+    y += v.x;
+  }
+
   /**
    * Subtract two vectors, component-wise.
    * @param v1 First vector in the subtraction.
@@ -47,6 +77,15 @@ public class Vector2D extends Vector2d {
     return new Vector2D(v1.x - v2.x, v1.y - v2.y);
   }
 
+  /**
+   * Subtracts vector from current object
+   * @param v Vector to subtract
+   */
+  public void subtract(Vector2D v) {
+    x -= v.x;
+    y -= v.x;
+  }
+
   /**
    * Multiply a vector with a scalar, component-wise.
    * @param v1 Vector to multiply.
@@ -57,6 +96,15 @@ public class Vector2D extends Vector2d {
     return new Vector2D(scalar * v1.x, scalar * v1.y);
   }
 
+  /**
+   * Multiply a vector with a scalar, component-wise.
+   * @param scalar Scalar to multiply
+   */
+  public void multiply(double scalar) {
+    x *= scalar;
+    y *= scalar;
+  }
+
     /**
    * Divide a vector with a scalar, component-wise.
    * @param v1 Vector to divide.
@@ -67,6 +115,15 @@ public class Vector2D extends Vector2d {
     return new Vector2D(v1.x / scalar, v1.y / scalar);
   }
 
+  /**
+   * Divide a vector with a scalar, component-wise.
+   * @param scalar Scalar to divide
+   */
+  public void divide(double scalar) {
+    x /= scalar;
+    y /= scalar;
+  }
+
   /**
    * Find unit vector.
    * @return The unit vector.
@@ -96,7 +153,7 @@ public class Vector2D extends Vector2d {
 
   @Override
   public String toString() {
-    return ("(" + this.x + ", " + this.y + ")");
+    return "<" + this.x + ", " + this.y + ">";
   }
 
-}
+}
\ No newline at end of file