Adding get angle, code isn't finished. need to. finish get_angle. imp…

…lement 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

@@ -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.
    *

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)