init

Author: StarrryNight

src/software/constants.h

@@ -52,6 +52,23 @@ const std::string ROBOT_KICK_CONSTANT_CONFIG_KEY     = "kick_constant";
 const std::string ROBOT_KICK_EXP_COEFF_CONFIG_KEY    = "kick_coeff";
 const std::string ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY  = "chip_pulse_width";
 
+// Position (x/y) controller PID gains
+const std::string ROBOT_POSITION_CONTROLLER_KP_CONFIG_KEY = "position_controller_kp";
+const std::string ROBOT_POSITION_CONTROLLER_KI_CONFIG_KEY = "position_controller_ki";
+const std::string ROBOT_POSITION_CONTROLLER_KD_CONFIG_KEY = "position_controller_kd";
+const std::string ROBOT_POSITION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY =
+    "position_controller_max_integral";
+
+// Orientation (heading) controller PID gains
+const std::string ROBOT_ORIENTATION_CONTROLLER_KP_CONFIG_KEY =
+    "orientation_controller_kp";
+const std::string ROBOT_ORIENTATION_CONTROLLER_KI_CONFIG_KEY =
+    "orientation_controller_ki";
+const std::string ROBOT_ORIENTATION_CONTROLLER_KD_CONFIG_KEY =
+    "orientation_controller_kd";
+const std::string ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY =
+    "orientation_controller_max_integral";
+
 const std::string SSL_VISION_ADDRESS          = "224.5.23.2";
 static constexpr unsigned int SSL_VISION_PORT = 10006;
 

src/software/embedded/motion_control/orientation_controller.cpp

@@ -1,5 +1,10 @@
 #include "software/embedded/motion_control/orientation_controller.h"
 
+OrientationController::OrientationController(const OrientationControllerConfig& config)
+    : w_pid_(config.kp, config.ki, config.kd, config.max_integral)
+{
+}
+
 AngularVelocity OrientationController::step(
     const Angle& orientation, const BangBangTrajectory1DAngular& target_trajectory,
     Duration elapsed_time, Duration delta_time)

src/software/embedded/motion_control/orientation_controller.h

@@ -7,15 +7,30 @@
 #include "software/geom/angular_velocity.h"
 #include "software/time/duration.h"
 
