Added measure offsets

EZ-Template-Example-Project/include/autons.hpp

@@ -14,4 +14,5 @@ void odom_drive_example();
 void odom_pure_pursuit_example();
 void odom_pure_pursuit_wait_until_example();
 void odom_boomerang_example();
-void odom_boomerang_injected_pure_pursuit_example();
+void odom_boomerang_injected_pure_pursuit_example();
+void measure_offsets();

EZ-Template-Example-Project/src/autons.cpp

@@ -316,6 +316,66 @@ void odom_boomerang_injected_pure_pursuit_example() {
   chassis.pid_wait();
 }
 
+///
+// Calculate the offsets of your tracking wheels
+///
+void measure_offsets() {
+  // Number of times to test
+  int iterations = 10;
+
+  // Our final offsets
+  double l_offset = 0.0, r_offset = 0.0, b_offset = 0.0, f_offset = 0.0;
+
+  // Reset all trackers if they exist
+  if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->reset();
+  if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->reset();
+  if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->reset();
+  if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->reset();
+
+  for (int i = 0; i < iterations; i++) {
+    // Reset pid targets and get ready for running an auton
+    chassis.pid_targets_reset();
+    chassis.drive_imu_reset();
+    chassis.drive_sensor_reset();
+    chassis.drive_brake_set(MOTOR_BRAKE_HOLD);
+    chassis.odom_xyt_set(0_in, 0_in, 0_deg);
+    double imu_start = chassis.odom_theta_get();
+    double target = i % 2 == 0 ? 90 : 270;  // Switch the turn target every run from 270 to 90
+
+    // Turn to target at half power
+    chassis.pid_turn_set(target, 63, ez::raw);
+    chassis.pid_wait();
+    pros::delay(250);
+
+    // Calculate delta in angle
+    double t_delta = util::to_rad(fabs(util::wrap_angle(chassis.odom_theta_get() - imu_start)));
+
+    // Calculate delta in sensor values that exist
+    double l_delta = chassis.odom_tracker_left != nullptr ? chassis.odom_tracker_left->get() : 0.0;
+    double r_delta = chassis.odom_tracker_right != nullptr ? chassis.odom_tracker_right->get() : 0.0;
+    double b_delta = chassis.odom_tracker_back != nullptr ? chassis.odom_tracker_back->get() : 0.0;
+    double f_delta = chassis.odom_tracker_front != nullptr ? chassis.odom_tracker_front->get() : 0.0;
+
+    // Calculate the radius that the robot traveled
+    l_offset += l_delta / t_delta;
+    r_offset += r_delta / t_delta;
+    b_offset += b_delta / t_delta;
+    f_offset += f_delta / t_delta;
+  }
+
+  // Average all offsets
+  l_offset /= iterations;
+  r_offset /= iterations;
+  b_offset /= iterations;
+  f_offset /= iterations;
+
+  // Set new offsets to trackers that exist
+  if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->distance_to_center_set(l_offset);
+  if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->distance_to_center_set(r_offset);
+  if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->distance_to_center_set(b_offset);
+  if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->distance_to_center_set(f_offset);
+}
+
 // . . .
 // Make your own autonomous functions here!
 // . . .

EZ-Template-Example-Project/src/main.cpp

