Merge pull request #221 from EZ-Robotics/feature/path-smoothing-const…

…ants

Added constants for path smoothing

include/EZ-Template/drive/drive.hpp

@@ -774,6 +774,35 @@ class Drive {
    */
   double odom_path_spacing_get();
 
+  /**
+   * Sets the constants for smoothing out a path.
+   *
+   * Path smoothing based on https://medium.com/@jaems33/understanding-robot-motion-path-smoothing-5970c8363bc4
+   *
+   * \param weight_smooth
+   *        how much weight to update the data
+   * \param weight_data
+   *        how much weight to smooth the coordinates
+   * \param tolerance
+   *        how much change per iteration is necessary to keep iterating
+   */
+  void odom_path_smooth_constants_set(double weight_smooth, double weight_data, double tolerance);
+
+  /**
+   * Returns the constants for smoothing out a path.
+   *
+   * In order of:
+   *  - weight_smooth
+   *  - weight_data
+   *  - tolerance
+   */
+  std::vector<double> odom_path_smooth_constants_get();
+
+  /**
+   * Prints the current path the robot is following.
+   */
+  void odom_path_print();
+
   /**
    * How far away the robot looks in the path during pure pursuits.
    *
@@ -3375,6 +3404,9 @@ class Drive {
   bool opcontrol_arcade_scaling_enabled();
 
  private:
+  double odom_smooth_weight_smooth = 0.0;
+  double odom_smooth_weight_data = 0.0;
+  double odom_smooth_tolerance = 0.0;
   bool odom_use_left = true;
   double odom_ime_track_width_left = 0.0;
   double odom_ime_track_width_right = 0.0;
@@ -3387,7 +3419,7 @@ class Drive {
   std::vector<odom> pp_movements;
   std::vector<int> injected_pp_index;
   int pp_index = 0;
-  std::vector<odom> smooth_path(std::vector<odom> ipath, double weight_smooth = 0.75, double weight_data = 0.03, double tolerance = 0.0001);
+  std::vector<odom> smooth_path(std::vector<odom> ipath, double weight_smooth, double weight_data, double tolerance);
   double is_past_target(pose target, pose current);
   void raw_pid_odom_pp_set(std::vector<odom> imovements, bool slew_on);
   bool ptf1_running = false;

src/EZ-Template/drive/drive.cpp

@@ -162,6 +162,14 @@ void Drive::drive_defaults_set() {
   pid_turn_min_set(30);
   pid_swing_min_set(30);
 
+  // Path Constants
+  odom_path_smooth_constants_set(0.75, 0.03, 0.0001);
+  odom_path_spacing_set(0.5_in);
+  odom_turn_bias_set(0.9);
+  odom_look_ahead_set(7_in);
+  odom_boomerang_distance_set(16_in);
+  odom_boomerang_dlead_set(0.625);
+
   // Slew constants
   slew_turn_constants_set(3_deg, 70);
   slew_drive_constants_set(3_in, 70);
@@ -174,6 +182,9 @@ void Drive::drive_defaults_set() {
   pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
   pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
 
+  pid_odom_behavior_set(ez::shortest);  // Default odom turning to shortest
+
+  // Motion chaining
   pid_turn_chain_constant_set(3_deg);
   pid_swing_chain_constant_set(5_deg);
   pid_drive_chain_constant_set(3_in);

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

@@ -4,12 +4,17 @@ 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/api.hpp"
+#include "EZ-Template/drive/drive.hpp"
 #include "okapi/api/units/QAngle.hpp"
 
 /////
 // Set constants
 /////
+void Drive::odom_path_print() {
+  for (int i = 0; i < pp_movements.size(); i++) {
+    printf("Point %i: (%.2f, %.2f, %.2f)\n", i, pp_movements[i].target.x, pp_movements[i].target.y, pp_movements[i].target.theta);
+  }
+}
 void Drive::pid_odom_behavior_set(ez::e_angle_behavior behavior) { default_odom_type = behavior; }
 ez::e_angle_behavior Drive::pid_odom_behavior_get() { return default_odom_type; }
 // Flip all inputs so it works internally
@@ -48,6 +53,16 @@ void Drive::pid_odom_angular_constants_set(double p, double i, double d, double
 void Drive::pid_odom_boomerang_constants_set(double p, double i, double d, double p_start_i) {
   boomerangPID.constants_set(p, i, d, p_start_i);
 }
+
+void Drive::odom_path_smooth_constants_set(double weight_smooth, double weight_data, double tolerance) {
+  odom_smooth_weight_smooth = weight_smooth;
+  odom_smooth_weight_data = weight_data;
+  odom_smooth_tolerance = tolerance;
+}
+std::vector<double> Drive::odom_path_smooth_constants_get() {
+  return {odom_smooth_weight_smooth, odom_smooth_weight_data, odom_smooth_tolerance};
+}
+
 void Drive::odom_x_flip(bool flip) { x_flipped = flip; }
 bool Drive::odom_x_direction_get() { return x_flipped; }
 void Drive::odom_y_flip(bool flip) { y_flipped = flip; }
@@ -222,7 +237,7 @@ void Drive::pid_odom_smooth_pp_set(std::vector<odom> imovements, bool slew_on) {
   current_a_odomPID.timers_reset();
 
   if (print_toggle) printf("Smooth Injected ");
-  std::vector<odom> input_path = smooth_path(inject_points(set_odoms_direction(imovements)), 0.75, 0.03, 0.0001);
+  std::vector<odom> input_path = smooth_path(inject_points(set_odoms_direction(imovements)), odom_smooth_weight_smooth, odom_smooth_weight_data, odom_smooth_tolerance);
   odom_turn_bias_enable(true);
   current_slew_on = slew_on;
   slew_min_when_it_enabled = 0;