✨Tracking is now an std::function

include/EZ-Template/drive/drive.hpp

@@ -3410,7 +3410,33 @@ class Drive {
    */
   bool opcontrol_arcade_scaling_enabled();
 
+  /**
+   * Current odom position.
+   */
+  pose odom_current = {0.0, 0.0, 0.0};
+
+  /**
+   * Tracking task that uses tracking wheels, this is used by default.
+   */
+  void tracking_wheels_tracking();
+
+  /**
+   * Sets a new task to use for tracking.
+   *
+   * In this function, you must:
+   *  - odom_current.x =
+   *  - odom_current.y =
+   *  - odom_current.theta =
+   *
+   * This function does not need to loop, that is done for you in EZ-Template.
+   *
+   * \param tracking_task
+   *        new function for tracking
+   */
+  void odom_tracking_set(std::function<void(void)> tracking_task);
+
  private:
+  std::function<void(void)> tracking;
   void opcontrol_drive_activebrake_targets_set();
   double odom_smooth_weight_smooth = 0.0;
   double odom_smooth_weight_data = 0.0;
@@ -3454,7 +3480,6 @@ class Drive {
   double global_track_width = 0.0;
   bool odometry_enabled = true;
   pose odom_target = {0.0, 0.0, 0.0};
-  pose odom_current = {0.0, 0.0, 0.0};
   pose odom_second_to_last = {0.0, 0.0, 0.0};
   pose odom_start = {0.0, 0.0, 0.0};
   pose odom_target_start = {0.0, 0.0, 0.0};

src/EZ-Template/drive/drive.cpp

@@ -152,6 +152,9 @@ void Drive::drive_defaults_set() {
   std::cout << std::fixed;
   std::cout << std::setprecision(2);
 
+  // Set tracking task, user can override this if they want
+  odom_tracking_set(std::bind(&ez::Drive::tracking_wheels_tracking, this));
+
   // PID Constants
   pid_drive_constants_set(20.0, 0.0, 100.0);
   pid_heading_constants_set(11.0, 0.0, 20.0);

src/EZ-Template/drive/tracking.cpp

@@ -67,6 +67,10 @@ double Drive::odom_theta_get() { return odom_current.theta; }
 pose Drive::odom_pose_get() { return odom_current; }
 double Drive::drive_width_get() { return global_track_width; }
 
+// Set tracking task
+// void Drive::odom_tracking_set(void (*tracking_task)()) { tracking = tracking_task; }
+void Drive::odom_tracking_set(std::function<void(void)> tracking_task) { tracking = tracking_task; }
+
 std::pair<float, float> Drive::decide_vert_sensor(ez::tracking_wheel* tracker, bool is_tracker_enabled, float ime, float ime_track) {
   float current = ime;
   float track_width = ime_track;
@@ -125,21 +129,10 @@ ez::pose Drive::solve_xy_horiz(float p_track_width, float current_t, float delta
 
   return output;
 }
-// pose central_pose;
-// Tracking based on https://wiki.purduesigbots.com/software/odometry
-void Drive::ez_tracking_task() {
-  // Don't let this function run if odom is disabled
-  // and make sure all the "lasts" are 0
-  if (!imu_calibration_complete || !odometry_enabled) {
-    h_last = 0.0;
-    t_last = 0.0;
-    l_last = 0.0;
-    r_last = 0.0;
-    return;
-  }
 
+// Tracking based on https://wiki.purduesigbots.com/software/odometry
+void Drive::tracking_wheels_tracking() {
   // Decide on using a horiz tracker vs not
-
   ez::tracking_wheel* h_sensor = odom_tracker_back != nullptr ? odom_tracker_back : odom_tracker_front;
   bool h_tracker_enabled = h_sensor == odom_tracker_back ? odom_tracker_back_enabled : odom_tracker_front_enabled;
   std::pair<float, float> h_cur_and_track = decide_vert_sensor(h_sensor, h_tracker_enabled);
@@ -225,6 +218,28 @@ void Drive::ez_tracking_task() {
 
   odom_current.theta = drive_imu_get();
 
+  // printf("odom_ime_track_width_left %f   l_ %f   r_ %f   t_current %f\n", odom_ime_track_width_left, r_, t_, t_current);
+
+  // printf("left (%.2f, %.2f)", l_pose.x, l_pose.y);
+  // printf("   right (%.2f, %.2f)", r_pose.x, r_pose.y);
+  // printf("   current used (%.2f, %.2f, %.2f)      l delta: %.2f   r delta: %.2f", odom_current.x, odom_current.y, odom_current.theta, l_, r_);
+  // printf("        htw: %f\n", h_track_width);
+}
+
+void Drive::ez_tracking_task() {
+  // Don't let this function run if odom is disabled
+  // and make sure all the "lasts" are 0
+  if (!imu_calibration_complete || !odometry_enabled) {
+    h_last = 0.0;
+    t_last = 0.0;
+    l_last = 0.0;
+    r_last = 0.0;
+    return;
+  }
+
+  // Use ez's tracking or a custom tracking function made by the user
+  tracking();
+
   // 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
   xy_current_fake = fabs(is_past_target({0.0, 0.0}, odom_pose_get()));
@@ -233,11 +248,4 @@ void Drive::ez_tracking_task() {
   else
     was_odom_just_set = false;
   xy_last_fake = xy_current_fake;
-
-  // printf("odom_ime_track_width_left %f   l_ %f   r_ %f   t_current %f\n", odom_ime_track_width_left, r_, t_, t_current);
-
-  // printf("left (%.2f, %.2f)", l_pose.x, l_pose.y);
-  // printf("   right (%.2f, %.2f)", r_pose.x, r_pose.y);
-  // printf("   current used (%.2f, %.2f, %.2f)      l delta: %.2f   r delta: %.2f", odom_current.x, odom_current.y, odom_current.theta, l_, r_);
-  // printf("        htw: %f\n", h_track_width);
 }