diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md
index a482260d..07fe968f 100644
--- a/.github/ISSUE_TEMPLATE/bug_report.md
+++ b/.github/ISSUE_TEMPLATE/bug_report.md
@@ -1,7 +1,7 @@
 ---
 name: Bug Report
 about: Create a report to help us improve
-title: "\U0001F41B[BUG] - "
+title: "\U0001F41B - "
 labels: bug
 assignees: ''
 ---
diff --git a/.github/ISSUE_TEMPLATE/feature-request-template.md b/.github/ISSUE_TEMPLATE/feature-request-template.md
index 41cb2877..96411760 100644
--- a/.github/ISSUE_TEMPLATE/feature-request-template.md
+++ b/.github/ISSUE_TEMPLATE/feature-request-template.md
@@ -1,7 +1,7 @@
 ---
 name: Feature Request
 about: Suggest an idea for this project
-title: "✨[FEATURE] - "
+title: "✨ - "
 labels: enhancement
 assignees: ''
 ---
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index b22f6c71..4aa26461 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -17,10 +17,10 @@ jobs:
 
       - name: Run LemLib/pros-build
         id: test
-        uses: LemLib/pros-build@v2.0.1
+        uses: LemLib/pros-build@v3.0.0
 
       - name: Upload Artifact
         uses: actions/upload-artifact@v4
         with:
           name: ${{ steps.test.outputs.name }}