+// TODO(#3737): tune constants
+// PID gains for OrientationController's underlying heading PID controller. Defaults
+// preserve the previously hardcoded tuning; robot_config.toml can override these on a
+// per-robot basis.
+struct OrientationControllerConfig
+{
+    double kp           = 0.4;
+    double ki           = 0.0;
+    double kd           = 0.0;
+    double max_integral = 0.0;
+};
+
 class OrientationController
     : public MotionController<Angle, BangBangTrajectory1DAngular, AngularVelocity>
 {
    public:
     /**
      * Constructs an orientation controller that uses measurements over multiple
      * time intervals to calculate the target angular velocity to minimize error.
+     *
+     * @param config The PID gains to use for the underlying heading PID controller.
      */
-    OrientationController() = default;
+    explicit OrientationController(
+        const OrientationControllerConfig& config = OrientationControllerConfig());
 
     /**
      * Given an orientation and target orientation, returns a target angular
@@ -36,8 +51,7 @@ class OrientationController
     void reset() override;
 
    private:
-    // TODO(#3737): tune constants
-    PidController<double> w_pid_{0.4, 0.0, 0.0, 0.0};
+    PidController<double> w_pid_;
 
     static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 5;
 };

src/software/embedded/motion_control/position_controller.cpp

@@ -2,6 +2,12 @@
 
 #include "software/geom/algorithms/distance.h"
 
+PositionController::PositionController(const PositionControllerConfig& config)
+    : x_pid_(config.kp, config.ki, config.kd, config.max_integral),
+      y_pid_(config.kp, config.ki, config.kd, config.max_integral)
+{
+}
+
 Vector PositionController::step(const Point& position,
                                 const TrajectoryPath& target_trajectory,
                                 Duration elapsed_time, Duration delta_time)

src/software/embedded/motion_control/position_controller.h

@@ -7,14 +7,28 @@
 #include "software/geom/vector.h"
 #include "software/time/duration.h"
 
+// TODO(#3737): tune constants
+// PID gains for PositionController's underlying x/y PID controllers. Both axes share
+// the same gains. Defaults preserve the previously hardcoded tuning; robot_config.toml
+// can override these on a per-robot basis.
+struct PositionControllerConfig
+{
+    double kp           = 1.2;
+    double ki           = 0.1;
+    double kd           = 0.0;
+    double max_integral = 10.0;
+};
+
 class PositionController : public MotionController<Point, TrajectoryPath, Vector>
 {
    public:
     /**
      * Constructs a position controller that uses measurements over multiple time
      * intervals to calculate the target velocity to minimize error.
+     *
+     * @param config The PID gains to use for the underlying x/y PID controllers.
      */
-    PositionController() = default;
+    explicit PositionController(const PositionControllerConfig& config = PositionControllerConfig());
 
     /**
      * Given a position and target trajectory, returns a target global velocity to
@@ -34,9 +48,8 @@ class PositionController : public MotionController<Point, TrajectoryPath, Vector
     void reset() override;
 
    private:
-    // TODO(#3737): tune constants
-    PidController<double> x_pid_{1.2, 0.1, 0.0, 10.0};
-    PidController<double> y_pid_{1.2, 0.1, 0.0, 10.0};
+    PidController<double> x_pid_;
+    PidController<double> y_pid_;
 
     static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05;
 };

src/software/embedded/primitive_executor.cpp

@@ -66,8 +66,14 @@ bool destinationsChangedSignificantly(const TrajectoryPath& current,
 }
 
 PrimitiveExecutor::PrimitiveExecutor(
-    const robot_constants::RobotConstants& robot_constants)
-    : state_(), current_primitive_(), robot_constants_(robot_constants)
+    const robot_constants::RobotConstants& robot_constants,
+    const PositionControllerConfig& position_controller_config,
+    const OrientationControllerConfig& orientation_controller_config)
+    : state_(),
+      current_primitive_(),
+      robot_constants_(robot_constants),
+      position_controller_(position_controller_config),
+      orientation_controller_(orientation_controller_config)
 {
 }
 

src/software/embedded/primitive_executor.h

@@ -19,8 +19,17 @@ class PrimitiveExecutor
      * Constructor
      * @param robot_constants The robot constants for the robot which uses this primitive
      * executor
+     * @param position_controller_config The PID gains for the position controller,
+     * typically read from this robot's robot_config.toml
+     * @param orientation_controller_config The PID gains for the orientation
+     * controller, typically read from this robot's robot_config.toml
      */
-    explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants);
+    explicit PrimitiveExecutor(
+        const robot_constants::RobotConstants& robot_constants,
+        const PositionControllerConfig& position_controller_config =
+            PositionControllerConfig(),
+        const OrientationControllerConfig& orientation_controller_config =
+            OrientationControllerConfig());
 
     /**
      * Update primitive executor with a new Primitive

src/software/embedded/thunderloop.cpp

@@ -81,7 +81,26 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
           std::stoi(toml_config_client_->get(ROBOT_MULTICAST_CHANNEL_CONFIG_KEY))),
       network_interface_(toml_config_client_->get(ROBOT_NETWORK_INTERFACE_CONFIG_KEY)),
       loop_hz_(loop_hz),
-      primitive_executor_(robot_constants),
+      primitive_executor_(
+          robot_constants,
+          PositionControllerConfig{
+              std::stod(toml_config_client_->get(
+                  ROBOT_POSITION_CONTROLLER_KP_CONFIG_KEY)),
+              std::stod(toml_config_client_->get(
+                  ROBOT_POSITION_CONTROLLER_KI_CONFIG_KEY)),
+              std::stod(toml_config_client_->get(
+                  ROBOT_POSITION_CONTROLLER_KD_CONFIG_KEY)),
+              std::stod(toml_config_client_->get(
+                  ROBOT_POSITION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY))},
+          OrientationControllerConfig{
+              std::stod(toml_config_client_->get(
+                  ROBOT_ORIENTATION_CONTROLLER_KP_CONFIG_KEY)),
+              std::stod(toml_config_client_->get(
+                  ROBOT_ORIENTATION_CONTROLLER_KI_CONFIG_KEY)),
+              std::stod(toml_config_client_->get(
+                  ROBOT_ORIENTATION_CONTROLLER_KD_CONFIG_KEY)),
+              std::stod(toml_config_client_->get(
+                  ROBOT_ORIENTATION_CONTROLLER_MAX_INTEGRAL_CONFIG_KEY))}),
       robot_localizer_(RobotLocalizer::RobotLocalizerConfig{
           robot_constants.kalman_process_noise_variance_rad_per_s_4,
           robot_constants.kalman_vision_noise_variance_rad_2,

src/software/embedded/toml_config/toml_config_client.cpp

@@ -47,6 +47,14 @@ void TomlConfigClient::loadConfig()
             {"battery_voltage", "0.0"},
             {"current_draw", "0.0"},
             {"cap_voltage", "0.0"},
+            {"position_controller_kp", "1.2"},
+            {"position_controller_ki", "0.1"},
+            {"position_controller_kd", "0.0"},
+            {"position_controller_max_integral", "10.0"},
+            {"orientation_controller_kp", "0.4"},
+            {"orientation_controller_ki", "0.0"},
+            {"orientation_controller_kd", "0.0"},
+            {"orientation_controller_max_integral", "0.0"},
         };
         writeConfig();
         return;
@@ -81,7 +89,7 @@ void TomlConfigClient::writeConfig()
     has_pending_changes_ = false;
 }
 
-std::string TomlConfigClient::get(const std::string& key)
+std::string TomlConfigClient::get(const std::string& key, const std::string& default_value)
 {
     std::lock_guard<std::mutex> lock(config_mutex_);
 
@@ -106,14 +114,17 @@ std::string TomlConfigClient::get(const std::string& key)
         }
         else
         {
-            LOG(WARNING) << "TOML key '" << key << "' exists but is not a supported type";
-            return "";
+            LOG(WARNING) << "TOML key '" << key
+                         << "' exists but is not a supported type, using default value '"
+                         << default_value << "'";
+            return default_value;
         }
     }
     catch (const std::exception& e)
     {
-        LOG(WARNING) << "TOML key '" << key << "' not found: " << e.what();
-        return "";
+        LOG(WARNING) << "TOML key '" << key << "' not found, using default value '"
+                     << default_value << "': " << e.what();
+        return default_value;
     }
 }
 

src/software/embedded/toml_config/toml_config_client.h

@@ -25,12 +25,15 @@ class TomlConfigClient
 
     /**
      * Gets the value corresponding to the key as a string.
-     * Returns empty string if key doesn't exist.
+     * Returns default_value if the key doesn't exist. This allows config files that
+     * predate the introduction of a key (e.g. an older robot_config.toml already
+     * deployed on a robot) to keep working without needing to be regenerated.
      *
      * @param key The configuration key (e.g., "/robot_id")
-     * @return The value as a string, or empty string if not found
+     * @param default_value The value to return if the key doesn't exist
+     * @return The value as a string, or default_value if not found
      */
-    std::string get(const std::string& key);
+    std::string get(const std::string& key, const std::string& default_value = "");
 
     /**
      * Sets a key-value pair in the TOML file.