✨Added imu scaling #104 (#106)

include/EZ-Template/drive/drive.hpp

@@ -641,18 +641,31 @@ 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.
    */
   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;

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