From 06ccba971ef3f4398946d823d5f9387537f1362b Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sat, 12 Mar 2022 14:35:47 -0700
Subject: [PATCH] vector2d angle

---
 src/main/java/frc4388/robot/RobotContainer.java | 1 +
 src/main/java/frc4388/utility/Vector2D.java     | 4 ++++
 2 files changed, 5 insertions(+)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index fb53f70..09051b9 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -79,6 +79,7 @@ import frc4388.robot.subsystems.VisionOdometry;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.ListeningSendableChooser;
 import frc4388.utility.PathPlannerUtil;
+import frc4388.utility.Vector2D;
 import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.DeadbandedXboxController;
 
diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java
index 0e194e4..7255d66 100644
--- a/src/main/java/frc4388/utility/Vector2D.java
+++ b/src/main/java/frc4388/utility/Vector2D.java
@@ -4,6 +4,7 @@
 
 package frc4388.utility;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.drive.Vector2d;
 
 /** Aarav's good vector class (better than WPILib) */
@@ -11,9 +12,11 @@ public class Vector2D extends Vector2d {
 
   public double x;
   public double y;
+  public double angle;
 
   public Vector2D() {
     super();
+    this.angle = Math.atan2(this.y, this.x);
   }
 
   public Vector2D(double x, double y) {
@@ -21,6 +24,7 @@ public class Vector2D extends Vector2d {
 
     this.x = x;
     this.y = y;
+    this.angle = Math.atan2(this.y, this.x);
   }
 
   public static Vector2D add(Vector2D v1, Vector2D v2) {