From 70f758805afd6f5a60e2d61a66c01ea34aa06636 Mon Sep 17 00:00:00 2001
From: Jess Zarchi <jess@roboticsisez.com>
Date: Sat, 1 Jun 2024 18:31:10 -0700
Subject: [PATCH] =?UTF-8?q?=E2=9C=A8Added=20imu=20scaling=20#104=20(#106)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 include/EZ-Template/drive/drive.hpp | 22 ++++++++++++++++++----
 src/EZ-Template/drive/drive.cpp     |  5 ++++-
 2 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
index 74593af1..204a3f96 100644
--- a/include/EZ-Template/drive/drive.hpp
+++ b/include/EZ-Template/drive/drive.hpp
@@ -641,7 +641,7 @@ class Drive {
   void drive_sensor_reset();
 
   /**
-   * Resets the current gyro value.  Defaults to 0, recommended to run at the start of your autonomous routine.
+   * Resets the current imu value.  Defaults to 0, recommended to run at the start of your autonomous routine.
    *
    * \param new_heading
    *        New heading value.
@@ -649,10 +649,23 @@ class Drive {
   void drive_imu_reset(double new_heading = 0);
 
   /**
-   * Returns the current gyro value.
+   * Returns the current imu value.
    */
   double drive_imu_get();
 
+  /**
+   * Sets a new imu scaling factor.  This value is multiplied by the imu to change its output.
+   *
+   * \param scaler
+   *        Factor to scale the imu by.
+   */
+  void drive_imu_scaler_set(double scaler);
+
+  /**
+   * Returns the current imu scaling factor.
+   */
+  double drive_imu_scaler_get();
+
   /**
    * Calibrates the IMU, recommended to run in initialize().
    *
@@ -834,12 +847,12 @@ class Drive {
   void pid_targets_reset();
 
   /**
-   * Sets heading of gyro and target of PID, okapi angle.
+   * Sets heading of imo and target of PID, okapi angle.
    */
   void drive_angle_set(okapi::QAngle p_angle);
 
   /**
-   * Sets heading of gyro and target of PID, takes double as an angle.
+   * Sets heading of imo and target of PID, takes double as an angle.
    */
   void drive_angle_set(double angle);
 
@@ -1294,6 +1307,7 @@ class Drive {
   double pid_tuner_increment_start_i_get();
 
  private:  // !Auton
+  double IMU_SCALER = 1.0;
   bool drive_toggle = true;
   bool print_toggle = true;
   int swing_min = 0;
diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp
index 53b4393f..ee7e658e 100644
--- a/src/EZ-Template/drive/drive.cpp
+++ b/src/EZ-Template/drive/drive.cpp
@@ -273,7 +273,10 @@ double Drive::drive_mA_left() { return left_motors.front().get_current_draw(); }
 bool Drive::drive_current_left_over() { return left_motors.front().is_over_current(); }
 
 void Drive::drive_imu_reset(double new_heading) { imu.set_rotation(new_heading); }
-double Drive::drive_imu_get() { return imu.get_rotation(); }
+double Drive::drive_imu_get() { return imu.get_rotation() * IMU_SCALER; }
+
+void Drive::drive_imu_scaler_set(double scaler) { IMU_SCALER = scaler; }
+double Drive::drive_imu_scaler_get() { return IMU_SCALER; }
 
 void Drive::drive_imu_display_loading(int iter) {
   // If the lcd is already initialized, don't run this function