-          path: ${{ github.workspace }}/template/*
+          path: ${{ github.workspace }}/template/*
\ No newline at end of file
diff --git a/EZ-Template-Example-Project.zip b/EZ-Template-Example-Project.zip
index 42b2a129..60707b6c 100644
Binary files a/EZ-Template-Example-Project.zip and b/EZ-Template-Example-Project.zip differ
diff --git a/EZ-Template-Example-Project/EZ-Template@3.2.1.zip b/EZ-Template-Example-Project/EZ-Template@3.2.1.zip
deleted file mode 100644
index e9e26513..00000000
Binary files a/EZ-Template-Example-Project/EZ-Template@3.2.1.zip and /dev/null differ
diff --git a/EZ-Template-Example-Project/EZ-Template@3.2.2.zip b/EZ-Template-Example-Project/EZ-Template@3.2.2.zip
new file mode 100644
index 00000000..bf022369
Binary files /dev/null and b/EZ-Template-Example-Project/EZ-Template@3.2.2.zip differ
diff --git a/EZ-Template-Example-Project/Makefile b/EZ-Template-Example-Project/Makefile
index c9c38269..4e183dad 100644
--- a/EZ-Template-Example-Project/Makefile
+++ b/EZ-Template-Example-Project/Makefile
@@ -14,7 +14,7 @@ INCDIR=$(ROOT)/include
 
 WARNFLAGS+=
 EXTRA_CFLAGS=
-EXTRA_CXXFLAGS=
+EXTRA_CXXFLAGS=-Wno-deprecated-enum-enum-conversion
 
 # Set to 1 to enable hot/cold linking
 USE_PACKAGE:=1
diff --git a/EZ-Template-Example-Project/common.mk b/EZ-Template-Example-Project/common.mk
index 33efe871..a601bcaf 100644
--- a/EZ-Template-Example-Project/common.mk
+++ b/EZ-Template-Example-Project/common.mk
@@ -34,7 +34,7 @@ LIBRARIES+=$(wildcard $(FWDIR)/*.a)
 EXCLUDE_COLD_LIBRARIES+=$(FWDIR)/libc.a $(FWDIR)/libm.a
 COLD_LIBRARIES=$(filter-out $(EXCLUDE_COLD_LIBRARIES), $(LIBRARIES))
 wlprefix=-Wl,$(subst $(SPACE),$(COMMA),$1)
-LNK_FLAGS=--gc-sections --start-group $(strip $(LIBRARIES)) -lgcc -lstdc++ --end-group -T$(FWDIR)/v5-common.ld
+LNK_FLAGS=--gc-sections --start-group $(strip $(LIBRARIES)) -lgcc -lstdc++ --end-group -T$(FWDIR)/v5-common.ld --no-warn-rwx-segments
 
 ASMFLAGS=$(MFLAGS) $(WARNFLAGS)
 CFLAGS=$(MFLAGS) $(CPPFLAGS) $(WARNFLAGS) $(GCCFLAGS) --std=$(C_STANDARD)
diff --git a/EZ-Template-Example-Project/firmware/EZ-Template.a b/EZ-Template-Example-Project/firmware/EZ-Template.a
index fb9526d3..06a1a2b1 100644
Binary files a/EZ-Template-Example-Project/firmware/EZ-Template.a and b/EZ-Template-Example-Project/firmware/EZ-Template.a differ
diff --git a/EZ-Template-Example-Project/project.pros b/EZ-Template-Example-Project/project.pros
index d670ed35..e68a81e1 100644
--- a/EZ-Template-Example-Project/project.pros
+++ b/EZ-Template-Example-Project/project.pros
@@ -5,7 +5,7 @@
         "target": "v5",
         "templates": {
             "EZ-Template": {
-                "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\EZ-Template@3.2.1",
+                "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\EZ-Template@3.2.2",
                 "metadata": {
                     "origin": "local"
                 },
@@ -13,21 +13,21 @@
                 "py/object": "pros.conductor.templates.local_template.LocalTemplate",
                 "supported_kernels": "^4.1.1",
                 "system_files": [
-                    "include\\EZ-Template\\slew.hpp",
-                    "include\\EZ-Template\\piston.hpp",
-                    "include\\EZ-Template\\api.hpp",
-                    "include\\EZ-Template\\auton_selector.hpp",
                     "include\\EZ-Template\\sdcard.hpp",
-                    "include\\EZ-Template\\tracking_wheel.hpp",
+                    "include\\EZ-Template\\api.hpp",
+                    "include\\EZ-Template\\piston.hpp",
+                    "include\\EZ-Template\\util.hpp",
                     "include\\EZ-Template\\auton.hpp",
+                    "include\\EZ-Template\\slew.hpp",
                     "include\\EZ-Template\\PID.hpp",
-                    "include\\EZ-Template\\util.hpp",
+                    "include\\EZ-Template\\tracking_wheel.hpp",
+                    "firmware\\EZ-Template.a",
                     "include\\EZ-Template\\drive\\drive.hpp",
-                    "firmware\\EZ-Template.a"
+                    "include\\EZ-Template\\auton_selector.hpp"
                 ],
                 "target": "v5",
                 "user_files": [],
-                "version": "3.2.1"
+                "version": "3.2.2"
             },
             "kernel": {
                 "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\kernel@4.1.1",
diff --git a/EZ-Template-Example-Project/src/main.cpp b/EZ-Template-Example-Project/src/main.cpp
index ce7bc638..79d745c4 100644
--- a/EZ-Template-Example-Project/src/main.cpp
+++ b/EZ-Template-Example-Project/src/main.cpp
@@ -15,12 +15,13 @@ ez::Drive chassis(
     4.125,  // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
     343);   // Wheel RPM = cartridge * (motor gear / wheel gear)
 
-// Are you using tracking wheels?  Comment out which ones you're using here!
-//  `2.75` is the wheel diameter
-//  `4.0` is the distance from the center of the wheel to the center of the robot
-// ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0);  // ADI Encoders
-// ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0);  // ADI Encoders plugged into a Smart port
-// ez::tracking_wheel horiz_tracker(1, 2.75, 4.0);             // Rotation sensors
+// Uncomment the trackers you're using here!
+// - `8` and `9` are smart ports (making these negative will reverse the sensor)
+//  - you should get positive values on the encoders going FORWARD and RIGHT
+// - `2.75` is the wheel diameter
+// - `4.0` is the distance from the center of the wheel to the center of the robot
+// ez::tracking_wheel horiz_tracker(8, 2.75, 4.0);  // This tracking wheel is perpendicular to the drive wheels
+// ez::tracking_wheel vert_tracker(9, 2.75, 4.0);   // This tracking wheel is parallel to the drive wheels
 
 /**
  * Runs initialization code. This occurs as soon as the program is started.
@@ -34,10 +35,14 @@ void initialize() {
 
   pros::delay(500);  // Stop the user from doing anything while legacy ports configure
 
-  // Are you using tracking wheels?  Comment out which ones you're using here!
-  // chassis.odom_tracker_right_set(&right_tracker);
-  // chassis.odom_tracker_left_set(&left_tracker);
-  // chassis.odom_tracker_back_set(&horiz_tracker);  // Replace `back` to `front` if your tracker is in the front!
+  // Look at your horizontal tracking wheel and decide if it's in front of the midline of your robot or behind it
+  //  - change `back` to `front` if the tracking wheel is in front of the midline
+  //  - ignore this if you aren't using a horizontal tracker
+  // chassis.odom_tracker_back_set(&horiz_tracker);
+  // Look at your vertical tracking wheel and decide if it's to the left or right of the center of the robot
+  //  - change `left` to `right` if the tracking wheel is to the right of the centerline
+  //  - ignore this if you aren't using a vertical tracker
+  // chassis.odom_tracker_left_set(&vert_tracker);
 
   // Configure your chassis controls
   chassis.opcontrol_curve_buttons_toggle(true);   // Enables modifying the controller curve with buttons on the joysticks
@@ -66,7 +71,7 @@ void initialize() {
       {"Pure Pursuit Wait Until\n\nGo to (24, 24) but start running an intake once the robot passes (12, 24)", odom_pure_pursuit_wait_until_example},
       {"Boomerang\n\nGo to (0, 24, 45) then come back to (0, 0, 0)", odom_boomerang_example},
       {"Boomerang Pure Pursuit\n\nGo to (0, 24, 45) on the way to (24, 24) then come back to (0, 0, 0)", odom_boomerang_injected_pure_pursuit_example},
-      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", odom_boomerang_injected_pure_pursuit_example},
+      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", measure_offsets},
   });
 
   // Initialize chassis and auton selector
diff --git a/EZ-Template@3.2.1.zip b/EZ-Template@3.2.1.zip
deleted file mode 100644
index e9e26513..00000000
Binary files a/EZ-Template@3.2.1.zip and /dev/null differ
diff --git a/EZ-Template@3.2.2.zip b/EZ-Template@3.2.2.zip
new file mode 100644
index 00000000..bf022369
Binary files /dev/null and b/EZ-Template@3.2.2.zip differ
diff --git a/Makefile b/Makefile
index 66c1449f..5b318ece 100644
--- a/Makefile
+++ b/Makefile
@@ -14,7 +14,7 @@ INCDIR=$(ROOT)/include
 
 WARNFLAGS+=
 EXTRA_CFLAGS=
-EXTRA_CXXFLAGS=
+EXTRA_CXXFLAGS=-Wno-deprecated-enum-enum-conversion
 
 # Set to 1 to enable hot/cold linking
 USE_PACKAGE:=1
@@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:=
 IS_LIBRARY:=1
 # TODO: CHANGE THIS!
 LIBNAME:=EZ-Template
-VERSION:=3.2.1
+VERSION:=3.2.2
 # EXCLUDE_SRC_FROM_LIB= $(SRCDIR)/unpublishedfile.c
 # this line excludes opcontrol.c and similar files
 EXCLUDE_SRC_FROM_LIB+=$(foreach file, $(SRCDIR)/autons $(SRCDIR)/main,$(foreach cext,$(CEXTS),$(file).$(cext)) $(foreach cxxext,$(CXXEXTS),$(file).$(cxxext)))
diff --git a/common.mk b/common.mk
index 33efe871..a601bcaf 100644
--- a/common.mk
+++ b/common.mk
@@ -34,7 +34,7 @@ LIBRARIES+=$(wildcard $(FWDIR)/*.a)
 EXCLUDE_COLD_LIBRARIES+=$(FWDIR)/libc.a $(FWDIR)/libm.a
 COLD_LIBRARIES=$(filter-out $(EXCLUDE_COLD_LIBRARIES), $(LIBRARIES))
 wlprefix=-Wl,$(subst $(SPACE),$(COMMA),$1)
-LNK_FLAGS=--gc-sections --start-group $(strip $(LIBRARIES)) -lgcc -lstdc++ --end-group -T$(FWDIR)/v5-common.ld
+LNK_FLAGS=--gc-sections --start-group $(strip $(LIBRARIES)) -lgcc -lstdc++ --end-group -T$(FWDIR)/v5-common.ld --no-warn-rwx-segments
 
 ASMFLAGS=$(MFLAGS) $(WARNFLAGS)
 CFLAGS=$(MFLAGS) $(CPPFLAGS) $(WARNFLAGS) $(GCCFLAGS) --std=$(C_STANDARD)
diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
index c64494a0..0e35bcfc 100644
--- a/include/EZ-Template/drive/drive.hpp
+++ b/include/EZ-Template/drive/drive.hpp
@@ -6,8 +6,10 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 #pragma once
 
+#include <deque>
 #include <functional>
 #include <iostream>
+#include <stack>
 #include <tuple>
 
 #include "EZ-Template/PID.hpp"
@@ -63,9 +65,14 @@ class Drive {
   std::vector<int> pto_active;
 
   /**
-   * Inertial sensor.
+   * Current focused Inertial sensor.
    */