@@ -53,19 +53,20 @@ void initialize() {
 
   // Autonomous Selector using LLEMU
   ez::as::auton_selector.autons_add({
-      {"Drive\n\nDrive forward and come back.", drive_example},
+      {"Drive\n\nDrive forward and come back", drive_example},
       {"Turn\n\nTurn 3 times.", turn_example},
-      {"Drive and Turn\n\nDrive forward, turn, come back. ", drive_and_turn},
-      {"Drive and Turn\n\nSlow down during drive.", wait_until_change_speed},
+      {"Drive and Turn\n\nDrive forward, turn, come back", drive_and_turn},
+      {"Drive and Turn\n\nSlow down during drive", wait_until_change_speed},
       {"Swing Turn\n\nSwing in an 'S' curve", swing_example},
       {"Motion Chaining\n\nDrive forward, turn, and come back, but blend everything together :D", motion_chaining},
       {"Combine all 3 movements", combining_movements},
-      {"Interference\n\nAfter driving forward, robot performs differently if interfered or not.", interfered_example},
+      {"Interference\n\nAfter driving forward, robot performs differently if interfered or not", interfered_example},
       {"Simple Odom\n\nThis is the same as the drive example, but it uses odom instead!", odom_drive_example},
       {"Pure Pursuit\n\nGo to (0, 30) and pass through (6, 10) on the way.  Come back to (0, 0)", odom_pure_pursuit_example},
       {"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},
   });
 
   // Initialize chassis and auton selector

include/autons.hpp

@@ -14,4 +14,5 @@ void odom_drive_example();
 void odom_pure_pursuit_example();
 void odom_pure_pursuit_wait_until_example();
 void odom_boomerang_example();
-void odom_boomerang_injected_pure_pursuit_example();
+void odom_boomerang_injected_pure_pursuit_example();
+void measure_offsets();

src/autons.cpp

@@ -316,6 +316,66 @@ void odom_boomerang_injected_pure_pursuit_example() {
   chassis.pid_wait();
 }
 
+///
+// Calculate the offsets of your tracking wheels
+///
+void measure_offsets() {
+  // Number of times to test
+  int iterations = 10;
+
+  // Our final offsets
+  double l_offset = 0.0, r_offset = 0.0, b_offset = 0.0, f_offset = 0.0;
+
+  // Reset all trackers if they exist
+  if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->reset();
+  if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->reset();
+  if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->reset();
+  if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->reset();
+
+  for (int i = 0; i < iterations; i++) {
+    // Reset pid targets and get ready for running an auton
+    chassis.pid_targets_reset();
+    chassis.drive_imu_reset();
+    chassis.drive_sensor_reset();
+    chassis.drive_brake_set(MOTOR_BRAKE_HOLD);
+    chassis.odom_xyt_set(0_in, 0_in, 0_deg);
+    double imu_start = chassis.odom_theta_get();
+    double target = i % 2 == 0 ? 90 : 270;  // Switch the turn target every run from 270 to 90
+
+    // Turn to target at half power
+    chassis.pid_turn_set(target, 63, ez::raw);
+    chassis.pid_wait();
+    pros::delay(250);
+
+    // Calculate delta in angle
+    double t_delta = util::to_rad(fabs(util::wrap_angle(chassis.odom_theta_get() - imu_start)));
+
+    // Calculate delta in sensor values that exist
+    double l_delta = chassis.odom_tracker_left != nullptr ? chassis.odom_tracker_left->get() : 0.0;
+    double r_delta = chassis.odom_tracker_right != nullptr ? chassis.odom_tracker_right->get() : 0.0;
+    double b_delta = chassis.odom_tracker_back != nullptr ? chassis.odom_tracker_back->get() : 0.0;
+    double f_delta = chassis.odom_tracker_front != nullptr ? chassis.odom_tracker_front->get() : 0.0;
+
+    // Calculate the radius that the robot traveled
+    l_offset += l_delta / t_delta;
+    r_offset += r_delta / t_delta;
+    b_offset += b_delta / t_delta;
+    f_offset += f_delta / t_delta;
+  }
+
+  // Average all offsets
+  l_offset /= iterations;
+  r_offset /= iterations;
+  b_offset /= iterations;
+  f_offset /= iterations;
+
+  // Set new offsets to trackers that exist
+  if (chassis.odom_tracker_left != nullptr) chassis.odom_tracker_left->distance_to_center_set(l_offset);
+  if (chassis.odom_tracker_right != nullptr) chassis.odom_tracker_right->distance_to_center_set(r_offset);
+  if (chassis.odom_tracker_back != nullptr) chassis.odom_tracker_back->distance_to_center_set(b_offset);
+  if (chassis.odom_tracker_front != nullptr) chassis.odom_tracker_front->distance_to_center_set(f_offset);
+}
+
 // . . .
 // Make your own autonomous functions here!
 // . . .

src/main.cpp

@@ -53,19 +53,20 @@ void initialize() {
 
   // Autonomous Selector using LLEMU
   ez::as::auton_selector.autons_add({
-      {"Drive\n\nDrive forward and come back.", drive_example},
+      {"Drive\n\nDrive forward and come back", drive_example},
       {"Turn\n\nTurn 3 times.", turn_example},
-      {"Drive and Turn\n\nDrive forward, turn, come back. ", drive_and_turn},
-      {"Drive and Turn\n\nSlow down during drive.", wait_until_change_speed},
+      {"Drive and Turn\n\nDrive forward, turn, come back", drive_and_turn},
+      {"Drive and Turn\n\nSlow down during drive", wait_until_change_speed},
       {"Swing Turn\n\nSwing in an 'S' curve", swing_example},
       {"Motion Chaining\n\nDrive forward, turn, and come back, but blend everything together :D", motion_chaining},
       {"Combine all 3 movements", combining_movements},
-      {"Interference\n\nAfter driving forward, robot performs differently if interfered or not.", interfered_example},
+      {"Interference\n\nAfter driving forward, robot performs differently if interfered or not", interfered_example},
       {"Simple Odom\n\nThis is the same as the drive example, but it uses odom instead!", odom_drive_example},
       {"Pure Pursuit\n\nGo to (0, 30) and pass through (6, 10) on the way.  Come back to (0, 0)", odom_pure_pursuit_example},
       {"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},
   });
 
   // Initialize chassis and auton selector