Robot Localizer

src/MODULE.bazel

@@ -219,7 +219,7 @@ git_repository(
     name = "mdv6_firmware",
     build_file = "@//extlibs:mdv6_firmware.bzl",
     # Pinning to the exact commit guarantees Bazel will rebuild when this commit SHA is updated
-    commit = "97e06b9ce8db1940a033b536cd55b4059943d601",  # latest commit on master branch
+    commit = "0f14b9482985159dd132c6fa012970783b0263c3",  # latest commit on master branch
     remote = "https://github.com/UBC-Thunderbots/MDv6_Firmware.git",
 )
 

src/cli/cli_params.py

@@ -192,6 +192,20 @@ class DebugBinary(str, Enum):
     ),
 ]
 
+# Deploy robot software "Options:" checkbox. Each option compiles Thunderloop
+# with a preprocessor flag that disables the corresponding service, letting
+# Thunderloop run on a robot whose powerboard or motorboard is unavailable.
+DEPLOY_ROBOT_SOFTWARE_OPTION_CHOICES = [
+    questionary.Choice(
+        title="DISABLE_POWER_SERVICE",
+        description="Compile Thunderloop without the Power Service (no powerboard)",
+    ),
+    questionary.Choice(
+        title="DISABLE_MOTOR_SERVICE",
+        description="Compile Thunderloop without the Motor Service (no motorboard)",
+    ),
+]
+
 # Marker value returned by the DEBUG_POWERLOOP playbook choice. It maps to the
 # deploy_powerboard.yml playbook but additionally compiles powerloop_main with
 # the DEBUG_POWERLOOP flag, swapping in bare setup()/loop() stubs so arbitrary

src/shared/constants.h

@@ -224,6 +224,9 @@ static const unsigned THUNDERLOOP_HZ = 300u;
 
 static const unsigned NUM_GENEVA_ANGLES = 5;
 
+
+static constexpr double RTT_S = 0.03;
+
 // Robot diagnostics constants
 constexpr double AUTO_CHIP_DISTANCE_DEFAULT_M     = 1.5;
 constexpr double AUTO_KICK_SPEED_DEFAULT_M_PER_S  = 1.5;

src/shared/robot_constants.h

@@ -116,6 +116,13 @@ struct RobotConstants
     // Found by sqrt(x^2 + y^2) of a wheel.
     // Front wheel arm = Rear wheel arm. See ASCII image above.
     float expected_lever_arm;
