From 61b340a9a69e4c6fa44600b33839328660a2f177 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Thu, 21 May 2026 16:21:09 -0700 Subject: [PATCH 01/28] Add PID controller class and basic test cases --- .../velocity_controller/pid_controller.cpp | 36 ++++++++ .../velocity_controller/pid_controller.h | 48 +++++++++++ .../pid_controller_test.cpp | 86 +++++++++++++++++++ 3 files changed, 170 insertions(+) create mode 100644 src/software/embedded/velocity_controller/pid_controller.cpp create mode 100644 src/software/embedded/velocity_controller/pid_controller.h create mode 100644 src/software/embedded/velocity_controller/pid_controller_test.cpp diff --git a/src/software/embedded/velocity_controller/pid_controller.cpp b/src/software/embedded/velocity_controller/pid_controller.cpp new file mode 100644 index 0000000000..7ecd6fc83a --- /dev/null +++ b/src/software/embedded/velocity_controller/pid_controller.cpp @@ -0,0 +1,36 @@ +#include "software/embedded/velocity_controller/pid_controller.h" + +#include +#include + +PidController::PidController(double k_p, double k_i, double k_d, double max_integral) + : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral) +{ + if (max_integral < 0.0) + { + throw std::invalid_argument("PidController max_integral must be >= 0.0"); + } +}; + +double PidController::step(double error, double delta_time) +{ + // if sign of error swaps, reset integrator + if (last_error_.value_or(0.0) * error <= 0.0) + { + integral_ = 0.0; + } + + integral_ = std::clamp(integral_ + error * delta_time, -max_integral, max_integral); + + const double derivative = (error - last_error_.value_or(error)) / delta_time; + + last_error_ = error; + + return error * k_p + integral_ * k_i + derivative * k_d; +} + +void PidController::reset() +{ + integral_ = 0.0; + last_error_ = std::nullopt; +} diff --git a/src/software/embedded/velocity_controller/pid_controller.h b/src/software/embedded/velocity_controller/pid_controller.h new file mode 100644 index 0000000000..cbe104b4ec --- /dev/null +++ b/src/software/embedded/velocity_controller/pid_controller.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +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 + * @throws std::invalid_argument if max_integral < 0.0 + **/ + PidController(double k_p, double k_i, double k_d, double max_integral); + + /** + * Given an error, returns the control effort to minimize it. + * + * @param error The amount of error between desired and actual output + * @param delta_time The time passed since last step, for calculating + * integrator and derivative. If this function is calling in invariant intervals, + * delta_time is by default set to 1 and any effects it would have can be handled + * by tuning k_i and k_d. + **/ + double step(double error, double delta_time = 1.0); + + /** + * Resets the integrator and clears the last error used for derivative calculation. + **/ + void reset(); + + double k_p; + double k_i; + double k_d; + double max_integral; + + private: + double integral_ = 0.0; + std::optional last_error_ = std::nullopt; +}; diff --git a/src/software/embedded/velocity_controller/pid_controller_test.cpp b/src/software/embedded/velocity_controller/pid_controller_test.cpp new file mode 100644 index 0000000000..96e15909e3 --- /dev/null +++ b/src/software/embedded/velocity_controller/pid_controller_test.cpp @@ -0,0 +1,86 @@ +#include "software/embedded/velocity_controller/pid_controller.h" + +#include + +TEST(PidControllerTest, OnlyProportionTermNonZero) +{ + PidController pid{1.0, 0.0, 0.0, 0.0}; + + EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(1.0), 1.0); + EXPECT_DOUBLE_EQ(pid.step(8.0), 8.0); + EXPECT_DOUBLE_EQ(pid.step(-5.0), -5.0); + EXPECT_DOUBLE_EQ(pid.step(-1.0), -1.0); + + pid = PidController{5.0, 0.0, 0.0, 0.0}; + + EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(1.0), 5.0); + EXPECT_DOUBLE_EQ(pid.step(8.0), 40.0); + EXPECT_DOUBLE_EQ(pid.step(-5.0), -25.0); + EXPECT_DOUBLE_EQ(pid.step(-1.0), -5.0); +} + +TEST(PidControllerTest, OnlyIntegralTermNonZero) +{ + constexpr double k_i = 2.0; + PidController pid{0.0, k_i, 0.0, 10.0}; + + EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 1.0); + EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 2.0); + EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 3.0); + EXPECT_DOUBLE_EQ(pid.step(0.5), k_i * 3.5); + + // switch error direction, integral term should reset + EXPECT_DOUBLE_EQ(pid.step(-0.2), k_i * -0.2); + EXPECT_DOUBLE_EQ(pid.step(-1.0), k_i * -1.2); + EXPECT_DOUBLE_EQ(pid.step(0.0), k_i * 0.0); + + // should not accumulate integral term above max_integral + EXPECT_DOUBLE_EQ(pid.step(9.0), k_i * 9.0); + EXPECT_DOUBLE_EQ(pid.step(5.0), k_i * 10.0); + EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 10.0); +} + +TEST(PidControllerTest, OnlyDerivativeTermNonZero) +{ + constexpr double k_d = 3.0; + PidController pid{0.0, 0.0, k_d, 0.0}; + + EXPECT_DOUBLE_EQ(pid.step(0.0), k_d * 0.0); + EXPECT_DOUBLE_EQ(pid.step(0.0), k_d * 0.0); + EXPECT_DOUBLE_EQ(pid.step(1.0), k_d * 1.0); + EXPECT_DOUBLE_EQ(pid.step(8.0), k_d * 7.0); + EXPECT_DOUBLE_EQ(pid.step(-5.0), k_d * -13.0); + EXPECT_DOUBLE_EQ(pid.step(0.0), k_d * 5.0); + EXPECT_DOUBLE_EQ(pid.step(5.0, 0.5), k_d * 5.0 / 0.5); + EXPECT_DOUBLE_EQ(pid.step(8.0, 2.0), k_d * 3.0 / 2.0); +} + +TEST(PidControllerTest, GeneralApplication) +{ + constexpr double k_p = 10.0; + constexpr double k_i = 3.0; + constexpr double k_d = -1.0; + constexpr double max_integral = 10.0; + PidController pid{k_p, k_i, k_d, max_integral}; + + EXPECT_DOUBLE_EQ(pid.step(12.0, 0.75), k_p * 12.0 + k_i * 9.0 + k_d * 0.0); + EXPECT_DOUBLE_EQ(pid.step(24.0, 0.75), k_p * 24.0 + k_i * 10.0 + k_d * 12.0 / 0.75); + EXPECT_DOUBLE_EQ(pid.step(4.0), k_p * 4.0 + k_i * 10.0 + k_d * -20.0); + EXPECT_DOUBLE_EQ(pid.step(0.0), k_p * 0.0 + k_i * 0.0 + k_d * -4.0); + EXPECT_DOUBLE_EQ(pid.step(2.0), k_p * 2.0 + k_i * 2.0 + k_d * 2.0); + EXPECT_DOUBLE_EQ(pid.step(-2.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0); +} + +TEST(PidControllerTest, InvalidArgumentsToConstructor) +{ + EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 9.0)); + EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 1.0)); + EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 0.0)); + EXPECT_THROW(PidController(0.0, 0.0, 0.0, -0.5), std::invalid_argument); + EXPECT_THROW(PidController(0.0, 0.0, 0.0, -1.0), std::invalid_argument); + EXPECT_THROW(PidController(0.0, 0.0, 0.0, -3.0), std::invalid_argument); +} From 682efb5d1486a18edf2102c98de910c9f013747c Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Thu, 21 May 2026 16:23:31 -0700 Subject: [PATCH 02/28] Rename directory to position_controller --- .../pid_controller.cpp | 2 +- .../pid_controller.h | 0 .../pid_controller_test.cpp | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename src/software/embedded/{velocity_controller => position_controller}/pid_controller.cpp (93%) rename src/software/embedded/{velocity_controller => position_controller}/pid_controller.h (100%) rename src/software/embedded/{velocity_controller => position_controller}/pid_controller_test.cpp (98%) diff --git a/src/software/embedded/velocity_controller/pid_controller.cpp b/src/software/embedded/position_controller/pid_controller.cpp similarity index 93% rename from src/software/embedded/velocity_controller/pid_controller.cpp rename to src/software/embedded/position_controller/pid_controller.cpp index 7ecd6fc83a..6665a1201c 100644 --- a/src/software/embedded/velocity_controller/pid_controller.cpp +++ b/src/software/embedded/position_controller/pid_controller.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/velocity_controller/pid_controller.h" +#include "software/embedded/position_controller/pid_controller.h" #include #include diff --git a/src/software/embedded/velocity_controller/pid_controller.h b/src/software/embedded/position_controller/pid_controller.h similarity index 100% rename from src/software/embedded/velocity_controller/pid_controller.h rename to src/software/embedded/position_controller/pid_controller.h diff --git a/src/software/embedded/velocity_controller/pid_controller_test.cpp b/src/software/embedded/position_controller/pid_controller_test.cpp similarity index 98% rename from src/software/embedded/velocity_controller/pid_controller_test.cpp rename to src/software/embedded/position_controller/pid_controller_test.cpp index 96e15909e3..251d9cf787 100644 --- a/src/software/embedded/velocity_controller/pid_controller_test.cpp +++ b/src/software/embedded/position_controller/pid_controller_test.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/velocity_controller/pid_controller.h" +#include "software/embedded/position_controller/pid_controller.h" #include From 60d50c59f81b18dd7f384225244527bcaf1273fb Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Fri, 22 May 2026 12:39:34 -0700 Subject: [PATCH 03/28] Create position controller class specs --- .../position_controller.cpp | 12 ++++++ .../position_controller/position_controller.h | 41 +++++++++++++++++++ .../position_controller_test.cpp | 7 ++++ 3 files changed, 60 insertions(+) create mode 100644 src/software/embedded/position_controller/position_controller.cpp create mode 100644 src/software/embedded/position_controller/position_controller.h create mode 100644 src/software/embedded/position_controller/position_controller_test.cpp diff --git a/src/software/embedded/position_controller/position_controller.cpp b/src/software/embedded/position_controller/position_controller.cpp new file mode 100644 index 0000000000..4e3fd9a274 --- /dev/null +++ b/src/software/embedded/position_controller/position_controller.cpp @@ -0,0 +1,12 @@ +#include "software/embedded/position_controller/position_controller.h" + +Vector PositionController::step(Vector error, double delta_time) +{ + // TODO + return Vector{}; +} + +void PositionController::reset() +{ + // TODO +} diff --git a/src/software/embedded/position_controller/position_controller.h b/src/software/embedded/position_controller/position_controller.h new file mode 100644 index 0000000000..9a99a8aac5 --- /dev/null +++ b/src/software/embedded/position_controller/position_controller.h @@ -0,0 +1,41 @@ +#pragma once + +#include "software/embedded/position_controller/pid_controller.h" +#include "software/geom/vector.h" + +// TODO: create angular velocity contorller as well + +class PositionController +{ + public: + /** + * Constructs a position controller that uses error over multiple time + * increments to calculate control effort to minimize it. + */ + PositionController() = default; + + /** + * Given an error, returns the control effort to minimize it. + * + * @param error The amount of error between desired and actual output + * in both x and y coordinate. + * @param delta_time The time passed since last time step. + */ + Vector step(Vector error, double delta_time = 1.0); + + /** + * Resets the state of this position controller. + */ + void reset(); + + private: + // TODO: tune constants + PidController x_pid_{0.8, 0.0, 0.0, 0.0}; + PidController y_pid_{0.8, 0.0, 0.0, 0.0}; + + PidController x_pid_close_{2.0, 0.0, 0.0, 0.0}; + PidController y_pid_close_{2.0, 0.0, 0.0, 0.0}; + + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25; +}; diff --git a/src/software/embedded/position_controller/position_controller_test.cpp b/src/software/embedded/position_controller/position_controller_test.cpp new file mode 100644 index 0000000000..473721bf69 --- /dev/null +++ b/src/software/embedded/position_controller/position_controller_test.cpp @@ -0,0 +1,7 @@ +#include "software/embedded/position_controller/position_controller.h" + +#include + +TEST(PositionControllerTest, TestConstructor) { + PositionController controller{}; +} From e80d76979140fe811f9293231645b76e21f8645f Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Fri, 22 May 2026 12:40:08 -0700 Subject: [PATCH 04/28] Add missing BUILD file --- .../embedded/position_controller/BUILD | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 src/software/embedded/position_controller/BUILD diff --git a/src/software/embedded/position_controller/BUILD b/src/software/embedded/position_controller/BUILD new file mode 100644 index 0000000000..d25897c4c9 --- /dev/null +++ b/src/software/embedded/position_controller/BUILD @@ -0,0 +1,46 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "position_controller", + srcs = [ + "position_controller.cpp" + ], + hdrs = [ + "position_controller.h" + ], + deps = [ + ":pid_controller" + ] +) + +cc_library( + name = "pid_controller", + srcs = [ + "pid_controller.cpp" + ], + hdrs = [ + "pid_controller.h" + ], +) + +cc_test( + name = "position_controller_test", + srcs = [ + "position_controller_test.cpp" + ], + deps = [ + ":position_controller", + "//shared/test_util:tbots_gtest_main", + ], +) + +cc_test( + name = "pid_controller_test", + srcs = [ + "pid_controller_test.cpp" + ], + deps = [ + ":pid_controller", + "//shared/test_util:tbots_gtest_main", + ], +) From e70f608e97328dd81d330cb06fad1d2e718c668c Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Fri, 22 May 2026 12:58:20 -0700 Subject: [PATCH 05/28] Fix BUILD file --- src/software/embedded/position_controller/BUILD | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/software/embedded/position_controller/BUILD b/src/software/embedded/position_controller/BUILD index d25897c4c9..9c26d5d1b0 100644 --- a/src/software/embedded/position_controller/BUILD +++ b/src/software/embedded/position_controller/BUILD @@ -3,30 +3,31 @@ package(default_visibility = ["//visibility:public"]) cc_library( name = "position_controller", srcs = [ - "position_controller.cpp" + "position_controller.cpp", ], hdrs = [ - "position_controller.h" + "position_controller.h", ], deps = [ - ":pid_controller" + ":pid_controller", + "//software/geom:vector", ] ) cc_library( name = "pid_controller", srcs = [ - "pid_controller.cpp" + "pid_controller.cpp", ], hdrs = [ - "pid_controller.h" + "pid_controller.h", ], ) cc_test( name = "position_controller_test", srcs = [ - "position_controller_test.cpp" + "position_controller_test.cpp", ], deps = [ ":position_controller", @@ -37,7 +38,7 @@ cc_test( cc_test( name = "pid_controller_test", srcs = [ - "pid_controller_test.cpp" + "pid_controller_test.cpp", ], deps = [ ":pid_controller", From c292a1dcd2fc1acd63d0651a339fdb3c6012d481 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Fri, 22 May 2026 13:03:48 -0700 Subject: [PATCH 06/28] Add position controller basic test cases --- .../position_controller_test.cpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/position_controller/position_controller_test.cpp b/src/software/embedded/position_controller/position_controller_test.cpp index 473721bf69..8747909dde 100644 --- a/src/software/embedded/position_controller/position_controller_test.cpp +++ b/src/software/embedded/position_controller/position_controller_test.cpp @@ -2,6 +2,24 @@ #include -TEST(PositionControllerTest, TestConstructor) { - PositionController controller{}; +TEST(PositionControllerTest, TestConstructor) +{ + PositionController controller{}; + + Vector error{-3.0, 1.0}; + Vector control_effort = controller.step(error); + + EXPECT_DOUBLE_EQ(error.x() * 0.8, control_effort.x()); + EXPECT_DOUBLE_EQ(error.y() * 0.8, control_effort.y()); +} + +TEST(PositionControllerTest, TestClosePid) +{ + PositionController controller{}; + + Vector error{0.005, 0.05}; + Vector control_effort = controller.step(error); + + EXPECT_DOUBLE_EQ(error.x() * 2.0, control_effort.x()); + EXPECT_DOUBLE_EQ(error.y() * 2.0, control_effort.y()); } From c8e822047cb42cbf271bf5d38d0acea18b366bfd Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Fri, 22 May 2026 13:04:03 -0700 Subject: [PATCH 07/28] Implement position controller --- .../position_controller.cpp | 20 +++++++++++++++---- .../position_controller/position_controller.h | 2 +- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/software/embedded/position_controller/position_controller.cpp b/src/software/embedded/position_controller/position_controller.cpp index 4e3fd9a274..81606644a8 100644 --- a/src/software/embedded/position_controller/position_controller.cpp +++ b/src/software/embedded/position_controller/position_controller.cpp @@ -1,12 +1,24 @@ #include "software/embedded/position_controller/position_controller.h" -Vector PositionController::step(Vector error, double delta_time) +Vector PositionController::step(const Vector& error, double delta_time) { - // TODO - return Vector{}; + // if close enough, use special PID to destination + if (error.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS) + { + return Vector{x_pid_close_.step(error.x(), delta_time), + y_pid_close_.step(error.y(), delta_time)}; + } + else + { + return Vector{x_pid_.step(error.x(), delta_time), + y_pid_.step(error.y(), delta_time)}; + } } void PositionController::reset() { - // TODO + x_pid_.reset(); + y_pid_.reset(); + x_pid_close_.reset(); + y_pid_close_.reset(); } diff --git a/src/software/embedded/position_controller/position_controller.h b/src/software/embedded/position_controller/position_controller.h index 9a99a8aac5..d11cf77b16 100644 --- a/src/software/embedded/position_controller/position_controller.h +++ b/src/software/embedded/position_controller/position_controller.h @@ -21,7 +21,7 @@ class PositionController * in both x and y coordinate. * @param delta_time The time passed since last time step. */ - Vector step(Vector error, double delta_time = 1.0); + Vector step(const Vector& error, double delta_time = 1.0); /** * Resets the state of this position controller. From 5e5ae5f362a4ee493303a6e256a68cbd157356c2 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 14:19:21 -0700 Subject: [PATCH 08/28] Re-write position controller for feedforward --- .../embedded/position_controller/BUILD | 2 +- .../position_controller.cpp | 24 +++++-- .../position_controller/position_controller.h | 12 ++-- .../position_controller_test.cpp | 64 +++++++++++++++---- 4 files changed, 78 insertions(+), 24 deletions(-) diff --git a/src/software/embedded/position_controller/BUILD b/src/software/embedded/position_controller/BUILD index 9c26d5d1b0..6f12d1f9a9 100644 --- a/src/software/embedded/position_controller/BUILD +++ b/src/software/embedded/position_controller/BUILD @@ -10,7 +10,7 @@ cc_library( ], deps = [ ":pid_controller", - "//software/geom:vector", + "//software/ai/navigator/trajectory:trajectory_path", ] ) diff --git a/src/software/embedded/position_controller/position_controller.cpp b/src/software/embedded/position_controller/position_controller.cpp index 81606644a8..39c456049c 100644 --- a/src/software/embedded/position_controller/position_controller.cpp +++ b/src/software/embedded/position_controller/position_controller.cpp @@ -1,17 +1,27 @@ #include "software/embedded/position_controller/position_controller.h" -Vector PositionController::step(const Vector& error, double delta_time) +Vector PositionController::step(const Point& position, const TrajectoryPath& target_path, + Duration time_since_trajectory_creation, + double delta_time) { - // if close enough, use special PID to destination - if (error.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS) + const Vector distance_from_destination = target_path.getDestination() - position; + + if (distance_from_destination.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS) { - return Vector{x_pid_close_.step(error.x(), delta_time), - y_pid_close_.step(error.y(), delta_time)}; + // if target destination is close enough, use pure PID for velocity + return Vector{x_pid_close_.step(distance_from_destination.x(), delta_time), + y_pid_close_.step(distance_from_destination.y(), delta_time)}; } else { - return Vector{x_pid_.step(error.x(), delta_time), - y_pid_.step(error.y(), delta_time)}; + // feedforward trajectory velocity with small pid control effort + const Vector error = + target_path.getPosition(time_since_trajectory_creation.toSeconds()) - + position; + const Vector control_effort{x_pid_.step(error.x(), delta_time), + y_pid_.step(error.y(), delta_time)}; + return target_path.getVelocity(time_since_trajectory_creation.toSeconds()) + + control_effort; } } diff --git a/src/software/embedded/position_controller/position_controller.h b/src/software/embedded/position_controller/position_controller.h index d11cf77b16..02e585364c 100644 --- a/src/software/embedded/position_controller/position_controller.h +++ b/src/software/embedded/position_controller/position_controller.h @@ -1,7 +1,10 @@ #pragma once +#include "software/ai/navigator/trajectory/trajectory_path.h" #include "software/embedded/position_controller/pid_controller.h" +#include "software/geom/point.h" #include "software/geom/vector.h" +#include "software/time/duration.h" // TODO: create angular velocity contorller as well @@ -15,13 +18,14 @@ class PositionController PositionController() = default; /** - * Given an error, returns the control effort to minimize it. + * Given an error, returns a target global velocity to minimize it. * - * @param error The amount of error between desired and actual output - * in both x and y coordinate. + * @param position The actual position + * @param target_path The target trajectory path * @param delta_time The time passed since last time step. */ - Vector step(const Vector& error, double delta_time = 1.0); + Vector step(const Point& position, const TrajectoryPath& target_path, + Duration time_since_trajectory_creation, double delta_time = 1.0); /** * Resets the state of this position controller. diff --git a/src/software/embedded/position_controller/position_controller_test.cpp b/src/software/embedded/position_controller/position_controller_test.cpp index 8747909dde..43fb4e4644 100644 --- a/src/software/embedded/position_controller/position_controller_test.cpp +++ b/src/software/embedded/position_controller/position_controller_test.cpp @@ -2,24 +2,64 @@ #include -TEST(PositionControllerTest, TestConstructor) +#include + +#include "software/ai/navigator/trajectory/bang_bang_trajectory_2d.h" +#include "software/ai/navigator/trajectory/trajectory_path.h" +#include "software/geom/point.h" + +class PositionControllerTest : public ::testing::Test +{ + protected: + PositionControllerTest() + : trajectory(std::make_shared()), + path(TrajectoryPath{trajectory, BangBangTrajectory2D::generator}) + { + } + + std::shared_ptr trajectory; + TrajectoryPath path; + PositionController controller; + + static constexpr double EPSILON = 1e-4; +}; + + +TEST_F(PositionControllerTest, PurePidControl) { - PositionController controller{}; + trajectory->generate({0.0, 0.0}, {5.0, 2.0}, {0.0, 0.0}, 1.0, 1e9, 1e9); + path = TrajectoryPath{trajectory, BangBangTrajectory2D::generator}; + + Vector target_velocity = + controller.step(Point{4.9, 2.1}, path, Duration::fromSeconds(1.0)); + + EXPECT_NEAR(2.0 * 0.1, target_velocity.x(), EPSILON); + EXPECT_NEAR(2.0 * -0.1, target_velocity.y(), EPSILON); +} + +TEST_F(PositionControllerTest, FeedforwardControl) +{ + trajectory->generate({0.0, 0.0}, {5.0, 0.0}, {0.0, 0.0}, 1.0, 0.45, 0.45); + path = TrajectoryPath{trajectory, BangBangTrajectory2D::generator}; + + Vector error = {3.0, 2.0}; - Vector error{-3.0, 1.0}; - Vector control_effort = controller.step(error); + Vector target_velocity = + controller.step(path.getPosition(2.0) + error, path, Duration::fromSeconds(2.0)); - EXPECT_DOUBLE_EQ(error.x() * 0.8, control_effort.x()); - EXPECT_DOUBLE_EQ(error.y() * 0.8, control_effort.y()); + EXPECT_NEAR(path.getVelocity(2.0).x() + 0.8 * -3.0, target_velocity.x(), EPSILON); + EXPECT_NEAR(0.8 * -2.0, target_velocity.y(), EPSILON); } -TEST(PositionControllerTest, TestClosePid) +TEST_F(PositionControllerTest, FeedforwardControlNoChange) { - PositionController controller{}; + trajectory->generate({0.0, 0.0}, {5.0, 0.0}, {0.0, 0.0}, 1.0, 1e9, + 1e9); // instant acceleration to 1ms + path = TrajectoryPath{trajectory, BangBangTrajectory2D::generator}; - Vector error{0.005, 0.05}; - Vector control_effort = controller.step(error); + Vector target_velocity = + controller.step(Point{2.0, 0.0}, path, Duration::fromSeconds(2.0)); - EXPECT_DOUBLE_EQ(error.x() * 2.0, control_effort.x()); - EXPECT_DOUBLE_EQ(error.y() * 2.0, control_effort.y()); + EXPECT_NEAR(1.0, target_velocity.x(), EPSILON); + EXPECT_NEAR(0.0, target_velocity.y(), EPSILON); } From c1e740b4dd533f1467f96a23dcf236fc61164349 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 14:23:06 -0700 Subject: [PATCH 09/28] Rename directory to motion_control --- .../pid_controller.cpp | 2 +- .../pid_controller.h | 0 .../pid_controller_test.cpp | 2 +- .../position_controller.cpp | 2 +- .../position_controller.h | 2 +- .../position_controller_test.cpp | 2 +- .../embedded/position_controller/BUILD | 47 ------------------- 7 files changed, 5 insertions(+), 52 deletions(-) rename src/software/embedded/{position_controller => motion_control}/pid_controller.cpp (92%) rename src/software/embedded/{position_controller => motion_control}/pid_controller.h (100%) rename src/software/embedded/{position_controller => motion_control}/pid_controller_test.cpp (97%) rename src/software/embedded/{position_controller => motion_control}/position_controller.cpp (94%) rename src/software/embedded/{position_controller => motion_control}/position_controller.h (95%) rename src/software/embedded/{position_controller => motion_control}/position_controller_test.cpp (96%) delete mode 100644 src/software/embedded/position_controller/BUILD diff --git a/src/software/embedded/position_controller/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp similarity index 92% rename from src/software/embedded/position_controller/pid_controller.cpp rename to src/software/embedded/motion_control/pid_controller.cpp index 6665a1201c..ce2aa809bc 100644 --- a/src/software/embedded/position_controller/pid_controller.cpp +++ b/src/software/embedded/motion_control/pid_controller.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/position_controller/pid_controller.h" +#include "software/embedded/motion_control/pid_controller.h" #include #include diff --git a/src/software/embedded/position_controller/pid_controller.h b/src/software/embedded/motion_control/pid_controller.h similarity index 100% rename from src/software/embedded/position_controller/pid_controller.h rename to src/software/embedded/motion_control/pid_controller.h diff --git a/src/software/embedded/position_controller/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp similarity index 97% rename from src/software/embedded/position_controller/pid_controller_test.cpp rename to src/software/embedded/motion_control/pid_controller_test.cpp index 251d9cf787..becac3cc90 100644 --- a/src/software/embedded/position_controller/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/position_controller/pid_controller.h" +#include "software/embedded/motion_control/pid_controller.h" #include diff --git a/src/software/embedded/position_controller/position_controller.cpp b/src/software/embedded/motion_control/position_controller.cpp similarity index 94% rename from src/software/embedded/position_controller/position_controller.cpp rename to src/software/embedded/motion_control/position_controller.cpp index 39c456049c..187d0f8acc 100644 --- a/src/software/embedded/position_controller/position_controller.cpp +++ b/src/software/embedded/motion_control/position_controller.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/position_controller/position_controller.h" +#include "software/embedded/motion_control/position_controller.h" Vector PositionController::step(const Point& position, const TrajectoryPath& target_path, Duration time_since_trajectory_creation, diff --git a/src/software/embedded/position_controller/position_controller.h b/src/software/embedded/motion_control/position_controller.h similarity index 95% rename from src/software/embedded/position_controller/position_controller.h rename to src/software/embedded/motion_control/position_controller.h index 02e585364c..7681feee70 100644 --- a/src/software/embedded/position_controller/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -1,7 +1,7 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory_path.h" -#include "software/embedded/position_controller/pid_controller.h" +#include "software/embedded/motion_control/pid_controller.h" #include "software/geom/point.h" #include "software/geom/vector.h" #include "software/time/duration.h" diff --git a/src/software/embedded/position_controller/position_controller_test.cpp b/src/software/embedded/motion_control/position_controller_test.cpp similarity index 96% rename from src/software/embedded/position_controller/position_controller_test.cpp rename to src/software/embedded/motion_control/position_controller_test.cpp index 43fb4e4644..c2441a8f17 100644 --- a/src/software/embedded/position_controller/position_controller_test.cpp +++ b/src/software/embedded/motion_control/position_controller_test.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/position_controller/position_controller.h" +#include "software/embedded/motion_control/position_controller.h" #include diff --git a/src/software/embedded/position_controller/BUILD b/src/software/embedded/position_controller/BUILD deleted file mode 100644 index 6f12d1f9a9..0000000000 --- a/src/software/embedded/position_controller/BUILD +++ /dev/null @@ -1,47 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "position_controller", - srcs = [ - "position_controller.cpp", - ], - hdrs = [ - "position_controller.h", - ], - deps = [ - ":pid_controller", - "//software/ai/navigator/trajectory:trajectory_path", - ] -) - -cc_library( - name = "pid_controller", - srcs = [ - "pid_controller.cpp", - ], - hdrs = [ - "pid_controller.h", - ], -) - -cc_test( - name = "position_controller_test", - srcs = [ - "position_controller_test.cpp", - ], - deps = [ - ":position_controller", - "//shared/test_util:tbots_gtest_main", - ], -) - -cc_test( - name = "pid_controller_test", - srcs = [ - "pid_controller_test.cpp", - ], - deps = [ - ":pid_controller", - "//shared/test_util:tbots_gtest_main", - ], -) From 9f40b42015cc6e0ba9dfb9e391df2bc561a78954 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 14:42:53 -0700 Subject: [PATCH 10/28] Add specification for OrientationController --- .../motion_control/orientation_controller.cpp | 8 +++++ .../motion_control/orientation_controller.h | 33 +++++++++++++++++++ .../orientation_controller_test.cpp | 7 ++++ .../motion_control/position_controller.h | 3 +- 4 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 src/software/embedded/motion_control/orientation_controller.cpp create mode 100644 src/software/embedded/motion_control/orientation_controller.h create mode 100644 src/software/embedded/motion_control/orientation_controller_test.cpp diff --git a/src/software/embedded/motion_control/orientation_controller.cpp b/src/software/embedded/motion_control/orientation_controller.cpp new file mode 100644 index 0000000000..788c64796f --- /dev/null +++ b/src/software/embedded/motion_control/orientation_controller.cpp @@ -0,0 +1,8 @@ +#include "software/embedded/motion_control/orientation_controller.h" + + +AngularVelocity OrientationController::step(Angle orientation, Angle target_orientation, + double delta_time) +{ + return AngularVelocity::zero(); // TODO +} diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h new file mode 100644 index 0000000000..603f8a9853 --- /dev/null +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -0,0 +1,33 @@ +#pragma once + +#include "software/embedded/motion_control/pid_controller.h" +#include "software/geom/angle.h" +#include "software/geom/angular_velocity.h" + +class OrientationController +{ + public: + /** + * Constructs an orientation controller that uses measurements over multiple + * time intervals to calculate the target angular velocity to minimize error. + */ + OrientationController() = default; + + /** + * Given an orientation and target orientation, returns a target angular + * velocity to minimize the error between the two. + * + * @param orientation The actual orientation. + * @param target_orientation The target trajectory path. + * @param delta_time The time passed since last time step. + */ + AngularVelocity step(Angle orientation, Angle target_orientation, + double delta_time = 1.0); + + private: + // TODO: tune constants + PidController w_pid_{0.7, 0.0, 2.0, 0.0}; + PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; + + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; +}; diff --git a/src/software/embedded/motion_control/orientation_controller_test.cpp b/src/software/embedded/motion_control/orientation_controller_test.cpp new file mode 100644 index 0000000000..567c2792d4 --- /dev/null +++ b/src/software/embedded/motion_control/orientation_controller_test.cpp @@ -0,0 +1,7 @@ +#include + +TEST(OrientationControllerTest, PurePidControl) {} + +TEST(OrientationControllerTest, FeedforwardControl) {} + +TEST(OrientationControllerTest, FeedforwardControlNoChange) {} diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index 7681feee70..5252d4a209 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -40,6 +40,5 @@ class PositionController PidController x_pid_close_{2.0, 0.0, 0.0, 0.0}; PidController y_pid_close_{2.0, 0.0, 0.0, 0.0}; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; }; From 5946a6e1f0bfc584e8b5da29fe234602f76d0ce4 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:12:19 -0700 Subject: [PATCH 11/28] Implement OrientationController --- .../motion_control/orientation_controller.cpp | 28 +++++++++++++++++-- .../motion_control/orientation_controller.h | 6 +++- 2 files changed, 30 insertions(+), 4 deletions(-) diff --git a/src/software/embedded/motion_control/orientation_controller.cpp b/src/software/embedded/motion_control/orientation_controller.cpp index 788c64796f..c1cc8857ff 100644 --- a/src/software/embedded/motion_control/orientation_controller.cpp +++ b/src/software/embedded/motion_control/orientation_controller.cpp @@ -1,8 +1,30 @@ #include "software/embedded/motion_control/orientation_controller.h" -AngularVelocity OrientationController::step(Angle orientation, Angle target_orientation, - double delta_time) +AngularVelocity OrientationController::step( + Angle orientation, const BangBangTrajectory1DAngular& angular_trajectory, + Duration time_since_trajectory_creation, double delta_time) { - return AngularVelocity::zero(); // TODO + const Angle difference_from_target = + (angular_trajectory.getDestination() - orientation).clamp(); + + if (difference_from_target.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES) + { + // if target orientation is close enough, use pure PID for angular velocity + return AngularVelocity::fromRadians( + w_pid_close_.step(difference_from_target.toRadians(), delta_time)); + } + else + { + // feedforward trajectory angular velocity with small pid control effort + const Angle error_angular = + (angular_trajectory.getPosition(time_since_trajectory_creation.toSeconds()) - + orientation) + .clamp(); + const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians( + w_pid_.step(error_angular.toRadians(), delta_time)); + return angular_trajectory.getVelocity( + time_since_trajectory_creation.toSeconds()) + + pid_effort_angular; + } } diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 603f8a9853..136df4e9be 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -1,8 +1,10 @@ #pragma once +#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/embedded/motion_control/pid_controller.h" #include "software/geom/angle.h" #include "software/geom/angular_velocity.h" +#include "software/time/duration.h" class OrientationController { @@ -21,7 +23,9 @@ class OrientationController * @param target_orientation The target trajectory path. * @param delta_time The time passed since last time step. */ - AngularVelocity step(Angle orientation, Angle target_orientation, + AngularVelocity step(Angle orientation, + const BangBangTrajectory1DAngular& angular_trajectory, + Duration time_since_trajectory_creation, double delta_time = 1.0); private: From 5b4c5f79e25f8390ee12671cbd948052fa9b4b03 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:12:46 -0700 Subject: [PATCH 12/28] Remove orientation and position controller tests --- .../orientation_controller_test.cpp | 7 -- .../position_controller_test.cpp | 65 ------------------- 2 files changed, 72 deletions(-) delete mode 100644 src/software/embedded/motion_control/orientation_controller_test.cpp delete mode 100644 src/software/embedded/motion_control/position_controller_test.cpp diff --git a/src/software/embedded/motion_control/orientation_controller_test.cpp b/src/software/embedded/motion_control/orientation_controller_test.cpp deleted file mode 100644 index 567c2792d4..0000000000 --- a/src/software/embedded/motion_control/orientation_controller_test.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include - -TEST(OrientationControllerTest, PurePidControl) {} - -TEST(OrientationControllerTest, FeedforwardControl) {} - -TEST(OrientationControllerTest, FeedforwardControlNoChange) {} diff --git a/src/software/embedded/motion_control/position_controller_test.cpp b/src/software/embedded/motion_control/position_controller_test.cpp deleted file mode 100644 index c2441a8f17..0000000000 --- a/src/software/embedded/motion_control/position_controller_test.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include "software/embedded/motion_control/position_controller.h" - -#include - -#include - -#include "software/ai/navigator/trajectory/bang_bang_trajectory_2d.h" -#include "software/ai/navigator/trajectory/trajectory_path.h" -#include "software/geom/point.h" - -class PositionControllerTest : public ::testing::Test -{ - protected: - PositionControllerTest() - : trajectory(std::make_shared()), - path(TrajectoryPath{trajectory, BangBangTrajectory2D::generator}) - { - } - - std::shared_ptr trajectory; - TrajectoryPath path; - PositionController controller; - - static constexpr double EPSILON = 1e-4; -}; - - -TEST_F(PositionControllerTest, PurePidControl) -{ - trajectory->generate({0.0, 0.0}, {5.0, 2.0}, {0.0, 0.0}, 1.0, 1e9, 1e9); - path = TrajectoryPath{trajectory, BangBangTrajectory2D::generator}; - - Vector target_velocity = - controller.step(Point{4.9, 2.1}, path, Duration::fromSeconds(1.0)); - - EXPECT_NEAR(2.0 * 0.1, target_velocity.x(), EPSILON); - EXPECT_NEAR(2.0 * -0.1, target_velocity.y(), EPSILON); -} - -TEST_F(PositionControllerTest, FeedforwardControl) -{ - trajectory->generate({0.0, 0.0}, {5.0, 0.0}, {0.0, 0.0}, 1.0, 0.45, 0.45); - path = TrajectoryPath{trajectory, BangBangTrajectory2D::generator}; - - Vector error = {3.0, 2.0}; - - Vector target_velocity = - controller.step(path.getPosition(2.0) + error, path, Duration::fromSeconds(2.0)); - - EXPECT_NEAR(path.getVelocity(2.0).x() + 0.8 * -3.0, target_velocity.x(), EPSILON); - EXPECT_NEAR(0.8 * -2.0, target_velocity.y(), EPSILON); -} - -TEST_F(PositionControllerTest, FeedforwardControlNoChange) -{ - trajectory->generate({0.0, 0.0}, {5.0, 0.0}, {0.0, 0.0}, 1.0, 1e9, - 1e9); // instant acceleration to 1ms - path = TrajectoryPath{trajectory, BangBangTrajectory2D::generator}; - - Vector target_velocity = - controller.step(Point{2.0, 0.0}, path, Duration::fromSeconds(2.0)); - - EXPECT_NEAR(1.0, target_velocity.x(), EPSILON); - EXPECT_NEAR(0.0, target_velocity.y(), EPSILON); -} From 2f0f8af683b12f699607438ea2cd7c0f6fe23971 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:13:31 -0700 Subject: [PATCH 13/28] Add motion_control BUILD file --- src/software/embedded/motion_control/BUILD | 74 ++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 src/software/embedded/motion_control/BUILD diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD new file mode 100644 index 0000000000..69dc2e1d93 --- /dev/null +++ b/src/software/embedded/motion_control/BUILD @@ -0,0 +1,74 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "position_controller", + srcs = [ + "position_controller.cpp", + ], + hdrs = [ + "position_controller.h", + ], + deps = [ + ":pid_controller", + "//software/ai/navigator/trajectory:trajectory_path", + ] +) + +cc_library( + name = "orientation_controller", + srcs = [ + "orientation_controller.cpp", + ], + hdrs = [ + "orientation_controller.h", + ], + deps = [ + ":pid_controller", + "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", + "//software/geom:angle", + "//software/geom:angular_velocity", + ] +) + +cc_library( + name = "pid_controller", + srcs = [ + "pid_controller.cpp", + ], + hdrs = [ + "pid_controller.h", + ], +) + +cc_test( + name = "position_controller_test", + srcs = [ + "position_controller_test.cpp", + ], + deps = [ + ":position_controller", + "//shared/test_util:tbots_gtest_main", + ], +) + +cc_test( + name = "pid_controller_test", + srcs = [ + "pid_controller_test.cpp", + ], + deps = [ + ":pid_controller", + "//shared/test_util:tbots_gtest_main", + ], +) + +cc_test( + name = "orientation_controller_test", + srcs = [ + "orientation_controller_test.cpp", + ], + deps = [ + ":orientation_controller", + "//shared/test_util:tbots_gtest_main", + ], +) From 14726e9a8492108cefce8351fa4e3bed79b4546f Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:15:46 -0700 Subject: [PATCH 14/28] Add reset() method --- .../embedded/motion_control/orientation_controller.cpp | 6 ++++++ .../embedded/motion_control/orientation_controller.h | 4 ++++ 2 files changed, 10 insertions(+) diff --git a/src/software/embedded/motion_control/orientation_controller.cpp b/src/software/embedded/motion_control/orientation_controller.cpp index c1cc8857ff..ae323d0d1c 100644 --- a/src/software/embedded/motion_control/orientation_controller.cpp +++ b/src/software/embedded/motion_control/orientation_controller.cpp @@ -28,3 +28,9 @@ AngularVelocity OrientationController::step( pid_effort_angular; } } + +void OrientationController::reset() +{ + w_pid_.reset(); + w_pid_close_.reset(); +} diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 136df4e9be..250988f86f 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -27,6 +27,10 @@ class OrientationController const BangBangTrajectory1DAngular& angular_trajectory, Duration time_since_trajectory_creation, double delta_time = 1.0); + /** + * Resets the state of this orientation controller. + */ + void reset(); private: // TODO: tune constants From 3a941c02e8cff40ed586562dd0d2be5686b4ff94 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:21:52 -0700 Subject: [PATCH 15/28] Update parameter names and doc comments --- .../motion_control/orientation_controller.cpp | 12 +++++------- .../motion_control/orientation_controller.h | 9 +++++---- .../motion_control/position_controller.cpp | 15 +++++++-------- .../motion_control/position_controller.h | 18 +++++++++--------- 4 files changed, 26 insertions(+), 28 deletions(-) diff --git a/src/software/embedded/motion_control/orientation_controller.cpp b/src/software/embedded/motion_control/orientation_controller.cpp index ae323d0d1c..cac6e7d476 100644 --- a/src/software/embedded/motion_control/orientation_controller.cpp +++ b/src/software/embedded/motion_control/orientation_controller.cpp @@ -2,11 +2,11 @@ AngularVelocity OrientationController::step( - Angle orientation, const BangBangTrajectory1DAngular& angular_trajectory, - Duration time_since_trajectory_creation, double delta_time) + Angle orientation, const BangBangTrajectory1DAngular& target_trajectory, + Duration elapsed_time, double delta_time) { const Angle difference_from_target = - (angular_trajectory.getDestination() - orientation).clamp(); + (target_trajectory.getDestination() - orientation).clamp(); if (difference_from_target.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES) { @@ -18,13 +18,11 @@ AngularVelocity OrientationController::step( { // feedforward trajectory angular velocity with small pid control effort const Angle error_angular = - (angular_trajectory.getPosition(time_since_trajectory_creation.toSeconds()) - - orientation) + (target_trajectory.getPosition(elapsed_time.toSeconds()) - orientation) .clamp(); const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians( w_pid_.step(error_angular.toRadians(), delta_time)); - return angular_trajectory.getVelocity( - time_since_trajectory_creation.toSeconds()) + + return target_trajectory.getVelocity(elapsed_time.toSeconds()) + pid_effort_angular; } } diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 250988f86f..171bab20f5 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -20,13 +20,14 @@ class OrientationController * velocity to minimize the error between the two. * * @param orientation The actual orientation. - * @param target_orientation The target trajectory path. + * @param target_trajectory The target angular trajectory. + * @param elapsed_time The elapsed time since the trajectory was created. * @param delta_time The time passed since last time step. */ AngularVelocity step(Angle orientation, - const BangBangTrajectory1DAngular& angular_trajectory, - Duration time_since_trajectory_creation, - double delta_time = 1.0); + const BangBangTrajectory1DAngular& target_trajectory, + Duration elapsed_time, double delta_time = 1.0); + /** * Resets the state of this orientation controller. */ diff --git a/src/software/embedded/motion_control/position_controller.cpp b/src/software/embedded/motion_control/position_controller.cpp index 187d0f8acc..3de7acfb4e 100644 --- a/src/software/embedded/motion_control/position_controller.cpp +++ b/src/software/embedded/motion_control/position_controller.cpp @@ -1,10 +1,11 @@ #include "software/embedded/motion_control/position_controller.h" -Vector PositionController::step(const Point& position, const TrajectoryPath& target_path, - Duration time_since_trajectory_creation, - double delta_time) +Vector PositionController::step(const Point& position, + const TrajectoryPath& target_trajectory, + Duration elapsed_time, double delta_time) { - const Vector distance_from_destination = target_path.getDestination() - position; + const Vector distance_from_destination = + target_trajectory.getDestination() - position; if (distance_from_destination.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS) { @@ -16,12 +17,10 @@ Vector PositionController::step(const Point& position, const TrajectoryPath& tar { // feedforward trajectory velocity with small pid control effort const Vector error = - target_path.getPosition(time_since_trajectory_creation.toSeconds()) - - position; + target_trajectory.getPosition(elapsed_time.toSeconds()) - position; const Vector control_effort{x_pid_.step(error.x(), delta_time), y_pid_.step(error.y(), delta_time)}; - return target_path.getVelocity(time_since_trajectory_creation.toSeconds()) + - control_effort; + return target_trajectory.getVelocity(elapsed_time.toSeconds()) + control_effort; } } diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index 5252d4a209..4211ed6418 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -6,26 +6,26 @@ #include "software/geom/vector.h" #include "software/time/duration.h" -// TODO: create angular velocity contorller as well - class PositionController { public: /** - * Constructs a position controller that uses error over multiple time - * increments to calculate control effort to minimize it. + * Constructs a position controller that uses measurements over multiple time + * intervals to calculate the target velocity to minimize error. */ PositionController() = default; /** - * Given an error, returns a target global velocity to minimize it. + * Given a position and target trajectory, returns a target global velocity to + * minimize error between the two. * - * @param position The actual position - * @param target_path The target trajectory path + * @param position The actual position. + * @param target_trajectory The target 2D trajectory path. + * @param elapsed_time The elapsed time since the trajectory was created. * @param delta_time The time passed since last time step. */ - Vector step(const Point& position, const TrajectoryPath& target_path, - Duration time_since_trajectory_creation, double delta_time = 1.0); + Vector step(const Point& position, const TrajectoryPath& target_trajectory, + Duration elapsed_time, double delta_time = 1.0); /** * Resets the state of this position controller. From 952652392f78f9ce72e94a99c3cbdce0df025a43 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:27:33 -0700 Subject: [PATCH 16/28] Remove old tests --- src/software/embedded/motion_control/BUILD | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD index 69dc2e1d93..c8a1f71736 100644 --- a/src/software/embedded/motion_control/BUILD +++ b/src/software/embedded/motion_control/BUILD @@ -40,17 +40,6 @@ cc_library( ], ) -cc_test( - name = "position_controller_test", - srcs = [ - "position_controller_test.cpp", - ], - deps = [ - ":position_controller", - "//shared/test_util:tbots_gtest_main", - ], -) - cc_test( name = "pid_controller_test", srcs = [ @@ -61,14 +50,3 @@ cc_test( "//shared/test_util:tbots_gtest_main", ], ) - -cc_test( - name = "orientation_controller_test", - srcs = [ - "orientation_controller_test.cpp", - ], - deps = [ - ":orientation_controller", - "//shared/test_util:tbots_gtest_main", - ], -) From 51081c9602314ccfbce916314150bb536517636f Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:29:34 -0700 Subject: [PATCH 17/28] Tag issue in TODO --- src/software/embedded/motion_control/orientation_controller.h | 2 +- src/software/embedded/motion_control/position_controller.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 171bab20f5..64100a961a 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -34,7 +34,7 @@ class OrientationController void reset(); private: - // TODO: tune constants + // TODO(#3737): tune constants PidController w_pid_{0.7, 0.0, 2.0, 0.0}; PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index 4211ed6418..a3625db0e1 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -33,7 +33,7 @@ class PositionController void reset(); private: - // TODO: tune constants + // TODO(#3737): tune constants PidController x_pid_{0.8, 0.0, 0.0, 0.0}; PidController y_pid_{0.8, 0.0, 0.0, 0.0}; From 77bdceecf7b8020317ae9d43a796574e713b42b9 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 15:32:54 -0700 Subject: [PATCH 18/28] Formatting --- src/software/embedded/motion_control/BUILD | 4 ++-- .../embedded/motion_control/orientation_controller.cpp | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD index c8a1f71736..9dfec056e4 100644 --- a/src/software/embedded/motion_control/BUILD +++ b/src/software/embedded/motion_control/BUILD @@ -11,7 +11,7 @@ cc_library( deps = [ ":pid_controller", "//software/ai/navigator/trajectory:trajectory_path", - ] + ], ) cc_library( @@ -27,7 +27,7 @@ cc_library( "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/geom:angle", "//software/geom:angular_velocity", - ] + ], ) cc_library( diff --git a/src/software/embedded/motion_control/orientation_controller.cpp b/src/software/embedded/motion_control/orientation_controller.cpp index cac6e7d476..2ca772d18c 100644 --- a/src/software/embedded/motion_control/orientation_controller.cpp +++ b/src/software/embedded/motion_control/orientation_controller.cpp @@ -1,6 +1,5 @@ #include "software/embedded/motion_control/orientation_controller.h" - AngularVelocity OrientationController::step( Angle orientation, const BangBangTrajectory1DAngular& target_trajectory, Duration elapsed_time, double delta_time) From c27da7040cdc21d008c3f02e2b023b1c9e96fa61 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 20:37:29 -0700 Subject: [PATCH 19/28] Remove default argument for delta_time --- .../motion_control/orientation_controller.h | 2 +- .../embedded/motion_control/pid_controller.h | 8 +-- .../motion_control/pid_controller_test.cpp | 64 +++++++++---------- .../motion_control/position_controller.h | 2 +- 4 files changed, 37 insertions(+), 39 deletions(-) diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 64100a961a..d63df12569 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -26,7 +26,7 @@ class OrientationController */ AngularVelocity step(Angle orientation, const BangBangTrajectory1DAngular& target_trajectory, - Duration elapsed_time, double delta_time = 1.0); + Duration elapsed_time, double delta_time); /** * Resets the state of this orientation controller. diff --git a/src/software/embedded/motion_control/pid_controller.h b/src/software/embedded/motion_control/pid_controller.h index cbe104b4ec..51a6842c62 100644 --- a/src/software/embedded/motion_control/pid_controller.h +++ b/src/software/embedded/motion_control/pid_controller.h @@ -25,12 +25,10 @@ class PidController * Given an error, returns the control effort to minimize it. * * @param error The amount of error between desired and actual output - * @param delta_time The time passed since last step, for calculating - * integrator and derivative. If this function is calling in invariant intervals, - * delta_time is by default set to 1 and any effects it would have can be handled - * by tuning k_i and k_d. + * @param delta_time The time passed since last step, for calculating the integrator + * and derivative. **/ - double step(double error, double delta_time = 1.0); + double step(double error, double delta_time); /** * Resets the integrator and clears the last error used for derivative calculation. diff --git a/src/software/embedded/motion_control/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp index becac3cc90..dbbaf4389d 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -6,21 +6,21 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) { PidController pid{1.0, 0.0, 0.0, 0.0}; - EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); - EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); - EXPECT_DOUBLE_EQ(pid.step(1.0), 1.0); - EXPECT_DOUBLE_EQ(pid.step(8.0), 8.0); - EXPECT_DOUBLE_EQ(pid.step(-5.0), -5.0); - EXPECT_DOUBLE_EQ(pid.step(-1.0), -1.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), 1.0); + EXPECT_DOUBLE_EQ(pid.step(8.0, 1.0), 8.0); + EXPECT_DOUBLE_EQ(pid.step(-5.0, 1.0), -5.0); + EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), -1.0); pid = PidController{5.0, 0.0, 0.0, 0.0}; - EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); - EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0); - EXPECT_DOUBLE_EQ(pid.step(1.0), 5.0); - EXPECT_DOUBLE_EQ(pid.step(8.0), 40.0); - EXPECT_DOUBLE_EQ(pid.step(-5.0), -25.0); - EXPECT_DOUBLE_EQ(pid.step(-1.0), -5.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); + EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), 5.0); + EXPECT_DOUBLE_EQ(pid.step(8.0, 1.0), 40.0); + EXPECT_DOUBLE_EQ(pid.step(-5.0, 1.0), -25.0); + EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), -5.0); } TEST(PidControllerTest, OnlyIntegralTermNonZero) @@ -28,20 +28,20 @@ TEST(PidControllerTest, OnlyIntegralTermNonZero) constexpr double k_i = 2.0; PidController pid{0.0, k_i, 0.0, 10.0}; - EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 1.0); - EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 2.0); - EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 3.0); - EXPECT_DOUBLE_EQ(pid.step(0.5), k_i * 3.5); + EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 1.0); + EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 2.0); + EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 3.0); + EXPECT_DOUBLE_EQ(pid.step(0.5, 1.0), k_i * 3.5); // switch error direction, integral term should reset - EXPECT_DOUBLE_EQ(pid.step(-0.2), k_i * -0.2); - EXPECT_DOUBLE_EQ(pid.step(-1.0), k_i * -1.2); - EXPECT_DOUBLE_EQ(pid.step(0.0), k_i * 0.0); + EXPECT_DOUBLE_EQ(pid.step(-0.2, 1.0), k_i * -0.2); + EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), k_i * -1.2); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * 0.0); // should not accumulate integral term above max_integral - EXPECT_DOUBLE_EQ(pid.step(9.0), k_i * 9.0); - EXPECT_DOUBLE_EQ(pid.step(5.0), k_i * 10.0); - EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 10.0); + EXPECT_DOUBLE_EQ(pid.step(9.0, 1.0), k_i * 9.0); + EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0); + EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 10.0); } TEST(PidControllerTest, OnlyDerivativeTermNonZero) @@ -49,12 +49,12 @@ TEST(PidControllerTest, OnlyDerivativeTermNonZero) constexpr double k_d = 3.0; PidController pid{0.0, 0.0, k_d, 0.0}; - EXPECT_DOUBLE_EQ(pid.step(0.0), k_d * 0.0); - EXPECT_DOUBLE_EQ(pid.step(0.0), k_d * 0.0); - EXPECT_DOUBLE_EQ(pid.step(1.0), k_d * 1.0); - EXPECT_DOUBLE_EQ(pid.step(8.0), k_d * 7.0); - EXPECT_DOUBLE_EQ(pid.step(-5.0), k_d * -13.0); - EXPECT_DOUBLE_EQ(pid.step(0.0), k_d * 5.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 0.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 0.0); + EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_d * 1.0); + EXPECT_DOUBLE_EQ(pid.step(8.0, 1.0), k_d * 7.0); + EXPECT_DOUBLE_EQ(pid.step(-5.0, 1.0), k_d * -13.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 5.0); EXPECT_DOUBLE_EQ(pid.step(5.0, 0.5), k_d * 5.0 / 0.5); EXPECT_DOUBLE_EQ(pid.step(8.0, 2.0), k_d * 3.0 / 2.0); } @@ -69,10 +69,10 @@ TEST(PidControllerTest, GeneralApplication) EXPECT_DOUBLE_EQ(pid.step(12.0, 0.75), k_p * 12.0 + k_i * 9.0 + k_d * 0.0); EXPECT_DOUBLE_EQ(pid.step(24.0, 0.75), k_p * 24.0 + k_i * 10.0 + k_d * 12.0 / 0.75); - EXPECT_DOUBLE_EQ(pid.step(4.0), k_p * 4.0 + k_i * 10.0 + k_d * -20.0); - EXPECT_DOUBLE_EQ(pid.step(0.0), k_p * 0.0 + k_i * 0.0 + k_d * -4.0); - EXPECT_DOUBLE_EQ(pid.step(2.0), k_p * 2.0 + k_i * 2.0 + k_d * 2.0); - EXPECT_DOUBLE_EQ(pid.step(-2.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0); + EXPECT_DOUBLE_EQ(pid.step(4.0, 1.0), k_p * 4.0 + k_i * 10.0 + k_d * -20.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_p * 0.0 + k_i * 0.0 + k_d * -4.0); + EXPECT_DOUBLE_EQ(pid.step(2.0, 1.0), k_p * 2.0 + k_i * 2.0 + k_d * 2.0); + EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0); } TEST(PidControllerTest, InvalidArgumentsToConstructor) diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index a3625db0e1..d4544a8706 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -25,7 +25,7 @@ class PositionController * @param delta_time The time passed since last time step. */ Vector step(const Point& position, const TrajectoryPath& target_trajectory, - Duration elapsed_time, double delta_time = 1.0); + Duration elapsed_time, double delta_time); /** * Resets the state of this position controller. From f43580dbb4fc5690fa2ed7c9daf623043b8e8bc3 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 23 May 2026 20:41:08 -0700 Subject: [PATCH 20/28] Remove integrator reset on error sign swap --- .../embedded/motion_control/pid_controller.cpp | 6 ------ .../motion_control/pid_controller_test.cpp | 15 +++++++-------- 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/src/software/embedded/motion_control/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp index ce2aa809bc..436cfe1edf 100644 --- a/src/software/embedded/motion_control/pid_controller.cpp +++ b/src/software/embedded/motion_control/pid_controller.cpp @@ -14,12 +14,6 @@ PidController::PidController(double k_p, double k_i, double k_d, double max_inte double PidController::step(double error, double delta_time) { - // if sign of error swaps, reset integrator - if (last_error_.value_or(0.0) * error <= 0.0) - { - integral_ = 0.0; - } - integral_ = std::clamp(integral_ + error * delta_time, -max_integral, max_integral); const double derivative = (error - last_error_.value_or(error)) / delta_time; diff --git a/src/software/embedded/motion_control/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp index dbbaf4389d..3c05675aa3 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -33,13 +33,12 @@ TEST(PidControllerTest, OnlyIntegralTermNonZero) EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 3.0); EXPECT_DOUBLE_EQ(pid.step(0.5, 1.0), k_i * 3.5); - // switch error direction, integral term should reset - EXPECT_DOUBLE_EQ(pid.step(-0.2, 1.0), k_i * -0.2); - EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), k_i * -1.2); - EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * 0.0); + EXPECT_DOUBLE_EQ(pid.step(-0.2, 1.0), k_i * 3.3); + EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), k_i * 2.3); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * 2.3); // should not accumulate integral term above max_integral - EXPECT_DOUBLE_EQ(pid.step(9.0, 1.0), k_i * 9.0); + EXPECT_DOUBLE_EQ(pid.step(6.7, 1.0), k_i * 9.0); EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0); EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 10.0); } @@ -70,9 +69,9 @@ TEST(PidControllerTest, GeneralApplication) EXPECT_DOUBLE_EQ(pid.step(12.0, 0.75), k_p * 12.0 + k_i * 9.0 + k_d * 0.0); EXPECT_DOUBLE_EQ(pid.step(24.0, 0.75), k_p * 24.0 + k_i * 10.0 + k_d * 12.0 / 0.75); EXPECT_DOUBLE_EQ(pid.step(4.0, 1.0), k_p * 4.0 + k_i * 10.0 + k_d * -20.0); - EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_p * 0.0 + k_i * 0.0 + k_d * -4.0); - EXPECT_DOUBLE_EQ(pid.step(2.0, 1.0), k_p * 2.0 + k_i * 2.0 + k_d * 2.0); - EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0); + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_p * 0.0 + k_i * 10.0 + k_d * -4.0); + EXPECT_DOUBLE_EQ(pid.step(2.0, 1.0), k_p * 2.0 + k_i * 10.0 + k_d * 2.0); + EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * 8.0 + k_d * -4.0); } TEST(PidControllerTest, InvalidArgumentsToConstructor) From 9fb6c0b68a6a3c241cff017adb96ca5d910c0097 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 12:48:57 -0700 Subject: [PATCH 21/28] Add doc comment --- src/software/embedded/motion_control/pid_controller.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/software/embedded/motion_control/pid_controller.h b/src/software/embedded/motion_control/pid_controller.h index 51a6842c62..68ef42e4e2 100644 --- a/src/software/embedded/motion_control/pid_controller.h +++ b/src/software/embedded/motion_control/pid_controller.h @@ -18,6 +18,12 @@ class PidController * * @pre max_integral must be >= 0.0 * @throws std::invalid_argument if max_integral < 0.0 + * + * @param k_p The proportional gain. + * @param k_i The integral gain. + * @param k_d The derivative gain. + * @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); From 1ebd776cc0f1a430600311890fde071bbed48e48 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 12:54:07 -0700 Subject: [PATCH 22/28] Use length instead of lengthSquared checking for pure pid --- src/software/embedded/motion_control/position_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/motion_control/position_controller.cpp b/src/software/embedded/motion_control/position_controller.cpp index 3de7acfb4e..c0ddbb504a 100644 --- a/src/software/embedded/motion_control/position_controller.cpp +++ b/src/software/embedded/motion_control/position_controller.cpp @@ -7,7 +7,7 @@ Vector PositionController::step(const Point& position, const Vector distance_from_destination = target_trajectory.getDestination() - position; - if (distance_from_destination.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS) + if (distance_from_destination.length() < LINEAR_PURE_PID_THRESHOLD_METERS) { // if target destination is close enough, use pure PID for velocity return Vector{x_pid_close_.step(distance_from_destination.x(), delta_time), From 7705bb3c6d5bb5cd2d055285e103d0a2e8fcdfb5 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 13:44:17 -0700 Subject: [PATCH 23/28] Add generic motion controller interface --- src/software/embedded/motion_control/BUILD | 12 +++++++ .../embedded/motion_control/controller.h | 32 +++++++++++++++++++ .../motion_control/orientation_controller.cpp | 10 +++--- .../motion_control/orientation_controller.h | 8 +++-- .../motion_control/position_controller.cpp | 11 ++++--- .../motion_control/position_controller.h | 7 ++-- 6 files changed, 64 insertions(+), 16 deletions(-) create mode 100644 src/software/embedded/motion_control/controller.h diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD index 9dfec056e4..c0d2140d15 100644 --- a/src/software/embedded/motion_control/BUILD +++ b/src/software/embedded/motion_control/BUILD @@ -1,5 +1,15 @@ package(default_visibility = ["//visibility:public"]) +cc_library( + name = "controller", + hdrs = [ + "controller.h", + ], + deps = [ + "//software/time:duration", + ], +) + cc_library( name = "position_controller", srcs = [ @@ -9,6 +19,7 @@ cc_library( "position_controller.h", ], deps = [ + ":controller", ":pid_controller", "//software/ai/navigator/trajectory:trajectory_path", ], @@ -23,6 +34,7 @@ cc_library( "orientation_controller.h", ], deps = [ + ":controller", ":pid_controller", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/geom:angle", diff --git a/src/software/embedded/motion_control/controller.h b/src/software/embedded/motion_control/controller.h new file mode 100644 index 0000000000..6de0d30237 --- /dev/null +++ b/src/software/embedded/motion_control/controller.h @@ -0,0 +1,32 @@ +#include "software/time/duration.h" + +/** + * A generic interface for a motion controller. + * + * @tparam StateType The type of the system's current feedback state + * @tparam TrajectoryType The type of the state's trajectory + * @tparam OutputType The type of the calculated control effort + */ +template +class MotionController +{ + public: + /** + * Evaluates a single time step of the control loop and returns an output to minimize + * error between the current state and target trajectory. + * + * @param current_state The actual measured current state. + * @param target_trajectory The target trajectory of motion. + * @param elapsed_time The total elapsed time since the trajectory was created. + * @param delta_time The elapsed time since the last time step. + */ + virtual OutputType step(const StateType& current_state, + const TrajectoryType& target_trajectory, + Duration elapsed_time, Duration delta_time) = 0; + + /** + * Resets the internal state of the motion controller. For example, any + * accumulated error terms used for calculating control effort. + */ + virtual void reset() = 0; +}; diff --git a/src/software/embedded/motion_control/orientation_controller.cpp b/src/software/embedded/motion_control/orientation_controller.cpp index 2ca772d18c..661821ea0b 100644 --- a/src/software/embedded/motion_control/orientation_controller.cpp +++ b/src/software/embedded/motion_control/orientation_controller.cpp @@ -1,8 +1,8 @@ #include "software/embedded/motion_control/orientation_controller.h" AngularVelocity OrientationController::step( - Angle orientation, const BangBangTrajectory1DAngular& target_trajectory, - Duration elapsed_time, double delta_time) + const Angle& orientation, const BangBangTrajectory1DAngular& target_trajectory, + Duration elapsed_time, Duration delta_time) { const Angle difference_from_target = (target_trajectory.getDestination() - orientation).clamp(); @@ -10,8 +10,8 @@ AngularVelocity OrientationController::step( if (difference_from_target.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES) { // if target orientation is close enough, use pure PID for angular velocity - return AngularVelocity::fromRadians( - w_pid_close_.step(difference_from_target.toRadians(), delta_time)); + return AngularVelocity::fromRadians(w_pid_close_.step( + difference_from_target.toRadians(), delta_time.toSeconds())); } else { @@ -20,7 +20,7 @@ AngularVelocity OrientationController::step( (target_trajectory.getPosition(elapsed_time.toSeconds()) - orientation) .clamp(); const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians( - w_pid_.step(error_angular.toRadians(), delta_time)); + w_pid_.step(error_angular.toRadians(), delta_time.toSeconds())); return target_trajectory.getVelocity(elapsed_time.toSeconds()) + pid_effort_angular; } diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index d63df12569..700df6347f 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -1,12 +1,14 @@ #pragma once #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" +#include "software/embedded/motion_control/controller.h" #include "software/embedded/motion_control/pid_controller.h" #include "software/geom/angle.h" #include "software/geom/angular_velocity.h" #include "software/time/duration.h" class OrientationController + : public MotionController { public: /** @@ -24,14 +26,14 @@ class OrientationController * @param elapsed_time The elapsed time since the trajectory was created. * @param delta_time The time passed since last time step. */ - AngularVelocity step(Angle orientation, + AngularVelocity step(const Angle& orientation, const BangBangTrajectory1DAngular& target_trajectory, - Duration elapsed_time, double delta_time); + Duration elapsed_time, Duration delta_time) override; /** * Resets the state of this orientation controller. */ - void reset(); + void reset() override; private: // TODO(#3737): tune constants diff --git a/src/software/embedded/motion_control/position_controller.cpp b/src/software/embedded/motion_control/position_controller.cpp index c0ddbb504a..77c382dacc 100644 --- a/src/software/embedded/motion_control/position_controller.cpp +++ b/src/software/embedded/motion_control/position_controller.cpp @@ -2,7 +2,7 @@ Vector PositionController::step(const Point& position, const TrajectoryPath& target_trajectory, - Duration elapsed_time, double delta_time) + Duration elapsed_time, Duration delta_time) { const Vector distance_from_destination = target_trajectory.getDestination() - position; @@ -10,16 +10,17 @@ Vector PositionController::step(const Point& position, if (distance_from_destination.length() < LINEAR_PURE_PID_THRESHOLD_METERS) { // if target destination is close enough, use pure PID for velocity - return Vector{x_pid_close_.step(distance_from_destination.x(), delta_time), - y_pid_close_.step(distance_from_destination.y(), delta_time)}; + return Vector{ + x_pid_close_.step(distance_from_destination.x(), delta_time.toSeconds()), + y_pid_close_.step(distance_from_destination.y(), delta_time.toSeconds())}; } else { // feedforward trajectory velocity with small pid control effort const Vector error = target_trajectory.getPosition(elapsed_time.toSeconds()) - position; - const Vector control_effort{x_pid_.step(error.x(), delta_time), - y_pid_.step(error.y(), delta_time)}; + const Vector control_effort{x_pid_.step(error.x(), delta_time.toSeconds()), + y_pid_.step(error.y(), delta_time.toSeconds())}; return target_trajectory.getVelocity(elapsed_time.toSeconds()) + control_effort; } } diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index d4544a8706..d00fc98dd3 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -1,12 +1,13 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory_path.h" +#include "software/embedded/motion_control/orientation_controller.h" #include "software/embedded/motion_control/pid_controller.h" #include "software/geom/point.h" #include "software/geom/vector.h" #include "software/time/duration.h" -class PositionController +class PositionController : public MotionController { public: /** @@ -25,12 +26,12 @@ class PositionController * @param delta_time The time passed since last time step. */ Vector step(const Point& position, const TrajectoryPath& target_trajectory, - Duration elapsed_time, double delta_time); + Duration elapsed_time, Duration delta_time) override; /** * Resets the state of this position controller. */ - void reset(); + void reset() override; private: // TODO(#3737): tune constants From 997111260dbdd50d036bb7d29a99dd42f27dc818 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 14:00:04 -0700 Subject: [PATCH 24/28] Fix include --- src/software/embedded/motion_control/position_controller.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index d00fc98dd3..ca2f41a8d5 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -1,7 +1,7 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory_path.h" -#include "software/embedded/motion_control/orientation_controller.h" +#include "software/embedded/motion_control/controller.h" #include "software/embedded/motion_control/pid_controller.h" #include "software/geom/point.h" #include "software/geom/vector.h" From ddb2e79e252c890ad2f3e5fbb70d67901aa1fddc Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 14:00:38 -0700 Subject: [PATCH 25/28] Add test placeholders for position and orientation controller --- src/software/embedded/motion_control/BUILD | 22 +++++++++++++++++++ .../orientation_controller_test.cpp | 14 ++++++++++++ .../position_controller_test.cpp | 17 ++++++++++++++ 3 files changed, 53 insertions(+) create mode 100644 src/software/embedded/motion_control/orientation_controller_test.cpp create mode 100644 src/software/embedded/motion_control/position_controller_test.cpp diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD index c0d2140d15..7fae1e1cd9 100644 --- a/src/software/embedded/motion_control/BUILD +++ b/src/software/embedded/motion_control/BUILD @@ -62,3 +62,25 @@ cc_test( "//shared/test_util:tbots_gtest_main", ], ) + +cc_test( + name = "position_controller_test", + srcs = [ + "position_controller_test.cpp", + ], + deps = [ + ":position_controller", + "//shared/test_util:tbots_gtest_main", + ], +) + +cc_test( + name = "orientation_controller_test", + srcs = [ + "orientation_controller_test.cpp", + ], + deps = [ + ":orientation_controller", + "//shared/test_util:tbots_gtest_main", + ], +) diff --git a/src/software/embedded/motion_control/orientation_controller_test.cpp b/src/software/embedded/motion_control/orientation_controller_test.cpp new file mode 100644 index 0000000000..aece0f2d07 --- /dev/null +++ b/src/software/embedded/motion_control/orientation_controller_test.cpp @@ -0,0 +1,14 @@ +#include "software/embedded/motion_control/orientation_controller.h" + +#include + +// TODO(#3743): write proper tests after pid constants have been tuned + +TEST(OrientationControllerTest, BasicTest) +{ + OrientationController controller; + BangBangTrajectory1DAngular trajectory; + controller.step(Angle::zero(), trajectory, Duration::fromSeconds(1.0), + Duration::fromSeconds(0.01)); + controller.reset(); +} diff --git a/src/software/embedded/motion_control/position_controller_test.cpp b/src/software/embedded/motion_control/position_controller_test.cpp new file mode 100644 index 0000000000..c3527eee6f --- /dev/null +++ b/src/software/embedded/motion_control/position_controller_test.cpp @@ -0,0 +1,17 @@ +#include "software/embedded/motion_control/position_controller.h" + +#include + +#include + +// TODO(#3743): write proper tests after pid constants have been tuned + +TEST(PositionControllerTest, BasicTest) +{ + PositionController controller; + std::shared_ptr trajectory_ptr = std::make_shared(); + TrajectoryPath trajectory{trajectory_ptr, BangBangTrajectory2D::generator}; + controller.step(Point{}, trajectory, Duration::fromSeconds(1.0), + Duration::fromSeconds(0.01)); + controller.reset(); +} From 7e0c28f3447cdf78d1e8d0e937b37551ec59f447 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 17:28:32 -0700 Subject: [PATCH 26/28] Use assertion instead of throwing exception --- src/software/embedded/motion_control/pid_controller.cpp | 7 ++----- src/software/embedded/motion_control/pid_controller.h | 1 - 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/software/embedded/motion_control/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp index 436cfe1edf..b9de85e5e2 100644 --- a/src/software/embedded/motion_control/pid_controller.cpp +++ b/src/software/embedded/motion_control/pid_controller.cpp @@ -1,15 +1,12 @@ #include "software/embedded/motion_control/pid_controller.h" #include -#include +#include PidController::PidController(double k_p, double k_i, double k_d, double max_integral) : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral) { - if (max_integral < 0.0) - { - throw std::invalid_argument("PidController max_integral must be >= 0.0"); - } + assert(max_integral >= 0.0); }; double PidController::step(double error, double delta_time) diff --git a/src/software/embedded/motion_control/pid_controller.h b/src/software/embedded/motion_control/pid_controller.h index 68ef42e4e2..7a983a1987 100644 --- a/src/software/embedded/motion_control/pid_controller.h +++ b/src/software/embedded/motion_control/pid_controller.h @@ -17,7 +17,6 @@ class PidController * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ * * @pre max_integral must be >= 0.0 - * @throws std::invalid_argument if max_integral < 0.0 * * @param k_p The proportional gain. * @param k_i The integral gain. From 36cde8c41dc688eec93740548f8537a7f871fd82 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 21:05:12 -0700 Subject: [PATCH 27/28] Remove exception tests --- .../embedded/motion_control/pid_controller_test.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/software/embedded/motion_control/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp index 3c05675aa3..c7c0db12b3 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -73,13 +73,3 @@ TEST(PidControllerTest, GeneralApplication) EXPECT_DOUBLE_EQ(pid.step(2.0, 1.0), k_p * 2.0 + k_i * 10.0 + k_d * 2.0); EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * 8.0 + k_d * -4.0); } - -TEST(PidControllerTest, InvalidArgumentsToConstructor) -{ - EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 9.0)); - EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 1.0)); - EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 0.0)); - EXPECT_THROW(PidController(0.0, 0.0, 0.0, -0.5), std::invalid_argument); - EXPECT_THROW(PidController(0.0, 0.0, 0.0, -1.0), std::invalid_argument); - EXPECT_THROW(PidController(0.0, 0.0, 0.0, -3.0), std::invalid_argument); -} From bdff8eb5098d7cb8a9cbf054696badd14cb42a85 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Tue, 26 May 2026 21:09:19 -0700 Subject: [PATCH 28/28] Make pid controller gain variables private --- .../embedded/motion_control/pid_controller.cpp | 6 +++--- src/software/embedded/motion_control/pid_controller.h | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/software/embedded/motion_control/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp index b9de85e5e2..fe19d403dd 100644 --- a/src/software/embedded/motion_control/pid_controller.cpp +++ b/src/software/embedded/motion_control/pid_controller.cpp @@ -4,20 +4,20 @@ #include PidController::PidController(double k_p, double k_i, double k_d, double max_integral) - : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral) + : k_p_(k_p), k_i_(k_i), k_d_(k_d), max_integral_(max_integral) { assert(max_integral >= 0.0); }; double PidController::step(double error, double delta_time) { - integral_ = std::clamp(integral_ + error * delta_time, -max_integral, max_integral); + integral_ = std::clamp(integral_ + error * delta_time, -max_integral_, max_integral_); const double derivative = (error - last_error_.value_or(error)) / delta_time; last_error_ = error; - return error * k_p + integral_ * k_i + derivative * k_d; + return error * k_p_ + integral_ * k_i_ + derivative * k_d_; } void PidController::reset() diff --git a/src/software/embedded/motion_control/pid_controller.h b/src/software/embedded/motion_control/pid_controller.h index 7a983a1987..6282e41317 100644 --- a/src/software/embedded/motion_control/pid_controller.h +++ b/src/software/embedded/motion_control/pid_controller.h @@ -40,12 +40,12 @@ class PidController **/ void reset(); - double k_p; - double k_i; - double k_d; - double max_integral; - private: + double k_p_; + double k_i_; + double k_d_; + double max_integral_; + double integral_ = 0.0; std::optional last_error_ = std::nullopt; };