From d7ff6f9192a7b74efe4bc02b01237a46e460c51d Mon Sep 17 00:00:00 2001
From: Hakop Zarikyan <hakopzarikyan@gmail.com>
Date: Mon, 6 Jan 2025 12:13:33 -0800
Subject: [PATCH] Adding get angle, code isn't finished. need to. finish
 get_angle. implement get_encoder_angle, and odom one too. Also need to figure
 out how to set priority. After meeting with Jess, will fine tune and review
 design choices.

---
 include/EZ-Template/drive/drive.hpp |  6 ++++++
 src/EZ-Template/drive/drive.cpp     | 17 ++++++++++++++++-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
index 67b5a07..78c656b 100644
--- a/include/EZ-Template/drive/drive.hpp
+++ b/include/EZ-Template/drive/drive.hpp
@@ -1415,6 +1415,12 @@ class Drive {
    * Get angle of the robot, depending on focused sensor
    */
   double get_angle();
+
+  /**
+   * Get angle with encoder
+   */
+  double get_encoder_angle();
+  
   /**
    * Practice mode for driver practice that shuts off the drive if you go max speed.
    *
diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp
index 0abedb0..79614d0 100644
--- a/src/EZ-Template/drive/drive.cpp
+++ b/src/EZ-Template/drive/drive.cpp
@@ -268,7 +268,22 @@ void Drive::drive_defaults_set() {
 
 double Drive::get_angle()
 {
-  return 0;
+  /*if there is a good imu*/
+  if(imu != nullptr) return drive_imu_get();
+
+  //this is odom tracking
+  if(is_tracker == ODOM_TRACKER) /**/;
+
+  //if width set, use ime
+  if(drive_width_get() != 0.0) /**/;
+  //
+
+  return INT_MAX;
+}
+
+double Drive::get_encoder_angle()
+{
+  //calcualte encoder angle  
 }
 double Drive::drive_tick_per_inch() {
   if (is_tracker == ODOM_TRACKER)