diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD new file mode 100644 index 0000000000..7fae1e1cd9 --- /dev/null +++ b/src/software/embedded/motion_control/BUILD @@ -0,0 +1,86 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "controller", + hdrs = [ + "controller.h", + ], + deps = [ + "//software/time:duration", + ], +) + +cc_library( + name = "position_controller", + srcs = [ + "position_controller.cpp", + ], + hdrs = [ + "position_controller.h", + ], + deps = [ + ":controller", + ":pid_controller", + "//software/ai/navigator/trajectory:trajectory_path", + ], +) + +cc_library( + name = "orientation_controller", + srcs = [ + "orientation_controller.cpp", + ], + hdrs = [ + "orientation_controller.h", + ], + deps = [ + ":controller", + ":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 = "pid_controller_test", + srcs = [ + "pid_controller_test.cpp", + ], + deps = [ + ":pid_controller", + "//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/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 new file mode 100644 index 0000000000..661821ea0b --- /dev/null +++ b/src/software/embedded/motion_control/orientation_controller.cpp @@ -0,0 +1,33 @@ +#include "software/embedded/motion_control/orientation_controller.h" + +AngularVelocity OrientationController::step( + const Angle& orientation, const BangBangTrajectory1DAngular& target_trajectory, + Duration elapsed_time, Duration delta_time) +{ + const Angle difference_from_target = + (target_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.toSeconds())); + } + else + { + // feedforward trajectory angular velocity with small pid control effort + const Angle error_angular = + (target_trajectory.getPosition(elapsed_time.toSeconds()) - orientation) + .clamp(); + const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians( + w_pid_.step(error_angular.toRadians(), delta_time.toSeconds())); + return target_trajectory.getVelocity(elapsed_time.toSeconds()) + + 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 new file mode 100644 index 0000000000..700df6347f --- /dev/null +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -0,0 +1,44 @@ +#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: + /** + * 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_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(const Angle& orientation, + const BangBangTrajectory1DAngular& target_trajectory, + Duration elapsed_time, Duration delta_time) override; + + /** + * Resets the state of this orientation controller. + */ + void reset() override; + + private: + // 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}; + + 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..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/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp new file mode 100644 index 0000000000..fe19d403dd --- /dev/null +++ b/src/software/embedded/motion_control/pid_controller.cpp @@ -0,0 +1,27 @@ +#include "software/embedded/motion_control/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) +{ + assert(max_integral >= 0.0); +}; + +double PidController::step(double error, double delta_time) +{ + 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/motion_control/pid_controller.h b/src/software/embedded/motion_control/pid_controller.h new file mode 100644 index 0000000000..6282e41317 --- /dev/null +++ b/src/software/embedded/motion_control/pid_controller.h @@ -0,0 +1,51 @@ +#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 + * + * @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); + + /** + * 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 the integrator + * and derivative. + **/ + double step(double error, double delta_time); + + /** + * Resets the integrator and clears the last error used for derivative calculation. + **/ + void reset(); + + private: + double k_p_; + double k_i_; + double k_d_; + double max_integral_; + + double integral_ = 0.0; + std::optional last_error_ = std::nullopt; +}; diff --git a/src/software/embedded/motion_control/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp new file mode 100644 index 0000000000..c7c0db12b3 --- /dev/null +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -0,0 +1,75 @@ +#include "software/embedded/motion_control/pid_controller.h" + +#include + +TEST(PidControllerTest, OnlyProportionTermNonZero) +{ + PidController pid{1.0, 0.0, 0.0, 0.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, 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) +{ + constexpr double k_i = 2.0; + PidController pid{0.0, k_i, 0.0, 10.0}; + + 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); + + 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(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); +} + +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, 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); +} + +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, 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 * 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); +} diff --git a/src/software/embedded/motion_control/position_controller.cpp b/src/software/embedded/motion_control/position_controller.cpp new file mode 100644 index 0000000000..77c382dacc --- /dev/null +++ b/src/software/embedded/motion_control/position_controller.cpp @@ -0,0 +1,34 @@ +#include "software/embedded/motion_control/position_controller.h" + +Vector PositionController::step(const Point& position, + const TrajectoryPath& target_trajectory, + Duration elapsed_time, Duration delta_time) +{ + const Vector distance_from_destination = + target_trajectory.getDestination() - 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.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.toSeconds()), + y_pid_.step(error.y(), delta_time.toSeconds())}; + return target_trajectory.getVelocity(elapsed_time.toSeconds()) + control_effort; + } +} + +void PositionController::reset() +{ + x_pid_.reset(); + y_pid_.reset(); + x_pid_close_.reset(); + y_pid_close_.reset(); +} diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h new file mode 100644 index 0000000000..ca2f41a8d5 --- /dev/null +++ b/src/software/embedded/motion_control/position_controller.h @@ -0,0 +1,45 @@ +#pragma once + +#include "software/ai/navigator/trajectory/trajectory_path.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" +#include "software/time/duration.h" + +class PositionController : public MotionController +{ + public: + /** + * Constructs a position controller that uses measurements over multiple time + * intervals to calculate the target velocity to minimize error. + */ + PositionController() = default; + + /** + * Given a position and target trajectory, returns a target global velocity to + * minimize error between the two. + * + * @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_trajectory, + Duration elapsed_time, Duration delta_time) override; + + /** + * Resets the state of this position controller. + */ + void reset() override; + + private: + // 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}; + + 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; +}; 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(); +}