Added missing functions

and some simple refactoring

include/EZ-Template/drive/drive.hpp

@@ -426,7 +426,15 @@ class Drive {
   void pid_wait_until_index(int index);
   void pid_wait_until_point(pose target);
   void pid_wait_until(pose target);
-  double dlead = 0.375;
+  double dlead = 0.5;
+  void odom_boomerang_dlead_set(double input);
+  double odom_boomerang_dlead_get();
+  double max_boomerang_distance = 12.0;
+  void odom_boomerang_distance_set(double distance);
+  double odom_boomerang_distance_get();
+  double odom_turn_bias_amount = 1.375;
+  void odom_turn_bias_set(double bias);
+  double odom_turn_bias_get();
   //  Odometry
   bool odometry_enabled = true;
   float track_width = 0.0;

src/EZ-Template/drive/pid_tasks.cpp

@@ -184,7 +184,7 @@ void Drive::ptp_task() {
   // Prioritize turning by scaling xy_out down
   double xy_out = xyPID.output;
   // xy_out = util::clamp(xy_out, max_slew_out, -max_slew_out);
-  xy_out *= cos(util::to_rad(aPID.error)) / 1.375;
+  xy_out *= cos(util::to_rad(aPID.error)) / odom_turn_bias_amount;
   double a_out = aPID.output;
   // a_out = util::clamp(a_out, max_slew_out, -max_slew_out);
 
@@ -231,7 +231,7 @@ void Drive::boomerang_task() {
   int dir = current_drive_direction == REV ? -1 : 1;
 
   double h = util::distance_to_point(target, odom_current) * dlead;
-  double max = 12.0;
+  double max = max_boomerang_distance;
   h = h > max ? max : h;
   h *= dir;
 

src/EZ-Template/drive/set_pid/set_odom_pid.cpp

@@ -44,6 +44,12 @@ void Drive::odom_x_direction_flip(bool flip) { x_flipped = flip; }
 bool Drive::odom_x_direction_get() { return x_flipped; }
 void Drive::odom_y_direction_flip(bool flip) { y_flipped = flip; }
 bool Drive::odom_y_direction_get() { return y_flipped; }
+void Drive::odom_boomerang_dlead_set(double input) { dlead = input; }
+double Drive::odom_boomerang_dlead_get() { return dlead; }
+void Drive::odom_boomerang_distance_set(double distance) { max_boomerang_distance = distance; }
+double Drive::odom_boomerang_distance_get() { return max_boomerang_distance; }
+void Drive::odom_turn_bias_set(double bias) { odom_turn_bias_amount = 1.375; }
+double Drive::odom_turn_bias_get() { return odom_turn_bias_amount; }
 
 /////
 // pid_odom_set

src/EZ-Template/drive/tracking.cpp

@@ -8,31 +8,19 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 using namespace ez;
 
-void Drive::odom_pose_x_set(double x) {
-  // odom_target.x = x;
-  odom_current.x = x;
-}
-void Drive::odom_pose_y_set(double y) {
-  // odom_target.y = y;
-  odom_current.y = y;
-}
-
+void Drive::odom_pose_x_set(double x) { odom_current.x = x; }
+void Drive::odom_pose_y_set(double y) { odom_current.y = y; }
+void Drive::odom_pose_theta_set(double a) { drive_angle_set(a); }
+void Drive::odom_reset() { odom_pose_set({0, 0, 0}); }
+void Drive::drive_width_set(double input) { track_width = input; }
+double Drive::drive_width_get() { return track_width; }
+void Drive::drive_odom_enable(bool input) { odometry_enabled = input; }
 void Drive::odom_pose_set(pose itarget) {
   odom_pose_theta_set(itarget.theta);
   odom_pose_x_set(itarget.x);
   odom_pose_y_set(itarget.y);
 }
 
-void Drive::odom_pose_theta_set(double a) { drive_angle_set(a); }
-void Drive::odom_reset() { odom_pose_set({0, 0, 0}); }
-
-void Drive::drive_width_set(double input) {
-  track_width = input;
-}
-double Drive::drive_width_get() { return track_width; }
-
-void Drive::drive_odom_enable(bool input) { odometry_enabled = input; }
-
 // Tracking based on https://wiki.purduesigbots.com/software/odometry
 void Drive::ez_tracking_task() {
   if (!imu_calibration_complete || !odometry_enabled) {

src/autons.cpp

@@ -14,21 +14,26 @@ const int SWING_SPEED = 110;
 // Constants
 ///
 void default_constants() {
+  // P, I, D, and Start I
+  // https://ez-robotics.github.io/EZ-Template/tutorials/tuning_constants
   chassis.pid_heading_constants_set(7, 0, 45);
   chassis.pid_drive_constants_set(20, 0, 100);
   chassis.pid_turn_constants_set(3, 0.05, 20, 15);
   chassis.pid_swing_constants_set(6, 0, 65);
 
+  // Exit conditions
+  // https://ez-robotics.github.io/EZ-Template/tutorials/tuning_exit_conditions
   chassis.pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
   chassis.pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
   chassis.pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms);
   chassis.pid_odom_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
   chassis.pid_odom_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
-
   chassis.pid_turn_chain_constant_set(3_deg);
   chassis.pid_swing_chain_constant_set(5_deg);
   chassis.pid_drive_chain_constant_set(3_in);
 
+  // Slew constants
+  // https://ez-robotics.github.io/EZ-Template/tutorials/slew_constants
   chassis.slew_turn_constants_set(3_deg, 70);
   chassis.slew_drive_constants_set(3_in, 70);
   chassis.slew_swing_constants_set(3_in, 80);

src/main.cpp

@@ -97,8 +97,6 @@ void autonomous() {
   chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency
   chassis.odom_pose_set({0, 0, 0});
   chassis.drive_width_set(11);  // just use a tape measure
-  chassis.dlead = 0.5;
-  chassis.odometry_enabled = true;
 
   chassis.pid_odom_set({{{0_in, 16_in}, fwd, 110},
                         {{16_in, 16_in}, fwd, 110}},