Added get angle but did not implement it yet. Will do so after meetin…

…g with jess.

include/EZ-Template/drive/drive.hpp

@@ -1411,6 +1411,10 @@ class Drive {
    */
   void drive_imu_display_loading(int iter);
 
+  /**
+   * Get angle of the robot, depending on focused sensor
+   */
+  double get_angle();
   /**
    * Practice mode for driver practice that shuts off the drive if you go max speed.
    *

src/EZ-Template/drive/drive.cpp

@@ -266,6 +266,10 @@ void Drive::drive_defaults_set() {
   as::limit_switch_lcd_initialize(nullptr, nullptr);
 }
 
+double Drive::get_angle()
+{
+  return 0;
+}
 double Drive::drive_tick_per_inch() {
   if (is_tracker == ODOM_TRACKER)
     return odom_tracker_right->ticks_per_inch();