+
+    // Various variances for the robot localizer Kalman filter
+    float kalman_process_noise_variance_rad_per_s_4;
+
+    float kalman_vision_noise_variance_rad_2;
+
+    float kalman_motor_sensor_noise_variance_rad_per_s_2;
 };
 
 /**
@@ -163,7 +170,12 @@ constexpr RobotConstants createRobotConstants()
 
         .wheel_radius_meters = 0.03f,
 
-        .expected_lever_arm = 0.0749f};
+        .expected_lever_arm = 0.0749f,
+
+        // Kalman filter variances for robot localizer
+        .kalman_process_noise_variance_rad_per_s_4      = 1.0f,
+        .kalman_vision_noise_variance_rad_2             = 0.01f * 0.01f,
+        .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f};
 }
 #elif CHECK_VERSION(2021)
 constexpr RobotConstants createRobotConstants()
@@ -194,7 +206,12 @@ constexpr RobotConstants createRobotConstants()
         .robot_trajectory_max_ang_speed_rad_per_s = 7.0f,
         .robot_max_ang_acceleration_rad_per_s_2   = 30.0f,
 
-        .wheel_radius_meters = 0.03f};
+        .wheel_radius_meters = 0.03f,
+
+        // Kalman filter variances for robot localizer
+        .kalman_process_noise_variance_rad_per_s_4      = 0.5f,
+        .kalman_vision_noise_variance_rad_2             = 0.01f * 0.01f,
+        .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f * 0.5f};
 }
 #endif
 

src/software/ai/evaluation/intercept.cpp

@@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p
     Point best_position   = intercept_position;
     double final_speed    = step_speed;
     bool finished         = false;
-    double max_speed      = robot.robotConstants().robot_max_speed_m_per_s;
+    double max_speed      = robot.robotConstants().robot_trajectory_max_speed_m_per_s;
     double max_acc        = robot.robotConstants().robot_max_acceleration_m_per_s_2;
 
     while (!finished)

src/software/ai/hl/stp/play/ball_placement/BUILD

@@ -37,6 +37,8 @@ py_test(
     srcs = [
         "ball_placement_play_test.py",
     ],
+    # Flaky: multi-robot simulated play sensitive to sim timing; passes on retry.
+    flaky = True,
     deps = [
         "//software:conftest",
         "//software/gameplay_tests/validation:validations",

src/software/ai/hl/stp/play/kickoff_enemy/BUILD

@@ -29,6 +29,8 @@ cc_library(
 py_test(
     name = "kickoff_enemy_play_test",
     srcs = ["kickoff_enemy_play_test.py"],
+    # Flaky: multi-robot simulated play sensitive to sim timing; passes on retry.
+    flaky = True,
     deps = [
         "//software:conftest",
         "//software/gameplay_tests/validation:validations",

src/software/embedded/BUILD

@@ -1,7 +1,37 @@
-load("@bazel_skylib//rules:common_settings.bzl", "string_flag")
+load("@bazel_skylib//rules:common_settings.bzl", "bool_flag", "string_flag")
 
 package(default_visibility = ["//visibility:public"])
 
+# When enabled, compiles Thunderloop with the DISABLE_POWER_SERVICE flag, which
+# skips initializing and polling the PowerService so Thunderloop can be run on a
+# robot without a functioning powerboard.
+bool_flag(
+    name = "disable_power_service",
+    build_setting_default = False,
+)
+
+config_setting(
+    name = "disable_power_service_enabled",
+    flag_values = {
+        ":disable_power_service": "True",
+    },
+)
+
+# When enabled, compiles Thunderloop with the DISABLE_MOTOR_SERVICE flag, which
+# skips initializing and polling the MotorService so Thunderloop can be run on a
+# robot without a functioning motorboard.
+bool_flag(
+    name = "disable_motor_service",
+    build_setting_default = False,
+)
+
+config_setting(
+    name = "disable_motor_service_enabled",
+    flag_values = {
+        ":disable_motor_service": "True",
+    },
+)
+
 config_setting(
     name = "build_trinamic",
     flag_values = {
@@ -57,15 +87,24 @@ cc_library(
     name = "thunderloop",
     srcs = ["thunderloop.cpp"],
     hdrs = ["thunderloop.h"],
+    local_defines = select({
+        ":disable_power_service_enabled": ["DISABLE_POWER_SERVICE"],
+        "//conditions:default": [],
+    }) + select({
+        ":disable_motor_service_enabled": ["DISABLE_MOTOR_SERVICE"],
+        "//conditions:default": [],
+    }),
     deps = [
         ":primitive_executor",
         "//proto:tbots_cc_proto",
+        "//software/embedded:robot_localizer",
         "//software/embedded/services:imu",
         "//software/embedded/services:motor",
         "//software/embedded/services:power",
         "//software/embedded/services/network",
         "//software/embedded/toml_config",
         "//software/logger:network_logger",
+        "//software/physics:velocity_conversion_util",
         "//software/tracy:tracy_constants",
         "//software/util/scoped_timespec_timer",
         "@tracy",
@@ -108,3 +147,32 @@ cc_test(
         "//shared/test_util:tbots_gtest_main",
     ],
 )
+
+cc_library(
+    name = "robot_localizer",
+    srcs = ["robot_localizer.cpp"],
+    hdrs = ["robot_localizer.h"],
+    deps = [
+        "//proto:tbots_cc_proto",
+        "//software:constants",
+        "//software/embedded/services:imu",
+        "//software/geom:angle",
+        "//software/geom:angular_velocity",
+        "//software/geom:point",
+        "//software/geom:vector",
+        "//software/sensor_fusion/filter:kalman_filter",
+        "//software/util/scoped_timespec_timer",
+        "@eigen",
+    ],
+)
+
+cc_test(
+    name = "robot_localizer_test",
+    srcs = ["robot_localizer_test.cpp"],
+    deps = [
+        ":robot_localizer",
+        "//shared:constants",
+        "//shared/test_util:tbots_gtest_main",
+        "//software/physics:velocity_conversion_util",
+    ],
+)

src/software/embedded/linux_configs/pi/config.txt

@@ -3,8 +3,8 @@
 # Some settings may impact device functionality. See link above for details
 
 # Uncomment some or all of these to enable the optional hardware interfaces
-#dtparam=i2c_arm=on
-#dtparam=i2s=on
+dtparam=i2c_arm=on
+dtparam=i2s=on
 dtparam=spi=on
 
 # Enable audio (loads snd_bcm2835)

src/software/embedded/motion_control/BUILD

@@ -45,9 +45,6 @@ cc_library(
 
 cc_library(
     name = "pid_controller",
-    srcs = [
-        "pid_controller.cpp",
-    ],
     hdrs = [
         "pid_controller.h",
     ],

src/software/embedded/motion_control/orientation_controller.h

@@ -37,7 +37,7 @@ class OrientationController
 
    private:
     // TODO(#3737): tune constants
-    PidController w_pid_{0.4, 0.0, 0.0, 0.0};
+    PidController<double> w_pid_{0.4, 0.0, 0.0, 0.0};
 
     static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 5;
 };

src/software/embedded/motion_control/pid_controller.h

@@ -1,21 +1,25 @@
 #pragma once
 
+#include <algorithm>
+#include <cassert>
 #include <optional>
 
+/**
+ * A PID controller is used to calculate corrections based on error values over
+ * time as the difference between a desired value and the actual measured value.
+ * This PID controller also limits integral windup using a max_integral value.
+ *
+ * Resources:
+ * - https://raw.org/book/control-theory/introduction-to-pid-controllers/
+ * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/
+ */
+template <typename T>
 class PidController
 {
    public:
     /**
      * Constructs a new PID controller.
      *
-     * A PID controller is used to calculate corrections based on error values over
-     * time as the difference between a desired value and the actual measured value.
-     * This PID controller also limits integral windup using a max_integral value.
-     *
-     * Resources:
-     * - https://raw.org/book/control-theory/introduction-to-pid-controllers/
-     * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/
-     *
      * @pre max_integral must be >= 0.0
      *
      * @param k_p The proportional gain.
@@ -24,7 +28,11 @@ class PidController
      * @param max_integral The maximum absolute value that the integrator term can
      * accumulate to.
      **/
-    PidController(double k_p, double k_i, double k_d, double max_integral);
+    PidController(T k_p, T k_i, T k_d, T max_integral)
+        : k_p_(k_p), k_i_(k_i), k_d_(k_d), max_integral_(max_integral)
+    {
+        assert(max_integral >= T(0.0));
+    }
 
     /**
      * Given an error, returns the control effort to minimize it.
@@ -33,19 +41,39 @@ class PidController
      * @param delta_time The time passed since last step, for calculating the integrator
      * and derivative.
      **/
-    double step(double error, double delta_time);
+    T step(T error, T delta_time = T(1.0))
+    {
+        // If sign of error swaps, reset integrator
+        if (last_error_.has_value() && (last_error_.value() * error < T(0.0)))
+        {
+            integral_ = T(0.0);
+        }
+
+        integral_ =
+            std::clamp(integral_ + error * delta_time, -max_integral_, max_integral_);
+
+        const T derivative = (error - last_error_.value_or(error)) / delta_time;
+
+        last_error_ = error;
+
+        return error * k_p_ + integral_ * k_i_ + derivative * k_d_;
+    }
 
     /**
      * Resets the integrator and clears the last error used for derivative calculation.
      **/
-    void reset();
+    void reset()
+    {
+        integral_   = T(0.0);
+        last_error_ = std::nullopt;
+    }
 
    private:
-    double k_p_;
-    double k_i_;
-    double k_d_;
-    double max_integral_;
+    T k_p_;
+    T k_i_;
+    T k_d_;
+    T max_integral_;
 
-    double integral_                  = 0.0;
-    std::optional<double> last_error_ = std::nullopt;
+    T integral_                  = T(0.0);
+    std::optional<T> last_error_ = std::nullopt;
 };