✨Feature/redundant imu

.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: ''
 ---

.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: ''
 ---

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

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

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)

EZ-Template-Example-Project/project.pros

@@ -5,29 +5,29 @@
         "target": "v5",
         "templates": {
             "EZ-Template": {
-                "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\[email protected].1",
+                "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\[email protected].2",
                 "metadata": {
                     "origin": "local"
                 },
                 "name": "EZ-Template",
                 "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\\[email protected]",

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

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)))

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)

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