-  pros::Imu imu;
+  pros::Imu* imu;
+
+  /**
+   * All good imus, for redundancy.
+   */
+  std::deque<pros::Imu*> good_imus;
 
   /**
    * Deprecated left tracking wheel.
@@ -470,6 +477,27 @@ class Drive {
    */
   Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ratio, int left_rotation_port, int right_rotation_port) __attribute__((deprecated("Use the integrated encoder constructor with odom_tracker_left_set() and odom_tracker_right_set() instead!")));
 
+  /**
+   * Creates a Drive Controller using internal encoders with redundant IMUs.
+   *
+   * \param left_motor_ports
+   *        input {1, -2...}. make ports negative if reversed
+   * \param right_motor_ports
+   *        input {-3, 4...}. make ports negative if reversed
+   * \param imu_port
+   *        input {5, 6...}. multiple IMU ports
+   * \param wheel_diameter
+   *        diameter of your drive wheels
+   * \param ticks
+   *        motor cartridge RPM
+   * \param ratio
+   *        external gear ratio, wheel gear / motor gear
+   */
+  Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, std::vector<int> imu_ports, double wheel_diameter, double ticks, double ratio = 1.0);
+
+  // Deconstructor
+  ~Drive();
+
   /**
    * Sets drive defaults.
    */
@@ -1348,6 +1376,21 @@ class Drive {
    */
   double drive_imu_scaler_get();
 
+  /*
+   * Sets a new IMU scaling factor for all IMUs.
+   *
+   * This value is multiplied by the imu to change its output.
+   *
+   * \param scales
+   *        input {0.99, 1.01...}
+   */
+  void drive_imus_scalers_set(std::vector<double> scales);
+
+  /*
+   * Returns the scaling factor for all IMUs.
+   */
+  std::map<int, double> drive_imus_scalers_get();
+
   /**
    * Calibrates the IMU, recommended to run in initialize().
    *
@@ -1368,6 +1411,11 @@ class Drive {
    */
   void drive_imu_display_loading(int iter);
 
+  /**
+   * Get angle of the robot, depending on focused sensor.
+   */
+  double drive_angle_get();
+
   /**
    * Practice mode for driver practice that shuts off the drive if you go max speed.
    *
@@ -3411,6 +3459,8 @@ class Drive {
   bool opcontrol_arcade_scaling_enabled();
 
  private:
+  std::map<int, double> imu_scale_map = {};
+  std::map<int, std::pair<double, int>> prev_imu_values = {};
   void opcontrol_drive_activebrake_targets_set();
   double odom_smooth_weight_smooth = 0.0;
   double odom_smooth_weight_data = 0.0;
@@ -3423,6 +3473,7 @@ class Drive {
   std::vector<const_and_name>* used_pid_tuner_pids;
   double opcontrol_speed_max = 127.0;
   bool arcade_vector_scaling = false;
+  double prev_imu_value = 0;
   // odom privates
   std::vector<odom> pp_movements;
   std::vector<int> injected_pp_index;
@@ -3513,8 +3564,6 @@ class Drive {
   double used_motion_chain_scale = 0.0;
   bool motion_chain_backward = false;
 
-  double IMU_SCALER = 1.0;
-
   bool drive_toggle = true;
   bool print_toggle = true;
   int swing_min = 0;
@@ -3542,6 +3591,11 @@ class Drive {
   std::string complete_pid_tuner_output = "";
   float p_increment = 0.1, i_increment = 0.001, d_increment = 0.25, start_i_increment = 1.0;
 
+  /**
+   * @brief
+   * Get the scaled imu value from given imu
+   */
+  double get_this_imu(pros::Imu* imu);
   /**
    * Private wait until for drive
    */
@@ -3594,6 +3648,7 @@ class Drive {
   void ptp_task();
   void boomerang_task();
   void pp_task();
+  void check_imu_task();
 
   /**
    * Starting value for left/right
diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp
index 0c659d47..3c86b4b7 100644
--- a/src/EZ-Template/drive/drive.cpp
+++ b/src/EZ-Template/drive/drive.cpp
@@ -16,7 +16,7 @@ using namespace ez;
 // Constructor for integrated encoders
 Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
              int imu_port, double wheel_diameter, double ticks, double ratio)
-    : imu(imu_port),
+    : imu(new pros::Imu(imu_port)),
       left_tracker(-1, -1, false),   // Default value
       right_tracker(-1, -1, false),  // Default value
       left_rotation(-1),
@@ -36,6 +36,51 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
     right_motors.push_back(temp);
   }
 
+  good_imus.push_back(imu);
+  drive_imu_scaler_set(1);
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = drive_tick_per_inch();
+
+  drive_defaults_set();
+}
+
+// Constructor for integrated encoders with redundant imu support
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             std::vector<int> imu_ports, double wheel_diameter, double ticks, double ratio)
+    : imu(new pros::Imu(imu_ports[0])),
+      left_tracker(-1, -1, false),   // Default value
+      right_tracker(-1, -1, false),  // Default value
+      left_rotation(-1),
+      right_rotation(-1),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_INTEGRATED;
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp((std::int8_t)abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp((std::int8_t)abs(i));
+    temp.set_reversed(util::reversed_active(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set all IMUs
+  std::vector<double> imu_scale_values = {};
+  good_imus.push_back(imu);
+  imu_scale_values.push_back(1);
+  for (int i = 1; i < imu_ports.size(); i++) {
+    pros::Imu* temp = new pros::Imu(imu_ports[i]);
+    good_imus.push_back(temp);
+    imu_scale_values.push_back(1);
+  }
+  drive_imus_scalers_set(imu_scale_values);
+
   // Set constants for tick_per_inch calculation
   WHEEL_DIAMETER = wheel_diameter;
   RATIO = ratio;
@@ -49,7 +94,7 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
 Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
              int imu_port, double wheel_diameter, double ticks, double ratio,
              std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports)
-    : imu(imu_port),
+    : imu(new pros::Imu(imu_port)),
       left_tracker(abs(left_tracker_ports[0]), abs(left_tracker_ports[1]), util::reversed_active(left_tracker_ports[0])),
       right_tracker(abs(right_tracker_ports[0]), abs(right_tracker_ports[1]), util::reversed_active(right_tracker_ports[0])),
       left_rotation(-1),
@@ -69,6 +114,8 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
     right_motors.push_back(temp);
   }
 
+  good_imus.push_back(imu);
+  drive_imu_scaler_set(1);
   // Set constants for tick_per_inch calculation
   WHEEL_DIAMETER = wheel_diameter;
   RATIO = ratio;
@@ -82,7 +129,7 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
 Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
              int imu_port, double wheel_diameter, double ticks, double ratio,
              std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports, int expander_smart_port)
-    : imu(imu_port),
+    : imu(new pros::Imu(imu_port)),
       left_tracker({expander_smart_port, abs(left_tracker_ports[0]), abs(left_tracker_ports[1])}, util::reversed_active(left_tracker_ports[0])),
       right_tracker({expander_smart_port, abs(right_tracker_ports[0]), abs(right_tracker_ports[1])}, util::reversed_active(right_tracker_ports[0])),
       left_rotation(-1),
@@ -102,6 +149,8 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
     right_motors.push_back(temp);
   }
 
+  good_imus.push_back(imu);
+  drive_imu_scaler_set(1);
   // Set constants for tick_per_inch calculation
   WHEEL_DIAMETER = wheel_diameter;
   RATIO = ratio;
@@ -115,7 +164,7 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
 Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
              int imu_port, double wheel_diameter, double ratio,
              int left_rotation_port, int right_rotation_port)
-    : imu(imu_port),
+    : imu(new pros::Imu(imu_port)),
       left_tracker(-1, -1, false),   // Default value
       right_tracker(-1, -1, false),  // Default value
       left_rotation(abs(left_rotation_port)),
@@ -137,6 +186,8 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
     right_motors.push_back(temp);
   }
 
+  good_imus.push_back(imu);
+  drive_imu_scaler_set(1);
   // Set constants for tick_per_inch calculation
   WHEEL_DIAMETER = wheel_diameter;
   RATIO = ratio;
@@ -146,8 +197,21 @@ Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
   drive_defaults_set();
 }
 
+Drive::~Drive() {
+  delete imu;
+  good_imus.pop_front();
+
+  while (!good_imus.empty()) {
+    delete good_imus.front();
+    good_imus.pop_front();
+  }
+}
+
+// set defaults
 void Drive::drive_defaults_set() {
-  imu.set_data_rate(5);
+  for (int i = 0; i < good_imus.size(); i++) {
+    imu->set_data_rate(5);
+  }
 
   std::cout << std::fixed;
   std::cout << std::setprecision(2);
@@ -204,6 +268,13 @@ void Drive::drive_defaults_set() {
   as::limit_switch_lcd_initialize(nullptr, nullptr);
 }
 
+double Drive::drive_angle_get() {
+  /*if there is a good imu*/
+  if (imu != nullptr) return drive_imu_get();
+
+  return INT_MAX;
+}
+
 double Drive::drive_tick_per_inch() {
   if (is_tracker == ODOM_TRACKER)
     return odom_tracker_right->ticks_per_inch();
@@ -330,15 +401,26 @@ 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);
+  for (int i = 0; i < good_imus.size(); i++) {
+    good_imus[i]->set_rotation(new_heading);
+  }
   angle_rad = util::to_rad(new_heading);
   t_last = angle_rad;
 }
-double Drive::drive_imu_get() { return imu.get_rotation() * IMU_SCALER; }
-double Drive::drive_imu_accel_get() { return imu.get_accel().x + imu.get_accel().y; }
+double Drive::get_this_imu(pros::Imu* imu) { return imu->get_rotation() * imu_scale_map[imu->get_port()]; }
 
-void Drive::drive_imu_scaler_set(double scaler) { IMU_SCALER = scaler; }
-double Drive::drive_imu_scaler_get() { return IMU_SCALER; }
+double Drive::drive_imu_get() { return get_this_imu(imu); }
+double Drive::drive_imu_accel_get() { return imu->get_accel().x + imu->get_accel().y; }
+
+void Drive::drive_imu_scaler_set(double scaler) { imu_scale_map[imu->get_port()] = scaler; }
+double Drive::drive_imu_scaler_get() { return imu_scale_map[imu->get_port()]; }
+
+void Drive::drive_imus_scalers_set(std::vector<double> scales) {
+  for (int i = 0; i < std::min(good_imus.size(), scales.size()); i++) {
+    imu_scale_map[good_imus[i]->get_port()] = scales[i];
+  }
+}
+std::map<int, double> Drive::drive_imus_scalers_get() { return imu_scale_map; }
 
 void Drive::drive_imu_display_loading(int iter) {
   // If the lcd is already initialized, don't run this function
@@ -373,38 +455,90 @@ void Drive::drive_imu_display_loading(int iter) {
 
 bool Drive::drive_imu_calibrate(bool run_loading_animation) {
   imu_calibration_complete = false;
-  imu.reset();
-  int iter = 0;
-  bool current_status = imu.is_calibrating();
-  bool last_status = current_status;
+  bool one_calibrated = false;
+
+  // No IMUs are calibrated yet, set them all to false
+  std::map<int, bool> imus_status, imus_done, imus_last_status;
+  for (int i = 0; i < good_imus.size(); i++) {
+    good_imus[i]->reset();
+    int port = good_imus[i]->get_port();
+    imus_status[port] = good_imus[i]->is_calibrating();
+    imus_last_status[port] = imus_status[port];
+    imus_done[port] = false;
+  }
+
   bool successful = false;
+  int iter = 0;
   while (true) {
     iter += util::DELAY_TIME;
 
     if (run_loading_animation) drive_imu_display_loading(iter);
 
     if (!successful) {
-      last_status = current_status;
-      current_status = imu.is_calibrating();
-      successful = !current_status && last_status ? true : false;
+      // Check if each IMU is done calibrating
+      for (int i = 0; i < good_imus.size(); i++) {
+        int port = good_imus[i]->get_port();
+        imus_last_status[port] = imus_status[port];
+        imus_status[port] = good_imus[i]->is_calibrating();
+        if (!imus_done[port])
+          imus_done[port] = !imus_status[port] && imus_last_status[port] ? true : false;
+      }
+    }
+
+    // Check if all IMUs have calibrated
+    successful = true;
+    for (int i = 0; i < good_imus.size(); i++) {
+      if (!imus_done[good_imus[i]->get_port()]) {
+        successful = false;
+      } else {
+        one_calibrated = true;  // Remember that at least 1 IMU has calibrated
+      }
     }
 
     if (iter >= 2000) {
       if (successful) {
+        printf("IMU is done calibrating (took %d ms)\n", iter);
+        imu_calibration_complete = true;
+        imu_calibrate_took_too_long = iter > 2000 ? true : false;
         break;
       }
       if (iter >= 3000) {
-        printf("No IMU plugged in, (took %d ms to realize that)\n", iter);
+        if (!one_calibrated) {
+          printf("No IMU plugged in");
+        } else {
+          printf("Only IMUs in ports {");
+          for (int i = 0; i < good_imus.size(); i++) {
+            int port = good_imus[i]->get_port();
+            if (imus_done[port])
+              printf(" %i", port);
+          }
+          printf(" } calibrated");
+        }
+        printf(", (took %d ms to realize that)\n", iter);
         imu_calibrate_took_too_long = true;
-        return false;
+        break;
       }
     }
     pros::delay(util::DELAY_TIME);
   }
-  printf("IMU is done calibrating (took %d ms)\n", iter);
-  imu_calibration_complete = true;
-  imu_calibrate_took_too_long = iter > 2000 ? true : false;
-  return true;
+
+  // Run through all of the IMUs and remove any IMUs that didn't calibrate successfully
+  for (int i = 0; i < good_imus.size(); i++) {
+    int port = good_imus[i]->get_port();
+
+    if (!imus_done[port]) {
+      good_imus.erase(good_imus.begin() + i);
+      if (i == 0 && !good_imus.empty())
+        imu = good_imus.front();
+    }
+  }
+
+  if (one_calibrated && !good_imus.empty())
+    imu_calibration_complete = true;
+
+  printf("one cali-%i\n", one_calibrated);
+
+  return imu_calibration_complete;
 }
 
 bool Drive::drive_imu_calibrated() {
diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp
index 159dc689..ae252dcd 100644
--- a/src/EZ-Template/drive/exit_conditions.cpp
+++ b/src/EZ-Template/drive/exit_conditions.cpp
@@ -253,10 +253,10 @@ void Drive::wait_until_turn_swing(double target) {
   }
 
   // Create new target that is the shortest from current
-  target = new_turn_target_compute(target, drive_imu_get(), shortest);
+  target = new_turn_target_compute(target, drive_angle_get(), shortest);
 
   // Calculate error between current and target (target needs to be an in between position)
-  double g_error = target - drive_imu_get();
+  double g_error = target - drive_angle_get();
   int g_sgn = util::sgn(g_error);
 
   exit_output turn_exit = RUNNING;
@@ -265,7 +265,7 @@ void Drive::wait_until_turn_swing(double target) {
   pros::Motor& sensor = current_swing == ez::LEFT_SWING ? left_motors[0] : right_motors[0];
 
   while (true) {
-    g_error = target - drive_imu_get();
+    g_error = target - drive_angle_get();
 
     // If turning...
     if (mode == TURN || mode == TURN_TO_POINT) {
@@ -276,7 +276,7 @@ void Drive::wait_until_turn_swing(double target) {
           turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
           pros::delay(util::DELAY_TIME);
         } else {
-          if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Wait Until Exit Failsafe, triggered at " << drive_imu_get() << " instead of " << target << "\n";
+          if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Wait Until Exit Failsafe, triggered at " << drive_angle_get() << " instead of " << target << "\n";
 
           if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) {
             interfered = true;
@@ -286,7 +286,7 @@ void Drive::wait_until_turn_swing(double target) {
       }
       // Once we've past target, return
       else if (util::sgn(g_error) != g_sgn) {
-        if (print_toggle) printf("  Turn Wait Until Exit Success, triggered at %.2f.  Target: %.2f\n", drive_imu_get(), target);
+        if (print_toggle) printf("  Turn Wait Until Exit Success, triggered at %.2f.  Target: %.2f\n", drive_angle_get(), target);
         turnPID.timers_reset();
         return;
       }
@@ -301,7 +301,7 @@ void Drive::wait_until_turn_swing(double target) {
           swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
           pros::delay(util::DELAY_TIME);
         } else {
-          if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Wait Until Exit Failsafe, triggered at " << drive_imu_get() << " instead of " << target << "\n";
+          if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Wait Until Exit Failsafe, triggered at " << drive_angle_get() << " instead of " << target << "\n";
 
           if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) {
             interfered = true;
@@ -311,7 +311,7 @@ void Drive::wait_until_turn_swing(double target) {
       }
       // Once we've past target, return
       else if (util::sgn(g_error) != g_sgn) {
-        if (print_toggle) printf("  Swing Wait Until Exit Success, triggered at %.2f. Target: %.2f\n", drive_imu_get(), target);
+        if (print_toggle) printf("  Swing Wait Until Exit Success, triggered at %.2f. Target: %.2f\n", drive_angle_get(), target);
         swingPID.timers_reset();
         return;
       }
diff --git a/src/EZ-Template/drive/maintenance.cpp b/src/EZ-Template/drive/maintenance.cpp
new file mode 100644
index 00000000..95c45188
--- /dev/null
+++ b/src/EZ-Template/drive/maintenance.cpp
@@ -0,0 +1,31 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/drive/drive.hpp"
+#include "EZ-Template/util.hpp"
+#include "api.h"
+
+using namespace ez;
+
+void Drive::check_imu_task() {
+  // Don't let this function run if IMU calibration is incomplete
+  if (!imu_calibration_complete) return;
+
+  // Erase indices only if imu val equals previous one
+  good_imus.erase(std::remove_if(good_imus.begin(), good_imus.end(), [this](pros::Imu *n) { return n->get_status() == pros::ImuStatus::error /*|| errno == PROS_ERR*/ || prev_imu_values[n->get_port()].second >= 5; }), good_imus.end());
+
+  // Increment every time an IMU doesn't update
+  for (size_t i = 0; i < good_imus.size(); i++) {
+    if (prev_imu_values[good_imus[i]->get_port()].first == get_this_imu(good_imus[i]))
+      prev_imu_values[good_imus[i]->get_port()].second += 1;
+    else
+      prev_imu_values[good_imus[i]->get_port()].second = 0;
+    prev_imu_values[good_imus[i]->get_port()].first = get_this_imu(good_imus[i]);
+  }
+
+  if (!good_imus.empty())
+    imu = good_imus.front();
+}
\ No newline at end of file
diff --git a/src/EZ-Template/drive/pid_tasks.cpp b/src/EZ-Template/drive/pid_tasks.cpp
index f2a888a7..c2e584fb 100644
--- a/src/EZ-Template/drive/pid_tasks.cpp
+++ b/src/EZ-Template/drive/pid_tasks.cpp
@@ -12,6 +12,9 @@ using namespace ez;
 
 void Drive::ez_auto_task() {
   while (true) {
+    // Check IMUs for redundancy
+    check_imu_task();
+
     // Run odom
     ez_tracking_task();
 
@@ -51,7 +54,7 @@ void Drive::drive_pid_task() {
   leftPID.compute(drive_sensor_left());
   rightPID.compute(drive_sensor_right());
 
-  headingPID.compute(drive_imu_get());
+  headingPID.compute(drive_angle_get());
 
   // Compute slew
   slew_left.iterate(drive_sensor_left());
@@ -93,7 +96,7 @@ void Drive::drive_pid_task() {
 void Drive::turn_pid_task() {
   // Compute PID if it's a normal turn
   if (mode == TURN) {
-    turnPID.compute(drive_imu_get());
+    turnPID.compute(drive_angle_get());
   }
   // Compute PID if we're turning to point
   else {
@@ -104,7 +107,7 @@ void Drive::turn_pid_task() {
   }
 
   // Compute slew
-  slew_turn.iterate(drive_imu_get());
+  slew_turn.iterate(drive_angle_get());
 
   // Clip gyroPID to max speed
   double gyro_out = util::clamp(turnPID.output, slew_turn.output(), -slew_turn.output());
@@ -123,12 +126,12 @@ void Drive::turn_pid_task() {
 // Swing PID task
 void Drive::swing_pid_task() {
   // Compute PID
-  swingPID.compute(drive_imu_get());
+  swingPID.compute(drive_angle_get());
   leftPID.compute(drive_sensor_left());
   rightPID.compute(drive_sensor_right());
 
   // Compute slew
-  double current = slew_swing_using_angle ? drive_imu_get() : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
+  double current = slew_swing_using_angle ? drive_angle_get() : (current_swing == LEFT_SWING ? drive_sensor_left() : drive_sensor_right());
   slew_swing.iterate(current);
 
   // Clip swingPID to max speed
diff --git a/src/EZ-Template/drive/set_pid/set_odom_pid.cpp b/src/EZ-Template/drive/set_pid/set_odom_pid.cpp
index 10a002a0..70fecbfa 100644
--- a/src/EZ-Template/drive/set_pid/set_odom_pid.cpp
+++ b/src/EZ-Template/drive/set_pid/set_odom_pid.cpp
@@ -402,7 +402,7 @@ void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) {
   point_to_face = find_point_to_face(odom_pose_get(), {imovement.target.x, imovement.target.y}, current_drive_direction, true);
   double target = util::absolute_angle_to_point(point_to_face[!ptf1_running], odom_pose_get());  // Calculate the point for angle to face
   if (imovement.turn_behavior != raw) {
-    odom_imu_start = drive_imu_get();
+    odom_imu_start = drive_angle_get();
     current_angle_behavior = imovement.turn_behavior;
   }
 
diff --git a/src/EZ-Template/drive/set_pid/set_swing_pid.cpp b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp
index 41d2b787..52c3156f 100644
--- a/src/EZ-Template/drive/set_pid/set_swing_pid.cpp
+++ b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp
@@ -93,7 +93,7 @@ ez::e_angle_behavior Drive::pid_swing_behavior_get() { return default_swing_type
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed) {
@@ -104,7 +104,7 @@ void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed) {
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed) {
   // Figure out if going forward or backward
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, 0, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed) {
@@ -117,7 +117,7 @@ void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int spe
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, 0, behavior, slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
@@ -128,7 +128,7 @@ void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_ang
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior) {
   // Figure out if going forward or backward
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, 0, behavior, slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior) {
@@ -141,18 +141,18 @@ void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int spe
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
   double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, slew_on);
 }
 // Relative
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed) {
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, opposite_speed, pid_swing_behavior_get(), slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed) {
@@ -185,18 +185,18 @@ void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int spe
 /////
 // Absolute
 void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior) {
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, behavior, slew_on);
 }
 void Drive::pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior) {
   double target = p_target.convert(okapi::degree);  // Convert okapi unit to degree
-  bool slew_on = is_swing_slew_enabled(type, target, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, target, drive_angle_get());
   pid_swing_set(type, target, speed, opposite_speed, behavior);
 }
 // Relative
 void Drive::pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior) {
   double absolute_heading = target + headingPID.target_get();
-  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_imu_get());
+  bool slew_on = is_swing_slew_enabled(type, absolute_heading, drive_angle_get());
   pid_swing_relative_set(type, target, speed, opposite_speed, behavior, slew_on);
 }
 void Drive::pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior) {
@@ -281,19 +281,19 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s
 
   // Compute new turn target based on new angle
   target = flip_angle_target(target);
-  target = new_turn_target_compute(target, drive_imu_get(), current_angle_behavior);
+  target = new_turn_target_compute(target, drive_angle_get(), current_angle_behavior);
 
   // Print targets
   if (print_toggle) printf("Swing Started... Target Value: %.2f\n", target);
 
-  chain_sensor_start = drive_imu_get();
+  chain_sensor_start = drive_angle_get();
   chain_target_start = target;
   used_motion_chain_scale = 0.0;
 
   // Flip the swing from left-right if rotation axis is flipped
-  if (odom_theta_direction_get()) 
-    current_swing = type == ez::LEFT_SWING ? ez::RIGHT_SWING : ez::LEFT_SWING;
-  
+  current_swing = type;
+  if (odom_theta_direction_get())
+    current_swing = current_swing == ez::LEFT_SWING ? ez::RIGHT_SWING : ez::LEFT_SWING;
 
   // Figure out if going forward or backward
   int side = type == ez::LEFT_SWING ? 1 : -1;
diff --git a/src/EZ-Template/drive/set_pid/set_turn_pid.cpp b/src/EZ-Template/drive/set_pid/set_turn_pid.cpp
index 8a7a222a..5c5aadf7 100644
--- a/src/EZ-Template/drive/set_pid/set_turn_pid.cpp
+++ b/src/EZ-Template/drive/set_pid/set_turn_pid.cpp
@@ -123,11 +123,11 @@ void Drive::pid_turn_set(double target, int speed, e_angle_behavior behavior, bo
 
   // Compute new turn target based on new angle
   target = flip_angle_target(target);
-  target = new_turn_target_compute(target, drive_imu_get(), current_angle_behavior);
+  target = new_turn_target_compute(target, drive_angle_get(), current_angle_behavior);
 
   // Print targets
   if (print_toggle) printf("Turn Started... Target Value: %.2f\n", target);
-  chain_sensor_start = drive_imu_get();
+  chain_sensor_start = drive_angle_get();
   chain_target_start = target;
   used_motion_chain_scale = 0.0;
 
@@ -176,7 +176,7 @@ void Drive::pid_turn_set(united_pose p_itarget, drive_directions dir, int speed,
 /////
 void Drive::pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on) {
   itarget = flip_pose(itarget);
-  odom_imu_start = drive_imu_get();
+  odom_imu_start = drive_angle_get();
 
   current_drive_direction = dir;
   current_angle_behavior = behavior;
diff --git a/src/EZ-Template/drive/tracking.cpp b/src/EZ-Template/drive/tracking.cpp
index c5ed77f9..8c9cb344 100644
--- a/src/EZ-Template/drive/tracking.cpp
+++ b/src/EZ-Template/drive/tracking.cpp
@@ -166,7 +166,7 @@ void Drive::ez_tracking_task() {
   r_last = r_current;
 
   // Angle and velocity
-  float t_current = -ez::util::to_rad(drive_imu_get());  // negative for math standard
+  float t_current = -ez::util::to_rad(drive_angle_get());  // negative for math standard
   float t_ = t_current - t_last;
   t_last = t_current;
 
@@ -223,7 +223,7 @@ void Drive::ez_tracking_task() {
     }
   }
 
-  odom_current.theta = drive_imu_get();
+  odom_current.theta = drive_angle_get();
 
   // This is used for PID as a "current" sensor value
   // what this value actually is doesn't matter, it just needs to move with the correct sign
diff --git a/src/main.cpp b/src/main.cpp
index 36291630..3bcd7b12 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -11,16 +11,17 @@ ez::Drive chassis(
     {-5, -6, -7, -8},  // Left Chassis Ports (negative port will reverse it!)
     {11, 15, 16, 17},  // Right Chassis Ports (negative port will reverse it!)
 
-    21,      // IMU Port
-    4.125,   // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
-    420.0);  // Wheel RPM = cartridge * (motor gear / wheel gear)
-
-// Are you using tracking wheels?  Comment out which ones you're using here!
-//  `2.75` is the wheel diameter
-//  `4.0` is the distance from the center of the wheel to the center of the robot
-// ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0);  // ADI Encoders
-// ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0);  // ADI Encoders plugged into a Smart port
-// ez::tracking_wheel horiz_tracker(1, 2.75, 4.0);             // Rotation sensors
+    {20, 21},  // IMU Port
+    4.125,     // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
+    420.0);    // Wheel RPM = cartridge * (motor gear / wheel gear)
+
+// Uncomment the trackers you're using here!
+// - `8` and `9` are smart ports (making these negative will reverse the sensor)
+//  - you should get positive values on the encoders going FORWARD and RIGHT
+// - `2.75` is the wheel diameter
+// - `4.0` is the distance from the center of the wheel to the center of the robot
+// ez::tracking_wheel horiz_tracker(8, 2.75, 4.0);  // This tracking wheel is perpendicular to the drive wheels
+// ez::tracking_wheel vert_tracker(9, 2.75, 4.0);   // This tracking wheel is parallel to the drive wheels
 
 /**
  * Runs initialization code. This occurs as soon as the program is started.
@@ -34,10 +35,14 @@ void initialize() {
 
   pros::delay(500);  // Stop the user from doing anything while legacy ports configure
 
-  // Are you using tracking wheels?  Comment out which ones you're using here!
-  // chassis.odom_tracker_right_set(&right_tracker);
-  // chassis.odom_tracker_left_set(&left_tracker);
-  // chassis.odom_tracker_back_set(&horiz_tracker);  // Replace `back` to `front` if your tracker is in the front!
+  // Look at your horizontal tracking wheel and decide if it's in front of the midline of your robot or behind it
+  //  - change `back` to `front` if the tracking wheel is in front of the midline
+  //  - ignore this if you aren't using a horizontal tracker
+  // chassis.odom_tracker_back_set(&horiz_tracker);
+  // Look at your vertical tracking wheel and decide if it's to the left or right of the center of the robot
+  //  - change `left` to `right` if the tracking wheel is to the right of the centerline
+  //  - ignore this if you aren't using a vertical tracker
+  // chassis.odom_tracker_left_set(&vert_tracker);
 
   // Configure your chassis controls
   chassis.opcontrol_curve_buttons_toggle(true);   // Enables modifying the controller curve with buttons on the joysticks
@@ -66,7 +71,7 @@ void initialize() {
       {"Pure Pursuit Wait Until\n\nGo to (24, 24) but start running an intake once the robot passes (12, 24)", odom_pure_pursuit_wait_until_example},
       {"Boomerang\n\nGo to (0, 24, 45) then come back to (0, 0, 0)", odom_boomerang_example},
       {"Boomerang Pure Pursuit\n\nGo to (0, 24, 45) on the way to (24, 24) then come back to (0, 0, 0)", odom_boomerang_injected_pure_pursuit_example},
-      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", odom_boomerang_injected_pure_pursuit_example},
+      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", measure_offsets},
   });
 
   // Initialize chassis and auton selector
diff --git a/version b/version
index 0444f320..acf9bf09 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-3.2.1
\ No newline at end of file
+3.2.2
\ No newline at end of file