From 42172bf57568c457aa61708e7330455f061ce514 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 00:33:23 -0700 Subject: [PATCH 01/46] init merge --- .../message_translation/tbots_protobuf.cpp | 16 +- .../message_translation/tbots_protobuf.h | 11 +- .../tbots_protobuf_test.cpp | 4 +- src/shared/constants.h | 3 + src/shared/robot_constants.h | 86 +++-- src/software/ai/evaluation/intercept.cpp | 2 +- .../ai/hl/stp/tactic/move_primitive.cpp | 2 +- src/software/ai/navigator/trajectory/BUILD | 1 + .../trajectory/bang_bang_trajectory_1d.h | 14 + .../bang_bang_trajectory_1d_angular.h | 13 + .../ai/navigator/trajectory/trajectory.hpp | 10 + .../ai/navigator/trajectory/trajectory_2d.h | 14 + src/software/constants.h | 2 +- src/software/embedded/BUILD | 23 ++ src/software/embedded/motion_control/BUILD | 3 - .../embedded/motion_control/controller.h | 2 + .../motion_control/orientation_controller.h | 4 +- .../motion_control/pid_controller.cpp | 27 -- .../embedded/motion_control/pid_controller.h | 62 +++- .../motion_control/pid_controller_test.cpp | 27 +- .../motion_control/position_controller.h | 8 +- .../stspin_motor_controller.cpp | 7 +- src/software/embedded/primitive_executor.cpp | 196 ++++++---- src/software/embedded/primitive_executor.h | 88 ++--- src/software/embedded/robot_localizer.cpp | 306 ++++++++++++++++ src/software/embedded/robot_localizer.h | 178 +++++++++ src/software/embedded/services/BUILD | 19 + src/software/embedded/spi_utils.h | 7 +- src/software/embedded/thunderloop.cpp | 345 +++++++++--------- src/software/embedded/thunderloop.h | 41 ++- .../field_tests/field_test_fixture.py | 120 ++++-- .../field_tests/movement_robot_field_test.py | 191 +++++----- src/software/logger/network_logger.cpp | 13 +- .../simulation/er_force_simulator.cpp | 75 ++-- src/software/simulation/er_force_simulator.h | 13 +- src/software/util/pid/BUILD | 8 + src/software/util/pid/pid_controller.hpp | 82 +++++ src/software/world/robot.cpp | 3 +- 38 files changed, 1423 insertions(+), 603 deletions(-) delete mode 100644 src/software/embedded/motion_control/pid_controller.cpp create mode 100644 src/software/embedded/robot_localizer.cpp create mode 100644 src/software/embedded/robot_localizer.h create mode 100644 src/software/util/pid/BUILD create mode 100644 src/software/util/pid/pid_controller.hpp diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 7c146b4c45..23e7d0c51c 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -425,7 +425,8 @@ std::unique_ptr createCostVisualization( } std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants) { double max_speed = convertMaxAllowedSpeedModeToMaxAllowedSpeed( @@ -449,8 +450,7 @@ std::optional createTrajectoryPathFromParams( } auto trajectory = std::make_shared( - createPoint(params.start_position()), initial_destination, initial_velocity, - constraints); + start_position, initial_destination, initial_velocity, constraints); TrajectoryPath trajectory_path(trajectory, BangBangTrajectory2D::generator); @@ -475,14 +475,14 @@ std::optional createTrajectoryPathFromParams( } BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants) { return BangBangTrajectory1DAngular( - createAngle(params.start_angle()), createAngle(params.final_angle()), - initial_velocity, - AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s), + start_angle, createAngle(params.final_angle()), initial_velocity, + AngularVelocity::fromRadians( + robot_constants.robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( @@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( switch (max_allowed_speed_mode) { case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT: - return robot_constants.robot_max_speed_m_per_s; + return robot_constants.robot_max_speed_trajectory_m_per_s; case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND: return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND - STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND; diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 5976da5fcc..2831857e02 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -238,25 +238,28 @@ std::unique_ptr createCostVisualization( * Generate a 2D Trajectory Path given 2D trajectory parameters * * @param params 2D Trajectory Path + * @param start_position Initial position to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return TrajectoryPath, or std::nullopt if the trajectory path could not be created * from the given parameters */ std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants); /** - * Generate an angular trajectory Path given angular trajectory proto parameters + * Generate an angular trajectory path given angular trajectory proto parameters * - * @param params angular Trajectory Path + * @param params Angular trajectory path + * @param start_angle Initial angle to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return Generate angular trajectory */ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 0f82de69d6..2735d57a3b 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -209,8 +209,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) *(params.add_sub_destinations()) = sub_destination_proto; } - auto converted_trajectory_path_opt = - createTrajectoryPathFromParams(params, initial_velocity, robot_constants); + auto converted_trajectory_path_opt = createTrajectoryPathFromParams( + params, createPoint(params.start_position()), initial_velocity, robot_constants); ASSERT_TRUE(converted_trajectory_path_opt.has_value()); TrajectoryPath converted_trajectory_path = converted_trajectory_path_opt.value(); diff --git a/src/shared/constants.h b/src/shared/constants.h index ed94c0cde9..e02cfb1b58 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -224,6 +224,9 @@ static const unsigned THUNDERLOOP_HZ = 300u; static const unsigned NUM_GENEVA_ANGLES = 5; + +static constexpr double RTT_S = 0.03; + // Robot diagnostics constants constexpr double AUTO_CHIP_DISTANCE_DEFAULT_M = 1.5; constexpr double AUTO_KICK_SPEED_DEFAULT_M_PER_S = 1.5; diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 00a77c94da..620861d8aa 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -21,21 +21,21 @@ struct RobotConstants // The angles are assumed to be symmetric for the left and right sides of the robot. // // FRONT OF ROBOT - // | ▲ X-Axis - // | / -------+--------- \ eric - // |A / .' │ '. \ ◄─── Front wheel - // | /.' │ .'.\ grayson - // |// │ . \\ samuel - // ; │ . Lever ; - // | │ . Arm | - // ◄─────┼──────────────┼ | - // Y-Axis| | - // ; ; - // |\\ // - // | \'. .'/ - // |B \ '. .' / ◄─── Back wheel - // | \ '-. _.-' / - // | ''------'' + // | ▲ X-Axis // + // | / -------+--------- \ // + // |A / .' │ '. \ ◄─── Front wheel // + // | /.' │ .'.\ // + // |// │ . |\ // + // ; │ . Lever ; // + // | │ . Arm | // + // ◄─────┼──────────────┼ | // + // Y-Axis| | // + // ; ; // + // |\\ // // + // | \'. .'/ // + // |B \ '. .' / ◄─── Back wheel // + // | \ '-. _.-' / // + // | ''------'' // // clang-format on @@ -88,6 +88,10 @@ struct RobotConstants // The maximum speed achievable by our robots, in metres per second [m/s] float robot_max_speed_m_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. [m/s] + float robot_max_speed_trajectory_m_per_s; + // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -97,6 +101,11 @@ struct RobotConstants // The maximum angular speed achievable by our robots [rad/s] float robot_max_ang_speed_rad_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. + // [rad/s] + float robot_max_ang_speed_trajectory_rad_per_s; + // The maximum angular acceleration achievable by our robots [rad/s^2] float robot_max_ang_acceleration_rad_per_s_2; @@ -107,6 +116,13 @@ struct RobotConstants // Found by sqrt(x^2 + y^2) of a wheel. // Front wheel arm = Rear wheel arm. See ASCII image above. float expected_lever_arm; + + // Various variances for the robot localizer + float kalman_process_noise_variance_rad_per_s_4; + + float kalman_vision_noise_variance_rad_2; + + float kalman_motor_sensor_noise_variance_rad_per_s_2; }; /** @@ -120,7 +136,7 @@ constexpr RobotConstants createRobotConstants() return { .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), .front_wheel_angle_deg = 32.0f, - .back_wheel_angle_deg = 44.0f, + .back_wheel_angle_deg = 46.0f, .fr_x_pos_meters = 0.03485f, .fr_y_pos_meters = -0.06632f, @@ -142,17 +158,24 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 2.0f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.0f, + .robot_max_speed_trajectory_m_per_s = 2.5f, + .robot_max_acceleration_m_per_s_2 = 2.0f, + .robot_max_deceleration_m_per_s_2 = 1.5f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 6.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, .wheel_radius_meters = 0.03f, - .expected_lever_arm = 0.0749f}; + .expected_lever_arm = 0.0749f, + + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 1.0f, + .kalman_vision_noise_variance_rad_2 = 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5}; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() @@ -173,15 +196,22 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 4.5f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.000f, + .robot_max_speed_trajectory_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f, - .wheel_radius_meters = 0.03f}; + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 0.5f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f * 0.5f}; } #endif diff --git a/src/software/ai/evaluation/intercept.cpp b/src/software/ai/evaluation/intercept.cpp index 457ffb0639..2c729dd516 100644 --- a/src/software/ai/evaluation/intercept.cpp +++ b/src/software/ai/evaluation/intercept.cpp @@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p Point best_position = intercept_position; double final_speed = step_speed; bool finished = false; - double max_speed = robot.robotConstants().robot_max_speed_m_per_s; + double max_speed = robot.robotConstants().robot_max_speed_trajectory_m_per_s; double max_acc = robot.robotConstants().robot_max_acceleration_m_per_s_2; while (!finished) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 7e748c2068..325da493a3 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -40,7 +40,7 @@ MovePrimitive::MovePrimitive( angular_trajectory.generate( robot.orientation(), final_angle, robot.angularVelocity(), AngularVelocity::fromRadians( - robot.robotConstants().robot_max_ang_speed_rad_per_s), + robot.robotConstants().robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( diff --git a/src/software/ai/navigator/trajectory/BUILD b/src/software/ai/navigator/trajectory/BUILD index abc4f5c82f..5b2ef42fb2 100644 --- a/src/software/ai/navigator/trajectory/BUILD +++ b/src/software/ai/navigator/trajectory/BUILD @@ -21,6 +21,7 @@ cc_library( deps = [ ":trajectory", "//software/geom:point", + "//software/geom/algorithms", ], ) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index d5a54bf7af..4095faea25 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "software/ai/navigator/trajectory/trajectory.hpp" @@ -103,6 +104,19 @@ class BangBangTrajectory1D : public Trajectory */ size_t getNumTrajectoryParts() const; + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different + * @return True if the trajectories are different, false otherwise + */ + bool isNew(const Trajectory& other, + double threshold) const override + { + return std::abs(getDestination() - other.getDestination()) > threshold; + } + private: /** * Generate a trapezoidal trajectory and fill in `trajectory_parts`. diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h index 2ffbff5ec2..859034685e 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h @@ -83,6 +83,19 @@ class BangBangTrajectory1DAngular */ double getTotalTime() const override; + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different in degrees. + * @return True if the trajectories are different, false otherwise + */ + bool isNew(const Trajectory& other, + double threshold) const override + { + return getDestination().minDiff(other.getDestination()).toDegrees() > threshold; + } + private: BangBangTrajectory1D trajectory; }; diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 4e7a7ee77e..57789684a3 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -59,4 +59,14 @@ class Trajectory { return getPosition(getTotalTime()); } + + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different + * @return True if the trajectories are different, false otherwise + */ + virtual bool isNew(const Trajectory& other, + double threshold) const = 0; }; diff --git a/src/software/ai/navigator/trajectory/trajectory_2d.h b/src/software/ai/navigator/trajectory/trajectory_2d.h index 9699f07c17..4f21a9a25b 100644 --- a/src/software/ai/navigator/trajectory/trajectory_2d.h +++ b/src/software/ai/navigator/trajectory/trajectory_2d.h @@ -1,6 +1,7 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory.hpp" +#include "software/geom/algorithms/distance.h" #include "software/geom/point.h" #include "software/geom/rectangle.h" @@ -14,4 +15,17 @@ class Trajectory2D : virtual public Trajectory * @return bounding boxes which this trajectory passes through */ virtual std::vector getBoundingBoxes() const = 0; + + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different + * @return True if the trajectories are different, false otherwise + */ + bool isNew(const Trajectory& other, + double threshold) const override + { + return distance(getDestination(), other.getDestination()) > threshold; + } }; diff --git a/src/software/constants.h b/src/software/constants.h index d1592e54b1..4e549fa4cf 100644 --- a/src/software/constants.h +++ b/src/software/constants.h @@ -53,7 +53,7 @@ const std::string ROBOT_KICK_EXP_COEFF_CONFIG_KEY = "kick_coeff"; const std::string ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY = "chip_pulse_width"; const std::string SSL_VISION_ADDRESS = "224.5.23.2"; -static constexpr unsigned int SSL_VISION_PORT = 10020; +static constexpr unsigned int SSL_VISION_PORT = 10006; const std::string SSL_REFEREE_ADDRESS = "224.5.23.1"; static constexpr unsigned int SSL_REFEREE_PORT = 10003; diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index e5bba4bd99..2ae80efcd7 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -37,6 +37,9 @@ cc_library( "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", + "//software/embedded/motion_control:orientation_controller", + "//software/embedded/motion_control:position_controller", + "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", ], @@ -58,12 +61,14 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded:robot_localizer", "//software/embedded/services:imu", "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", "//software/embedded/toml_config", "//software/logger:network_logger", + "//software/physics:velocity_conversion_util", "//software/tracy:tracy_constants", "//software/util/scoped_timespec_timer", "@tracy", @@ -101,3 +106,21 @@ cc_test( "//shared/test_util:tbots_gtest_main", ], ) + +cc_library( + name = "robot_localizer", + srcs = ["robot_localizer.cpp"], + hdrs = ["robot_localizer.h"], + deps = [ + "//proto:tbots_cc_proto", + "//software:constants", + "//software/embedded/services:imu", + "//software/geom:angle", + "//software/geom:angular_velocity", + "//software/geom:point", + "//software/geom:vector", + "//software/sensor_fusion/filter:kalman_filter", + "//software/util/scoped_timespec_timer", + "@eigen", + ], +) diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD index 7fae1e1cd9..715cb54839 100644 --- a/src/software/embedded/motion_control/BUILD +++ b/src/software/embedded/motion_control/BUILD @@ -44,9 +44,6 @@ cc_library( cc_library( name = "pid_controller", - srcs = [ - "pid_controller.cpp", - ], hdrs = [ "pid_controller.h", ], diff --git a/src/software/embedded/motion_control/controller.h b/src/software/embedded/motion_control/controller.h index 6de0d30237..6882618082 100644 --- a/src/software/embedded/motion_control/controller.h +++ b/src/software/embedded/motion_control/controller.h @@ -1,3 +1,5 @@ +#pragma once + #include "software/time/duration.h" /** diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 700df6347f..63b90ed6d1 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -37,8 +37,8 @@ class OrientationController 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}; + 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/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp deleted file mode 100644 index fe19d403dd..0000000000 --- a/src/software/embedded/motion_control/pid_controller.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#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 index 6282e41317..16c10b9393 100644 --- a/src/software/embedded/motion_control/pid_controller.h +++ b/src/software/embedded/motion_control/pid_controller.h @@ -1,21 +1,25 @@ #pragma once +#include +#include #include +/** + * A PID controller is used to calculate corrections based on error values over + * time as the difference between a desired value and the actual measured value. + * This PID controller also limits integral windup using a max_integral value. + * + * Resources: + * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ + * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ + */ +template class PidController { public: /** * Constructs a new PID controller. * - * A PID controller is used to calculate corrections based on error values over - * time as the difference between a desired value and the actual measured value. - * This PID controller also limits integral windup using a max_integral value. - * - * Resources: - * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ - * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ - * * @pre max_integral must be >= 0.0 * * @param k_p The proportional gain. @@ -24,7 +28,11 @@ class PidController * @param max_integral The maximum absolute value that the integrator term can * accumulate to. **/ - PidController(double k_p, double k_i, double k_d, double max_integral); + PidController(T k_p, T k_i, T k_d, T max_integral) + : k_p_(k_p), k_i_(k_i), k_d_(k_d), max_integral_(max_integral) + { + assert(max_integral >= T(0.0)); + } /** * Given an error, returns the control effort to minimize it. @@ -33,19 +41,39 @@ class PidController * @param delta_time The time passed since last step, for calculating the integrator * and derivative. **/ - double step(double error, double delta_time); + T step(T error, T delta_time = T(1.0)) + { + // If sign of error swaps, reset integrator + if (last_error_.has_value() && (last_error_.value() * error < T(0.0))) + { + integral_ = T(0.0); + } + + integral_ = + std::clamp(integral_ + error * delta_time, -max_integral_, max_integral_); + + const T derivative = (error - last_error_.value_or(error)) / delta_time; + + last_error_ = error; + + return error * k_p_ + integral_ * k_i_ + derivative * k_d_; + } /** * Resets the integrator and clears the last error used for derivative calculation. **/ - void reset(); + void reset() + { + integral_ = T(0.0); + last_error_ = std::nullopt; + } private: - double k_p_; - double k_i_; - double k_d_; - double max_integral_; + T k_p_; + T k_i_; + T k_d_; + T max_integral_; - double integral_ = 0.0; - std::optional last_error_ = std::nullopt; + T integral_ = T(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 index c7c0db12b3..307100d84a 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -4,7 +4,7 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) { - PidController pid{1.0, 0.0, 0.0, 0.0}; + 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); @@ -13,7 +13,7 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) 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}; + 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); @@ -26,27 +26,31 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) TEST(PidControllerTest, OnlyIntegralTermNonZero) { constexpr double k_i = 2.0; - PidController pid{0.0, k_i, 0.0, 10.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); + // Sign swap should reset integrator to 0, then add -0.2 + 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); + // Sign swap back to 0.0? No, 0.0 doesn't swap sign usually (0*X is 0, not < 0) + // My implementation: last_error_.value() * error < T(0.0) + // If error is 0, it's not < 0. + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * -1.2); // 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(6.7, 1.0), k_i * 5.5); // Sign not swapped from 0.0 to 6.7 + EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0); // Clamped (5.5 + 5.0 = 10.5 -> 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}; + 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); @@ -64,12 +68,13 @@ TEST(PidControllerTest, GeneralApplication) 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}; + 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); + // Sign swap reset: integral becomes 0 + -2.0*1.0 = -2.0 + EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0); } diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index ca2f41a8d5..12725e09d7 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -35,11 +35,11 @@ class PositionController : public MotionController 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}; + 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/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 1c29915b4d..7324b0a6a4 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -165,12 +165,7 @@ void StSpinMotorController::updateFaults(const MotorIndex motor, motor_faults.drive_enabled = false; } - LOG(WARNING) - << oss.str(); // Consider moving this to a helper and also making the stream - // object persistent so we do not need to continuously recreate it - // (it is notoriously expensive). We could also consider another - // approach to formatting this string as using streams in general - // is less than ideal. Or We can probably just use string::append + LOG(WARNING) << oss.str(); } int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d1f56161ee..9c1783ef07 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,13 +11,9 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, const RobotId robot_id) - : current_primitive_(), - friendly_team_colour_(friendly_team_colour), - robot_constants_(robot_constants), - time_step_(time_step), - robot_id_(robot_id) + const robot_constants::RobotConstants& robot_constants) + : last_step_time_(std::chrono::steady_clock::now()), + robot_constants_(robot_constants) { } @@ -27,82 +23,118 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (current_primitive_.has_move()) { - trajectory_path_ = createTrajectoryPathFromParams( - current_primitive_.move().xy_traj_params(), velocity_, robot_constants_); + const std::optional new_trajectory_path = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); - angular_trajectory_ = + const bool is_linear_traj_new = + (new_trajectory_path.has_value() != trajectory_path_.has_value()) || + (new_trajectory_path.has_value() && + trajectory_path_->isNew(*new_trajectory_path, + LINEAR_DESTINATION_THRESHOLD_METERS)); + + if (is_linear_traj_new) + { + trajectory_path_ = new_trajectory_path; + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); + position_controller_.reset(); + } + + const BangBangTrajectory1DAngular new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - angular_velocity_, robot_constants_); + orientation_, angular_velocity_, + robot_constants_); + + const bool is_angular_traj_new = + !angular_trajectory_.has_value() || + angular_trajectory_->isNew(new_angular_trajectory, + ANGULAR_DESTINATION_THRESHOLD_DEGREES); - time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + if (is_angular_traj_new) + { + angular_trajectory_ = new_angular_trajectory; + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); + orientation_controller_.reset(); + } } } -void PrimitiveExecutor::setStopPrimitive() +void PrimitiveExecutor::updateState(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity) { - current_primitive_ = *createStopPrimitiveProto(); -} + position_ = position; + velocity_ = velocity; + orientation_ = orientation; + angular_velocity_ = angular_velocity; + + // If we are lagging behind trajectory too much, we have stalled! We need to + // regenerate trajectory. + if (trajectory_path_.has_value()) + { + const double linear_following_error = + (position_ - trajectory_path_->getPosition( + time_since_linear_trajectory_creation_.count())) + .length(); -void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity) -{ - Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_); - velocity_ = actual_global_velocity; - angular_velocity_ = angular_velocity; -} + if (linear_following_error > LINEAR_STALL_ERROR_MAX_METERS) + { + // regenerate linear trajectory + trajectory_path_ = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); -Vector PrimitiveExecutor::getTargetLinearVelocity() -{ - Vector local_velocity = globalToLocalVelocity( - trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()), - orientation_); - Point position = - trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds()); - double distance_to_destination = - distance(position, trajectory_path_->getDestination()); - - // Dampen velocity as we get closer to the destination to reduce jittering - if (distance_to_destination < MAX_DAMPENING_VELOCITY_DISTANCE_M) - { - local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M; + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); + } } - return local_velocity; -} -AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() -{ - orientation_ = - angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds()); - - AngularVelocity angular_velocity = - angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds()); - Angle orientation_to_destination = - orientation_.minDiff(angular_trajectory_->getDestination()); - if (orientation_to_destination.toDegrees() < 5) + if (angular_trajectory_.has_value()) { - angular_velocity *= orientation_to_destination.toDegrees() / 5; - } + const double angular_following_error = + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.count()) + .minDiff(orientation_) + .toDegrees(); - return angular_velocity; -} + if (angular_following_error > ANGULAR_STALL_ERROR_MAX_DEGREES) + { + // regenerate angular trajectory + angular_trajectory_ = createAngularTrajectoryFromParams( + current_primitive_.move().w_traj_params(), orientation_, + angular_velocity_, robot_constants_); + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); + } + } +} std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status) { - time_since_trajectory_creation_ += time_step_; + const auto current_time = std::chrono::steady_clock::now(); + const auto delta_time_chrono = current_time - last_step_time_; + const Duration delta_time = Duration::fromSeconds( + std::chrono::duration(delta_time_chrono).count()); + + time_since_linear_trajectory_creation_ += delta_time_chrono; + time_since_angular_trajectory_creation_ += delta_time_chrono; + last_step_time_ = current_time; + status.set_running_primitive(true); switch (current_primitive_.primitive_case()) { case TbotsProto::Primitive::kStop: { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( - prim->direct_control()); + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); status.set_running_primitive(false); - return output; + return std::make_unique( + prim->direct_control()); } case TbotsProto::Primitive::kDirectControl: { @@ -113,40 +145,46 @@ std::unique_ptr PrimitiveExecutor::stepPrimi { if (!trajectory_path_.has_value() || !angular_trajectory_.has_value()) { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); + LOG(INFO) << "Not moving because trajectory_path_ has value " + << trajectory_path_.has_value() << " angular has " + << angular_trajectory_.has_value(); + return std::make_unique( prim->direct_control()); - LOG(INFO) - << "Not moving because trajectory_path_ or angular_trajectory_ is not set"; - return output; } - Vector local_velocity = getTargetLinearVelocity(); - AngularVelocity angular_velocity = getTargetAngularVelocity(); + const Duration elapsed_linear = Duration::fromSeconds( + time_since_linear_trajectory_creation_.count()); + const Duration elapsed_angular = Duration::fromSeconds( + time_since_angular_trajectory_creation_.count()); + + const Vector target_linear_velocity_global = position_controller_.step( + position_, *trajectory_path_, elapsed_linear, delta_time); + + const AngularVelocity target_angular_velocity = orientation_controller_.step( + orientation_, *angular_trajectory_, elapsed_angular, delta_time); + + Vector target_linear_velocity_local = + globalToLocalVelocity(target_linear_velocity_global, orientation_); + + // Make sure target linear velocity is clamped + target_linear_velocity_local = target_linear_velocity_local.normalize( + std::min(target_linear_velocity_local.length(), + static_cast(robot_constants_.robot_max_speed_m_per_s))); - auto output = createDirectControlPrimitive( - local_velocity, angular_velocity, + const auto prim = createDirectControlPrimitive( + target_linear_velocity_local, target_angular_velocity, convertDribblerModeToDribblerSpeed( current_primitive_.move().dribbler_mode(), robot_constants_), current_primitive_.move().auto_chip_or_kick()); return std::make_unique( - output->direct_control()); + prim->direct_control()); } case TbotsProto::Primitive::PRIMITIVE_NOT_SET: { - // TODO (#2283) Once we can add/remove robots, this log should - // be re-enabled. Right now it just gets spammed because we command - // 6 robots for Div B when there are 11 on the field. - // - // LOG(DEBUG) << "No primitive set!"; } } return std::make_unique(); } - -void PrimitiveExecutor::setRobotId(const RobotId robot_id) -{ - robot_id_ = robot_id; -} diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 145874f3c2..952816282a 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -5,96 +5,80 @@ #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/ai/navigator/trajectory/trajectory_path.h" #include "software/geom/vector.h" -#include "software/time/duration.h" #include "software/world/robot_state.h" -#include "software/world/team_types.h" +#include "software/embedded/motion_control/orientation_controller.h" +#include "software/embedded/motion_control/position_controller.h" class PrimitiveExecutor { public: /** * Constructor - * @param time_step Time step which this primitive executor operates in - * @param robot_constants The robot constants for the robot which uses this primitive - * executor - * @param friendly_team_colour The colour of the friendly team - * @param robot_id The id of the robot which uses this primitive executor + * + * @param robot_constants The robot constants for the robot */ - explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, - const RobotId robot_id); + explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive + * * @param primitive_msg The primitive to start */ void updatePrimitive(const TbotsProto::Primitive& primitive_msg); /** - * Set the current primitive to the stop primitive - */ - void setStopPrimitive(); - - /** - * Update primitive executor with the current velocity of the robot + * Update primitive executor with the current velocity and orientation of the robot * - * @param local_velocity The current _local_ velocity + * @param position The current position + * @param velocity The current velocity + * @param orientation The current orientation of the robot * @param angular_velocity The current angular velocity */ - void updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity); - - /** - * Set the robot id - * @param robot_id The id of the robot which uses this primitive executor - */ - void setRobotId(RobotId robot_id); + void updateState(const Point& position, const Vector& velocity, + const Angle& orientation, const AngularVelocity& angular_velocity); /** * Steps the current primitive and returns a direct control primitive with the * target wheel velocities * @param status The status of the primitive executor, set to false if current * primitive is a Stop primitive - * * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status); private: - /* - * Compute the next target linear _local_ velocity the robot should be at. - * @returns Vector The target linear _local_ velocity - */ - Vector getTargetLinearVelocity(); + TbotsProto::Primitive current_primitive_; - /* - * Returns the next target angular velocity the robot - * - * @returns AngularVelocity The target angular velocity - */ - AngularVelocity getTargetAngularVelocity(); + std::optional trajectory_path_; + std::optional angular_trajectory_; - TbotsProto::Primitive current_primitive_; - Duration time_since_trajectory_creation_; + std::chrono::time_point last_step_time_; + std::chrono::duration time_since_linear_trajectory_creation_; + std::chrono::duration time_since_angular_trajectory_creation_; + + Point position_; Vector velocity_; AngularVelocity angular_velocity_; Angle orientation_; - TeamColour friendly_team_colour_; + + Point last_position_; + Angle last_orientation_; + robot_constants::RobotConstants robot_constants_; - std::optional trajectory_path_; - std::optional angular_trajectory_; - // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant - // time step to be used, in Seconds - Duration time_step_; - RobotId robot_id_; + PositionController position_controller_; + OrientationController orientation_controller_; - // Estimated delay between a vision frame to AI processing to robot executing - static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; + // If distance between current linear trajectory destination and new one is larger + // than this, we change trajectories. + static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; + static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; - // The distance away from the destination at which we start dampening the velocity - // to avoid jittering around the destination. - static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; + // These constants were lost during a refactor/revert and are currently set to + // estimated defaults. + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp new file mode 100644 index 0000000000..41744aa047 --- /dev/null +++ b/src/software/embedded/robot_localizer.cpp @@ -0,0 +1,306 @@ +#include "robot_localizer.h" + +#include + +RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) + : process_linear_acceleration_noise_variance_(config.process_noise_variance), + process_angular_acceleration_noise_variance_(config.process_noise_variance) +{ + filter_.state_covariance = + Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal(); + + filter_.measurement_covariance = + Eigen::Vector( + config.vision_noise_variance, config.vision_noise_variance, + config.vision_noise_variance, config.motor_sensor_noise_variance, + config.motor_sensor_noise_variance, config.motor_sensor_noise_variance, + ImuService::IMU_VARIANCE) + .asDiagonal(); + + last_step_time_ = std::chrono::steady_clock::now(); +} + +void RobotLocalizer::step(const Vector& linear_acceleration) +{ + FilterStep step{ + .prediction = FilterStep::Predict{}, + .update = std::nullopt, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + const std::chrono::duration delta_time = step.time - last_step_time_; + const double delta_time_seconds = delta_time.count(); + last_step_time_ = step.time; + + // clang-format off + step.prediction->process_model << + 1, 0, 0, delta_time_seconds, 0, 0, + 0, 1, 0, 0, delta_time_seconds, 0, + 0, 0, 1, 0, 0, delta_time_seconds, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + // clang-format on + + const double delta_time_squared = delta_time_seconds * delta_time_seconds; + const double delta_time_cubed = delta_time_squared * delta_time_seconds; + const double delta_time_fourth = delta_time_cubed * delta_time_seconds; + + auto& process_covariance = step.prediction->process_covariance; + process_covariance.setZero(); + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = + delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ORIENTATION)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_squared * process_angular_acceleration_noise_variance_; + + auto& control_model = step.prediction->control_model; + control_model.setZero(); + + control_model(static_cast(StateIndex::X_POSITION), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::Y_POSITION), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::X_VELOCITY), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_seconds; + + control_model(static_cast(StateIndex::Y_VELOCITY), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_seconds; + + step.prediction->control_input << linear_acceleration.x(), linear_acceleration.y(); + + filter_.process_model = step.prediction->process_model; + filter_.process_covariance = step.prediction->process_covariance; + filter_.control_model = step.prediction->control_model; + + history.push_front(step); + filter_.predict(step.prediction->control_input); +} + +void RobotLocalizer::update(const VisionData& data) +{ + if (history.empty()) + { + updateFilterWithVision(data.position, data.orientation); + return; + } + + const auto current_time = std::chrono::steady_clock::now(); + const auto sample_age = std::chrono::duration(data.age_seconds); + + const auto rollback_point = std::find_if( + history.begin(), history.end(), + [&](const FilterStep& step) { return step.time - current_time >= sample_age; }); + + if (rollback_point == history.begin()) + { + // All history predates the sample, no need to rollback + updateFilterWithVision(data.position, data.orientation); + return; + } + + auto replay_iter = std::make_reverse_iterator(rollback_point); + + // Truncate history after the rollback point + history.erase(rollback_point, history.end()); + + filter_.state_estimate = replay_iter->state_estimate; + filter_.state_covariance = replay_iter->state_covariance; + + updateFilterWithVision(data.position, data.orientation); + + // Replay from the rollback point back to the current estimate + for (; replay_iter != history.rbegin(); --replay_iter) + { + if (replay_iter->prediction.has_value()) + { + const auto& prediction = replay_iter->prediction.value(); + filter_.process_model = prediction.process_model; + filter_.process_covariance = prediction.process_covariance; + filter_.control_model = prediction.control_model; + filter_.predict(prediction.control_input); + } + + if (replay_iter->update.has_value()) + { + const auto& update = replay_iter->update.value(); + filter_.measurement_model = update.measurement_model; + filter_.update(update.measurement); + } + } +} + +void RobotLocalizer::updateFilterWithVision(const Point& position, + const Angle& orientation) +{ + const double orientation_estimate = + filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); + + Eigen::Vector measurement; + measurement.setZero(); + + measurement(static_cast(MeasurementIndex::VISION_X_POSITION)) = + position.x(); + measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = + position.y(); + + // Coterminal angle that is closest to current estimate + measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = + orientation_estimate + + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); + + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_X_POSITION), + static_cast(StateIndex::X_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = 1; + + filter_.update(measurement); +} + +void RobotLocalizer::update(const MotorData& data) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast(MeasurementIndex::MOTOR_X_VELOCITY)) = + data.velocity.x(); + update.measurement(static_cast(MeasurementIndex::MOTOR_Y_VELOCITY)) = + data.velocity.y(); + update.measurement(static_cast( + MeasurementIndex::MOTOR_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +void RobotLocalizer::update(const ImuData& data) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast( + MeasurementIndex::IMU_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +Point RobotLocalizer::getPosition() const +{ + return Point( + filter_.state_estimate(static_cast(StateIndex::X_POSITION)), + filter_.state_estimate(static_cast(StateIndex::Y_POSITION))); +} + +Vector RobotLocalizer::getVelocity() const +{ + return Vector( + filter_.state_estimate(static_cast(StateIndex::X_VELOCITY)), + filter_.state_estimate(static_cast(StateIndex::Y_VELOCITY))); +} + +Angle RobotLocalizer::getOrientation() const +{ + return Angle::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ORIENTATION))) + .clamp(); +} + +AngularVelocity RobotLocalizer::getAngularVelocity() const +{ + return AngularVelocity::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ANGULAR_VELOCITY))); +} diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h new file mode 100644 index 0000000000..3a46a2ff5e --- /dev/null +++ b/src/software/embedded/robot_localizer.h @@ -0,0 +1,178 @@ +#pragma once + +#include +#include +#include + +#include "proto/world.pb.h" +#include "software/embedded/services/imu.h" +#include "software/geom/angle.h" +#include "software/geom/point.h" +#include "software/geom/vector.h" +#include "software/sensor_fusion/filter/kalman_filter.hpp" +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, + ANGULAR_VELOCITY); + +MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, + MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, + IMU_ANGULAR_VELOCITY); + +MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION); + +/** + * Estimates robot orientation, angular velocity, and angular acceleration + * using a Kalman filter. + * + * The filter keeps a history of recent predict/update operations. When delayed + * vision data arrives, the localizer rewinds to the matching historical state, + * applies the delayed measurement, then replays newer steps to recover the + * current estimate. + */ +class RobotLocalizer +{ + public: + struct VisionData + { + Point position; + Angle orientation; + double age_seconds; + }; + + struct MotorData + { + Vector velocity; + AngularVelocity angular_velocity; + }; + + struct ImuData + { + AngularVelocity angular_velocity; + }; + + struct RobotLocalizerConfig + { + double process_noise_variance; + double vision_noise_variance; + double motor_sensor_noise_variance; + }; + + /** + * Creates a new robot localizer. + * + * The variances determine how strongly each source influences the estimate. + * + * @param config Configuration for the localizer variances. + */ + explicit RobotLocalizer(const RobotLocalizerConfig& config); + + /** + * Runs one prediction step using elapsed time since the previous call. + * + * @param linear_acceleration The current linear acceleration of the robot + */ + void step(const Vector& linear_acceleration); + + /** + * Update the robot's position and orientation from data reported by vision. + * + * @param data Vision reading of the robot's position, orientation and age + */ + void update(const VisionData& data); + + /** + * Update the robot's velocity from data reported by motor sensors + * (i.e. encoders or Hall sensors). + * + * @param data Motor sensor reading of the robot's velocity and angular velocity + */ + void update(const MotorData& data); + + /** + * Update the angular velocity from IMU. + * + * @param data IMU reading of the robot's angular velocity + */ + void update(const ImuData& data); + + /** + * Gets the estimated position of the robot in world space. + * + * @return the estimated position of the robot in world space + */ + Point getPosition() const; + + /** + * Gets the estimated velocity of the robot in world space. + * + * @return the estimated velocity of the robot in world space + */ + Vector getVelocity() const; + + /** + * Gets the estimated orientation of the robot in world space. + * + * @return estimated orientation of the robot in world space + */ + Angle getOrientation() const; + + /** + * Gets the estimated angular velocity of the robot. + * + * @return estimated angular velocity of the robot + */ + AngularVelocity getAngularVelocity() const; + + private: + /** + * Update the Kalman filter with the robot's position and orientation from vision. + * + * @param position Vision reading of the robot's position in world space + * @param orientation Vision reading of the robot's orientation in world space + */ + void updateFilterWithVision(const Point& position, const Angle& orientation); + + static constexpr size_t STATE_SIZE = reflective_enum::size(); + static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); + static constexpr size_t CONTROL_SIZE = reflective_enum::size(); + + /** + * Snapshot of a Kalman filter predict/update step needed for rollback/replay. + */ + struct FilterStep + { + struct Predict + { + Eigen::Matrix process_model; + Eigen::Matrix process_covariance; + Eigen::Matrix control_model; + Eigen::Vector control_input; + }; + + struct Update + { + Eigen::Matrix measurement_model; + Eigen::Vector measurement; + }; + + std::optional prediction; + std::optional update; + + Eigen::Vector state_estimate; + Eigen::Matrix state_covariance; + + std::chrono::time_point time; + }; + + KalmanFilter filter_; + + // Process noise variance used in prediction + double process_linear_acceleration_noise_variance_; + double process_angular_acceleration_noise_variance_; + + std::chrono::time_point last_step_time_; + + // History is ordered newest-first (front is the most recent step) + std::deque history; +}; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index fa9571a16c..c0d1eff518 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -51,3 +51,22 @@ cc_library( "@eigen", ], ) + +cc_binary( + name = "robot_auto_test", + srcs = ["robot_auto_test.cpp"], + linkopts = [ + "-lpthread", + "-lrt", + ], + deps = [ + ":motor", + ":power", + "//proto/message_translation:tbots_geometry", + "//proto/primitive:primitive_msg_factory", + "//shared:robot_constants", + "//software/embedded:primitive_executor", + "//software/logger", + "@trinamic", + ], +) diff --git a/src/software/embedded/spi_utils.h b/src/software/embedded/spi_utils.h index 1b38e14f22..ee5933d172 100644 --- a/src/software/embedded/spi_utils.h +++ b/src/software/embedded/spi_utils.h @@ -32,7 +32,6 @@ void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, * @param read_rx the buffer our read response will be placed in * @param spi_speed the speed to run spi at in Hz */ -void readThenWriteSpiTransfer( - int fd, const uint8_t* read_tx, const uint8_t* write_tx, const uint8_t* read_rx, - const uint32_t read_len, const uint32_t write_len, - uint32_t spi_speed); // refactor to take std::array, not raw pointers +void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, + const uint8_t* read_rx, const uint32_t read_len, + const uint32_t write_len, uint32_t spi_speed); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index afc30bdd03..fca9b69cd1 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -4,6 +4,7 @@ #include #include "proto/message_translation/tbots_protobuf.h" +#include "proto/primitive/primitive_msg_factory.h" #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" @@ -15,6 +16,7 @@ #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" +#include "software/physics/velocity_conversion_util.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" @@ -71,7 +73,6 @@ extern "C" Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz) - // TODO (#2495): Set the friendly team colour : toml_config_client_(std::make_unique(TOML_CONFIG_FILE_PATH)), motor_status_(std::nullopt), robot_constants_(robot_constants), @@ -84,8 +85,11 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, - TeamColour::YELLOW, robot_id_) + primitive_executor_(robot_constants), + robot_localizer_(RobotLocalizer::RobotLocalizerConfig{ + robot_constants.kalman_process_noise_variance_rad_per_s_4, + robot_constants.kalman_vision_noise_variance_rad_2, + robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2}) { waitForNetworkUp(); @@ -117,7 +121,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; - power_service_ = std::make_unique(); + // power_service_ = std::make_unique(); LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; @@ -154,45 +158,27 @@ void Thunderloop::runLoop() struct timespec last_kicker_fired; struct timespec prev_iter_start_time; - // Input buffer - TbotsProto::Primitive new_primitive; - // Loop interval int interval = static_cast(1.0f / static_cast(loop_hz_) * NANOSECONDS_PER_SECOND); - // Get current time - // Note: CLOCK_MONOTONIC is used over CLOCK_REALTIME since - // CLOCK_REALTIME can jump backwards clock_gettime(CLOCK_MONOTONIC, &next_shot); clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); clock_gettime(CLOCK_MONOTONIC, &prev_iter_start_time); + // Initial Version Setup std::string thunderloop_hash, thunderloop_date_flashed; - std::ifstream hashFile("~/thunderbots_hashes/thunderloop.hash"); std::ifstream dateFile("~/thunderbots_hashes/thunderloop.date"); - std::getline(hashFile, thunderloop_hash); std::getline(dateFile, thunderloop_date_flashed); - hashFile.close(); dateFile.close(); - robot_status_.set_thunderloop_version(thunderloop_hash); robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); - - std::optional imu_poll = imu_service_->poll(); - - // TODO (3725): Replace with actual IMU data usage - if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) - { - LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); - } - for (;;) { struct timespec time_since_prev_iter; @@ -200,139 +186,19 @@ void Thunderloop::runLoop() ScopedTimespecTimer::timespecDiff(¤t_time, &prev_iter_start_time, &time_since_prev_iter); prev_iter_start_time = current_time; + { - // Wait until next shot - // - // Note: CLOCK_MONOTONIC is used over CLOCK_REALTIME since - // CLOCK_REALTIME can jump backwards clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_shot, NULL); - FrameMarkStart(TracyConstants::THUNDERLOOP_FRAME_MARKER); - ScopedTimespecTimer iteration_timer(&iteration_time); - // Network Service: receive newest primitives and send out the last - // robot status - { - ScopedTimespecTimer timer(&poll_time); + processNetworkPolling(poll_time, last_primitive_received_time); - ZoneNamedN(_tracy_network_poll, "Thunderloop: Poll NetworkService", true); + processLocalizationUpdates(); - new_primitive = network_service_->poll(robot_status_); - } + processPrimitiveExecution(poll_time, last_primitive_received_time); - thunderloop_status_.set_network_service_poll_time_ms( - getMilliseconds(poll_time)); - - uint64_t last_handled_primitive_set = primitive_.sequence_number(); - - // Updating primitives with newly received data - // and setting the correct time elasped since last primitive - - struct timespec time_since_last_primitive_received; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, - &last_primitive_received_time, - &time_since_last_primitive_received); - network_status_.set_ms_since_last_primitive_received( - getMilliseconds(time_since_last_primitive_received)); - - // If the primitive msg is new, update the internal buffer - // and start the new primitive. - if (new_primitive.time_sent().epoch_timestamp_seconds() > - primitive_.time_sent().epoch_timestamp_seconds()) - { - // Save new primitive - primitive_ = new_primitive; - - // Update primitive executor's primitive set - { - clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); - - // Start new primitive - { - ScopedTimespecTimer timer(&poll_time); - primitive_executor_.updatePrimitive(primitive_); - } - - thunderloop_status_.set_primitive_executor_start_time_ms( - getMilliseconds(poll_time)); - } - } - - if (motor_status_.has_value()) - { - auto status = motor_status_.value(); - primitive_executor_.updateVelocity( - createVector(status.local_velocity()), - createAngularVelocity(status.angular_velocity())); - } - - // Timeout Overrides for Primitives - // These should be after the new primitive update section above - - // If primitive not received in a while, stop robot - // Primitive Executor: run the last primitive if we have not timed out - { - ScopedTimespecTimer timer(&poll_time); - - ZoneNamedN(_tracy_step_primitive, "Thunderloop: Step Primitive", true); - - // Handle emergency stop override - auto nanoseconds_elapsed_since_last_primitive = - getNanoseconds(time_since_last_primitive_received); - - if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) - { - primitive_executor_.setStopPrimitive(); - } - - direct_control_ = - *primitive_executor_.stepPrimitive(primitive_executor_status_); - } - - thunderloop_status_.set_primitive_executor_step_time_ms( - getMilliseconds(poll_time)); - - // Power Service: execute the power control command - power_status_ = pollPowerService(poll_time); - thunderloop_status_.set_power_service_poll_time_ms( - getMilliseconds(poll_time)); - - struct timespec time_since_kicker_fired; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, &last_kicker_fired, - &time_since_kicker_fired); - chipper_kicker_status_.set_ms_since_kicker_fired( - getMilliseconds(time_since_kicker_fired)); - - struct timespec time_since_chipper_fired; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, &last_chipper_fired, - &time_since_chipper_fired); - chipper_kicker_status_.set_ms_since_chipper_fired( - getMilliseconds(time_since_chipper_fired)); - - // if a kick proto is sent or if autokick is on - if (direct_control_.power_control().chicker().has_kick_speed_m_per_s() || - direct_control_.power_control() - .chicker() - .auto_chip_or_kick() - .has_autokick_speed_m_per_s()) - { - clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); - } - // if a chip proto is sent or if autochip is on - else if (direct_control_.power_control() - .chicker() - .has_chip_distance_meters() || - direct_control_.power_control() - .chicker() - .auto_chip_or_kick() - .has_autochip_distance_meters()) - { - clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); - } + updateChickerStatus(last_chipper_fired, last_kicker_fired); // Motor Service: execute the motor control command motor_status_ = pollMotorService(poll_time, direct_control_.motor_control(), @@ -340,37 +206,182 @@ void Thunderloop::runLoop() thunderloop_status_.set_motor_service_poll_time_ms( getMilliseconds(poll_time)); - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - time_sent_.set_epoch_timestamp_seconds( - static_cast(current_time.tv_sec)); - - // Update Robot Status with poll responses - robot_status_.set_robot_id(robot_id_); - robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); - *(robot_status_.mutable_time_sent()) = time_sent_; - *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; - *(robot_status_.mutable_motor_status()) = motor_status_.value(); - *(robot_status_.mutable_power_status()) = power_status_; - *(robot_status_.mutable_network_status()) = network_status_; - *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; - *(robot_status_.mutable_primitive_executor_status()) = - primitive_executor_status_; - - updateErrorCodes(); + updateAndSendRobotStatus(primitive_.sequence_number()); } auto loop_duration_ns = getNanoseconds(iteration_time); thunderloop_status_.set_iteration_time_ms(loop_duration_ns / NANOSECONDS_PER_MILLISECOND); - // Calculate next shot (which is an absolute time) next_shot.tv_nsec += interval; timespecNorm(next_shot); - FrameMarkEnd(TracyConstants::THUNDERLOOP_FRAME_MARKER); } } +inline void Thunderloop::processNetworkPolling(struct timespec& poll_time, + struct timespec& last_primitive_received_time) +{ + struct timespec current_time; + TbotsProto::Primitive new_primitive; + + { + ScopedTimespecTimer timer(&poll_time); + ZoneNamedN(_tracy_network_poll, "Thunderloop: Poll NetworkService", true); + new_primitive = network_service_->poll(robot_status_); + } + + thunderloop_status_.set_network_service_poll_time_ms(getMilliseconds(poll_time)); + + struct timespec time_since_last_primitive_received; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + ScopedTimespecTimer::timespecDiff(¤t_time, &last_primitive_received_time, + &time_since_last_primitive_received); + network_status_.set_ms_since_last_primitive_received( + getMilliseconds(time_since_last_primitive_received)); + + if (new_primitive.time_sent().epoch_timestamp_seconds() > + primitive_.time_sent().epoch_timestamp_seconds()) + { + primitive_ = new_primitive; + + if (primitive_.has_move()) + { + const Point position = + createPoint(primitive_.move().xy_traj_params().start_position()); + const Angle orientation = + createAngle(primitive_.move().w_traj_params().start_angle()); + + robot_localizer_.update( + RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); + } + + clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); + + { + ScopedTimespecTimer timer(&poll_time); + primitive_executor_.updatePrimitive(primitive_); + } + + thunderloop_status_.set_primitive_executor_start_time_ms( + getMilliseconds(poll_time)); + } +} + +inline void Thunderloop::processLocalizationUpdates() +{ + const std::optional imu_poll = imu_service_->poll(); + + if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) + { + robot_localizer_.update( + RobotLocalizer::ImuData{imu_poll->angular_velocity.value()}); + } + + if (motor_status_.has_value()) + { + auto status = motor_status_.value(); + + robot_localizer_.update(RobotLocalizer::MotorData{ + localToGlobalVelocity(createVector(status.local_velocity()), + robot_localizer_.getOrientation()), + createAngularVelocity(status.angular_velocity())}); + + Vector linear_acceleration; + if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) + { + const auto accel = imu_poll->linear_acceleration.value(); + linear_acceleration = Vector(accel[1], accel[0]); + } + + robot_localizer_.step(linear_acceleration); + + primitive_executor_.updateState(robot_localizer_.getPosition(), + robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), + robot_localizer_.getAngularVelocity()); + } +} + +inline void Thunderloop::processPrimitiveExecution( + struct timespec& poll_time, struct timespec& last_primitive_received_time) +{ + struct timespec current_time; + struct timespec time_since_last_primitive_received; + + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + ScopedTimespecTimer::timespecDiff(¤t_time, &last_primitive_received_time, + &time_since_last_primitive_received); + + { + ScopedTimespecTimer timer(&poll_time); + ZoneNamedN(_tracy_step_primitive, "Thunderloop: Step Primitive", true); + + auto nanoseconds_elapsed_since_last_primitive = + getNanoseconds(time_since_last_primitive_received); + + if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) + { + primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); + } + + direct_control_ = *primitive_executor_.stepPrimitive(primitive_executor_status_); + } + + thunderloop_status_.set_primitive_executor_step_time_ms(getMilliseconds(poll_time)); +} + +inline void Thunderloop::updateChickerStatus(struct timespec& last_chipper_fired, + struct timespec& last_kicker_fired) +{ + struct timespec current_time; + struct timespec time_diff; + + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + ScopedTimespecTimer::timespecDiff(¤t_time, &last_kicker_fired, &time_diff); + chipper_kicker_status_.set_ms_since_kicker_fired(getMilliseconds(time_diff)); + + ScopedTimespecTimer::timespecDiff(¤t_time, &last_chipper_fired, &time_diff); + chipper_kicker_status_.set_ms_since_chipper_fired(getMilliseconds(time_diff)); + + if (direct_control_.power_control().chicker().has_kick_speed_m_per_s() || + direct_control_.power_control() + .chicker() + .auto_chip_or_kick() + .has_autokick_speed_m_per_s()) + { + clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); + } + else if (direct_control_.power_control().chicker().has_chip_distance_meters() || + direct_control_.power_control() + .chicker() + .auto_chip_or_kick() + .has_autochip_distance_meters()) + { + clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); + } +} + +inline void Thunderloop::updateAndSendRobotStatus(uint64_t last_handled_primitive_set) +{ + struct timespec current_time; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + time_sent_.set_epoch_timestamp_seconds(static_cast(current_time.tv_sec)); + + robot_status_.set_robot_id(robot_id_); + robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); + *(robot_status_.mutable_time_sent()) = time_sent_; + *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; + *(robot_status_.mutable_motor_status()) = motor_status_.value_or(TbotsProto::MotorStatus()); + *(robot_status_.mutable_power_status()) = power_status_; + *(robot_status_.mutable_network_status()) = network_status_; + *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; + *(robot_status_.mutable_primitive_executor_status()) = primitive_executor_status_; + + updateErrorCodes(); +} + double Thunderloop::getMilliseconds(timespec time) { return (static_cast(time.tv_sec) * MILLISECONDS_PER_SECOND) + diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 94d43da359..347c655d83 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -3,13 +3,12 @@ #include #include #include -#include -#include #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/robot_localizer.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" @@ -125,6 +124,41 @@ class Thunderloop */ void waitForNetworkUp(); + /** + * Polls the network service for new primitives and updates timing status. + * + * @param poll_time Output for polling duration + * @param last_primitive_received_time Input/Output for tracking last received packet + */ + inline void processNetworkPolling(struct timespec& poll_time, + struct timespec& last_primitive_received_time); + + /** + * Processes robot localization updates from sensors (IMU, Motors). + */ + inline void processLocalizationUpdates(); + + /** + * Steps the primitive executor and handles timeouts. + * + * @param poll_time Output for execution duration + * @param last_primitive_received_time Input for checking timeouts + */ + inline void processPrimitiveExecution(struct timespec& poll_time, + struct timespec& last_primitive_received_time); + + /** + * Updates internal tracking for chipper/kicker firing events. + */ + inline void updateChickerStatus(struct timespec& last_chipper_fired, + struct timespec& last_kicker_fired); + + /** + * Aggregates and sends the final robot status message. + * @param last_handled_primitive_set + */ + inline void updateAndSendRobotStatus(uint64_t last_handled_primitive_set); + // Input Msg Buffers TbotsProto::World world_; @@ -157,6 +191,9 @@ class Thunderloop // Primitive Executor PrimitiveExecutor primitive_executor_; + // Robot localization model + RobotLocalizer robot_localizer_; + // 500 millisecond timeout on receiving primitives before we stop the robots const double PACKET_TIMEOUT_NS = 500.0 * NANOSECONDS_PER_MILLISECOND; diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index ebf0480ac3..9535e727d1 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -43,6 +43,7 @@ def __init__( blue_full_system_proto_unix_io, yellow_full_system_proto_unix_io, gamecontroller, + robot_communication, publish_validation_protos=True, is_yellow_friendly=False, ): @@ -53,6 +54,7 @@ def __init__( :param blue_full_system_proto_unix_io: The blue full system proto unix io to use :param yellow_full_system_proto_unix_io: The yellow full system proto unix io to use :param gamecontroller: The gamecontroller context managed instance + :param robot_communication: The robot communication instance :param publish_validation_protos: whether to publish validation protos :param: is_yellow_friendly: if yellow is the friendly team """ @@ -66,25 +68,45 @@ def __init__( ) self.publish_validation_protos = publish_validation_protos self.is_yellow_friendly = is_yellow_friendly + self.robot_communication = robot_communication logger.info("determining robots on field") # survey field for available robot ids - try: - world = self.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) - self.initial_world = world - self.friendly_robot_ids_field = [ - robot.id for robot in world.friendly_team.team_robots - ] - - logger.info(f"friendly team ids {self.friendly_robot_ids_field}") - - if len(self.friendly_robot_ids_field) == 0: - raise Exception("no friendly robots found on field") + survey_start_time = time.time() + self.friendly_robot_ids_field = [] + while time.time() - survey_start_time < WORLD_BUFFER_TIMEOUT: + try: + world = self.world_buffer.get(block=True, timeout=0.1) + self.initial_world = world + self.friendly_robot_ids_field = [ + robot.id for robot in world.friendly_team.team_robots + ] + + if len(self.friendly_robot_ids_field) > 0: + logger.info(f"friendly team ids {self.friendly_robot_ids_field}") + break + except queue.Empty: + continue + + if len(self.friendly_robot_ids_field) == 0: + raise Exception("no friendly robots found on field within timeout") + + def wait_for_estop_play(self): + """Blocks until the estop is in the PLAY state""" + if self.robot_communication.estop_is_playing: + return + + logger.info("\x1b[33m" + "Waiting for Estop to be in PLAY state..." + "\x1b[0m") + while not self.robot_communication.estop_is_playing: + # We must process events if Thunderscope is running to keep it responsive + if self.thunderscope: + from pyqtgraph.Qt import QtWidgets - except queue.Empty: - raise Exception( - f"No Worlds were received with in {WORLD_BUFFER_TIMEOUT} seconds. Please make sure atleast 1 robot and 1 ball is present on the field." - ) + QtWidgets.QApplication.processEvents() + time.sleep(0.1) + logger.info( + "\x1b[32m" + "Estop is in PLAY state. Proceeding with test." + "\x1b[0m" + ) @override def send_gamecontroller_command( @@ -99,7 +121,7 @@ def send_gamecontroller_command( :param team: The team which the command as attributed to :param final_ball_placement_point: The ball placement point """ - self.gamecontroller.send_ci_input( + self.gamecontroller.send_gc_command( gc_command=gc_command, team=team, final_ball_placement_point=final_ball_placement_point, @@ -124,14 +146,19 @@ def run_test( def stop_test(delay): time.sleep(delay) - if self.thunderscope: - self.thunderscope.close() + # We no longer close thunderscope here, because a test might call run_test multiple times. + # Thunderscope will be closed when the fixture is torn down. def __runner(): time.sleep(LAUNCH_DELAY_S) test_end_time = time.time() + test_timeout_s + # Keep track if we started with any eventually validations + has_eventually_validations = any( + len(seq) > 0 for seq in eventually_validation_sequence_set + ) + while time.time() < test_end_time: while True: try: @@ -172,9 +199,15 @@ def __runner(): # Check that all always validations are always valid validation.check_validation(always_validation_proto_set) - # Check that all eventually validations are eventually valid + # Break if eventually validation passes + if has_eventually_validations and all( + len(seq) == 0 for seq in eventually_validation_sequence_set + ): + break + validation.check_validation(eventually_validation_proto_set) - stop_test(TEST_END_DELAY) + + stop_test(delay=PROCESS_BUFFER_DELAY_S) def excepthook(args): """This function is _critical_ for show_thunderscope to work. @@ -189,14 +222,22 @@ def excepthook(args): threading.excepthook = excepthook + # If visualization is enabled, we need to be careful. + # Thunderscope.show() is blocking. if self.thunderscope: run_test_thread = threading.Thread(target=__runner, daemon=True) run_test_thread.start() - self.thunderscope.show() + + # Only call show if the window is not already open. + # If it IS open, it means we are ALREADY in the Qt event loop, + # which can only happen if we are running this run_test in a background thread. + if not self.thunderscope.is_open(): + self.thunderscope.show() + run_test_thread.join() if self.last_exception: - pytest.fail(str(ex.last_exception)) + pytest.fail(str(self.last_exception)) else: __runner() @@ -322,6 +363,13 @@ def load_command_line_arguments(): help="Disables checking for estop plugged in (ONLY USE FOR LOCAL TESTING)", ) + parser.add_argument( + "--no_visualization", + action="store_true", + default=False, + help="Disables the Thunderscope GUI", + ) + return parser.parse_args() @@ -388,15 +436,17 @@ def field_test_runner(): simulator_proto_unix_io=simulator_proto_unix_io, ) # Inject the proto unix ios into thunderscope and start the test - tscope = Thunderscope( - configure_field_test_view( - simulator_proto_unix_io=simulator_proto_unix_io, - blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, - yellow_is_friendly=args.run_yellow, - ), - layout_path=None, - ) + tscope = None + if not args.no_visualization: + tscope = Thunderscope( + configure_field_test_view( + simulator_proto_unix_io=simulator_proto_unix_io, + blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, + yellow_is_friendly=args.run_yellow, + ), + layout_path=None, + ) # Set control mode for all robots to AI so that packets are sent to the robots for robot_id in range(MAX_ROBOT_IDS_PER_SIDE): @@ -407,9 +457,10 @@ def field_test_runner(): # connect the keyboard estop toggle to the key event if needed if estop_mode == EstopMode.KEYBOARD_ESTOP: - tscope.keyboard_estop_shortcut.activated.connect( - rc_friendly.toggle_keyboard_estop - ) + if tscope: + tscope.keyboard_estop_shortcut.activated.connect( + rc_friendly.toggle_keyboard_estop + ) # we call this method to enable estop automatically when a field test starts rc_friendly.toggle_keyboard_estop() logger.warning( @@ -425,6 +476,7 @@ def field_test_runner(): yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, gamecontroller=gamecontroller, thunderscope=tscope, + robot_communication=rc_friendly, is_yellow_friendly=args.run_yellow, ) diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index 56c31df60b..2aaa6fa7e7 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -3,68 +3,10 @@ from software.simulated_tests.simulated_test_fixture import * from software.logger.logger import create_logger -import math logger = create_logger(__name__) - - -# TODO 2908: Support running this test in both simulator or field mode -# this test can be run either in simulation or on the field -# @pytest.mark.parametrize( -# "robot_x_destination, robot_y_destination", -# [(-2.0, -1), (-2.0, 1.0), (0.0, 1.0), (0.0, -1.0)], -# ) -# def test_basic_movement(simulated_test_runner): -# -# robot_starting_x = 0 -# robot_starting_y = 0 -# ROBOT_ID = 0 -# angle = 0 -# robot_x_destination = -2 -# robot_y_destination = -1 -# rob_pos_p = Point(x_meters=robot_x_destination, y_meters=robot_y_destination) -# -# move_tactic = MoveTactic() -# move_tactic.destination.CopyFrom(rob_pos_p) -# move_tactic.dribbler_mode = DribblerMode.OFF -# move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) -# move_tactic.ball_collision_type = BallCollisionType.AVOID -# move_tactic.auto_chip_or_kick.CopyFrom(AutoChipOrKick(autokick_speed_m_per_s=0.0)) -# move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT -# move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE -# -# # setup world state -# initial_worldstate = create_world_state( -# yellow_robot_locations=[], -# blue_robot_locations=[tbots_cpp.Point(robot_starting_x, robot_starting_y)], -# ball_location=tbots_cpp.Point(1, 1), -# ball_velocity=tbots_cpp.Point(0, 0), -# ) -# simulated_test_runner.set_world_state(initial_worldstate) -# -# # Setup Tactic -# params = AssignedTacticPlayControlParams() -# -# params.assigned_tactics[ROBOT_ID].move.CopyFrom(move_tactic) -# -# # Eventually Validation -# eventually_validation_sequence_set = [ -# [ -# RobotEventuallyEntersRegion( -# regions=[ -# tbots_cpp.Circle( -# tbots_cpp.Point(robot_x_destination, robot_y_destination), 0.2 -# ) -# ] -# ), -# ] -# ] -# simulated_test_runner.set_tactics(params, True) -# -# simulated_test_runner.run_test( -# eventually_validation_sequence_set=eventually_validation_sequence_set, -# test_timeout_s=5, -# ) +import math +import threading # this test can only be run on the field @@ -88,38 +30,60 @@ def test_basic_rotation(field_test_runner): robot = world.friendly_team.team_robots[0] rob_pos_p = robot.current_state.global_position - logger.info("staying in pos {rob_pos_p}") - - for angle in test_angles: - move_tactic = MoveTactic() - move_tactic.destination.CopyFrom(rob_pos_p) - move_tactic.dribbler_mode = DribblerMode.OFF - move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) - move_tactic.ball_collision_type = BallCollisionType.AVOID - move_tactic.auto_chip_or_kick.CopyFrom( - AutoChipOrKick(autokick_speed_m_per_s=0.0) - ) - move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT - move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE - # Setup Tactic - field_test_runner.set_tactics( - blue_tactics={id: move_tactic}, yellow_tactics=None - ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=5, + def execute_test(): + # Wait for the user to flip the estop to PLAY + field_test_runner.wait_for_estop_play() + + # Force start the game automatically + field_test_runner.gamecontroller.send_gc_command( + proto.ssl_gc_state_pb2.Command.FORCE_START, + proto.ssl_gc_common_pb2.Team.UNKNOWN, + final_ball_placement_point=None, ) + + for angle in test_angles: + print(f"Rotating to {angle} degrees") + move_tactic = MoveTactic() + move_tactic.destination.CopyFrom(rob_pos_p) + move_tactic.dribbler_mode = DribblerMode.OFF + move_tactic.final_orientation.CopyFrom( + Angle(radians=angle * math.pi / 180.0) + ) + move_tactic.ball_collision_type = BallCollisionType.AVOID + move_tactic.auto_chip_or_kick.CopyFrom( + AutoChipOrKick(autokick_speed_m_per_s=0.0) + ) + move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT + move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE + + # Setup Tactic + field_test_runner.set_tactics( + blue_tactics={id: move_tactic}, yellow_tactics=None + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=5, + ) + + # validate by eye + logger.info(f"robot set to {angle} orientation") + time.sleep(2) + # Send a halt tactic after the test finishes field_test_runner.set_tactics( blue_tactics={id: HaltTactic()}, yellow_tactics=None ) - # validate by eye - logger.info(f"robot set to {angle} orientation") + test_thread = threading.Thread(target=execute_test, daemon=True) + test_thread.start() - time.sleep(2) + # If thunderscope is enabled, show it in the main thread (blocking) + if field_test_runner.thunderscope: + field_test_runner.thunderscope.show() + + test_thread.join() def test_one_robots_square(field_test_runner): @@ -138,10 +102,10 @@ def test_one_robots_square(field_test_runner): id = world.friendly_team.team_robots[0].id print(f"Running test on robot {id}") - point1 = Point(x_meters=-0.3, y_meters=0.6) - point2 = Point(x_meters=-0.3, y_meters=-0.6) - point3 = Point(x_meters=-1.5, y_meters=-0.6) - point4 = Point(x_meters=-1.5, y_meters=0.6) + point1 = Point(x_meters=0.4, y_meters=0.4) + point2 = Point(x_meters=0.4, y_meters=-0.4) + point3 = Point(x_meters=-0.4, y_meters=-0.4) + point4 = Point(x_meters=-0.4, y_meters=0.4) tactic_0 = MoveTactic( destination=point1, @@ -179,25 +143,48 @@ def test_one_robots_square(field_test_runner): max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, ) - tactics = [tactic_0, tactic_1, tactic_2, tactic_3] - for tactic in tactics: - print(f"Going to {tactic.destination}") + tactics = [tactic_0, tactic_1, tactic_2, tactic_3, tactic_0] - field_test_runner.set_tactics( - blue_tactics={ - id: tactic, - }, - yellow_tactics=None, + def execute_test(): + # Wait for the user to flip the estop to PLAY + field_test_runner.wait_for_estop_play() + + # Force start the game automatically + field_test_runner.gamecontroller.send_gc_command( + proto.ssl_gc_state_pb2.Command.FORCE_START, + proto.ssl_gc_common_pb2.Team.UNKNOWN, + final_ball_placement_point=None, ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=4, + + for tactic in tactics: + print(f"Going to {tactic.destination}") + + field_test_runner.set_tactics( + blue_tactics={ + id: tactic, + }, + yellow_tactics=None, + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=8, + ) + + # Send a halt tactic after the test finishes + field_test_runner.set_tactics( + blue_tactics={id: HaltTactic()}, yellow_tactics=None ) - # Send a halt tactic after the test finishes - field_test_runner.set_tactics(blue_tactics={id: HaltTactic()}, yellow_tactics=None) + test_thread = threading.Thread(target=execute_test, daemon=True) + test_thread.start() + + # If thunderscope is enabled, show it in the main thread (blocking) + if field_test_runner.thunderscope: + field_test_runner.thunderscope.show() + + test_thread.join() if __name__ == "__main__": diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index f14ed77157..833d103ef3 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -14,18 +14,19 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(robot_id, enable_log_merging), &NetworkSink::sendToNetwork); - // Sink for outputting logs to the terminal - auto colour_cout_sink_handle = logWorker->addSink( - std::make_unique(true), &ColouredCoutSink::displayColouredLog); + logWorker->addSink(std::make_unique(true), + &ColouredCoutSink::displayColouredLog); - auto csv_sink_handle = - logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); + logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting auto plotjuggler_handle = logWorker->addSink(std::make_unique(network_interface), &PlotJugglerSink::sendToPlotJuggler); + g3::only_change_at_initialization::addLogLevel(CSV); + g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); + g3::initializeLogging(logWorker.get()); } @@ -34,7 +35,7 @@ void NetworkLoggerSingleton::initializeLogger(RobotId robot_id, bool enable_log_ { if (!instance) { - NetworkLoggerSingleton::instance = std::shared_ptr( + instance = std::shared_ptr( new NetworkLoggerSingleton(robot_id, enable_log_merging, network_interface)); } } diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..7593b87ce1 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -280,16 +280,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::BLUE, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::YELLOW, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } @@ -299,19 +297,22 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.yellow_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.yellow_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); yellow_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *yellow_team_world_msg; + for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -320,20 +321,22 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.blue_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.blue_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); blue_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *blue_team_world_msg; for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -342,8 +345,8 @@ void ErForceSimulator::setRobotPrimitive( RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity) + const TbotsProto::World& world_msg, const Point& position, const Angle& orientation, + const Vector& velocity, const AngularVelocity& angular_velocity) { // Set to NEG_X because the world msg in this simulator is normalized // correctly @@ -352,8 +355,9 @@ void ErForceSimulator::setRobotPrimitive( if (robot_primitive_executor_iter != robot_primitive_executor_map.end()) { auto primitive_executor = robot_primitive_executor_iter->second; + primitive_executor->updateState(position, velocity, orientation, + angular_velocity); primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id)); - primitive_executor->updateVelocity(local_velocity, angular_velocity); } else { @@ -370,16 +374,16 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( SSLSimulationProto::RobotControl robot_control; auto sim_state = getSimulatorState(); - std::map> current_velocity_map; + std::map> current_state; if (side == gameController::Team::BLUE) { const auto& sim_robots = sim_state.blue_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } else { const auto& sim_robots = sim_state.yellow_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } for (auto& primitive_executor_with_id : robot_primitive_executor_map) @@ -393,8 +397,9 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, + globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), + std::get<1>(current_state.at(robot_id))), + std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, primitive_executor_time_step_s); } else @@ -564,18 +569,18 @@ void ErForceSimulator::resetCurrentTime() current_time = Timestamp::fromSeconds(0); } -std::map> -ErForceSimulator::getRobotIdToLocalVelocityMap( +std::map> +ErForceSimulator::getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots) { - std::map> robot_to_local_velocity; + std::map> map; for (const auto& sim_robot : sim_robots) { - const Vector local_vel = - globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()), - Angle::fromRadians(sim_robot.angle())); - const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); - robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel}; + const Point position = {sim_robot.p_x(), sim_robot.p_y()}; + const Angle orientation = Angle::fromRadians(sim_robot.angle()); + const Vector velocity = {sim_robot.v_x(), sim_robot.v_y()}; + const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); + map[sim_robot.id()] = {position, orientation, velocity, angular_vel}; } - return robot_to_local_velocity; + return map; } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..9b65c81449 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -158,18 +158,19 @@ class ErForceSimulator RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity); + const TbotsProto::World& world_msg, const Point& position, + const Angle& orientation, const Vector& velocity, + const AngularVelocity& angular_velocity); /** - * Gets a map from robot id to local and angular velocity from repeated sim robots + * Gets a map from robot id to position and velocity from repeated sim robots * * @param sim_robots Repeated er force sim robot protos * - * @return a map from robot id to local velocity and angular velocity + * @return a map from robot id to position and velocity */ - static std::map> - getRobotIdToLocalVelocityMap( + static std::map> + getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots); /** diff --git a/src/software/util/pid/BUILD b/src/software/util/pid/BUILD new file mode 100644 index 0000000000..156e74085e --- /dev/null +++ b/src/software/util/pid/BUILD @@ -0,0 +1,8 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "pid", + hdrs = [ + "pid_controller.hpp", + ], +) diff --git a/src/software/util/pid/pid_controller.hpp b/src/software/util/pid/pid_controller.hpp new file mode 100644 index 0000000000..d3e673efc2 --- /dev/null +++ b/src/software/util/pid/pid_controller.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include + +namespace controls +{ +template +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(T k_p, T k_i, T k_d, T max_integral); + + /** + * Given an error, returns the control effort to minimize it. + * + * @param deltaTime The time passed since last step, for calculating integrator and + *derivative. If this function is calling in invariant intervals, deltaTime can be set + *to 1 and any effects it would have can be handled by tuning kD. + **/ + T step(T error, T delta_time = 1.0f); + + /** + * Resets the integrator and clears the last error used for derivative calculation. + **/ + void reset(); + + T k_p; + T k_i; + T k_d; + T max_integral; + + protected: + T integral; + std::optional last_error = std::nullopt; +}; +} // namespace controls + +template +controls::PIDController::PIDController(T k_p, T k_i, T k_d, T max_integral) + : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral){}; + +template +T controls::PIDController::step(T error, T delta_time) +{ + // If sign of error swaps, reset integrator + if (last_error.value_or(0) * error < 0) + { + integral = 0; + } + + integral += std::max(std::min(error * delta_time, max_integral), -max_integral); + // set derivative, if no last_error, just set to 0 + const T derivative = (last_error.value_or(error) - error) / delta_time; + last_error = error; + return error * k_p + integral * k_i + derivative * k_d; +} + +template +void controls::PIDController::reset() +{ + integral = 0; + last_error = std::nullopt; +} diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 66e2244d76..5a13d519a6 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -218,7 +218,8 @@ Duration Robot::getTimeToPosition(const Point& destination, double initial_velocity_1d = velocity().dot(dist_vector.normalize()); double final_velocity_1d = final_velocity.dot(dist_vector.normalize()); - return getTimeToTravelDistance(dist, robot_constants_.robot_max_speed_m_per_s, + return getTimeToTravelDistance(dist, + robot_constants_.robot_max_speed_trajectory_m_per_s, robot_constants_.robot_max_acceleration_m_per_s_2, initial_velocity_1d, final_velocity_1d); } From fa58f79194cbb9f7be0822a5895145803632fe36 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 00:34:01 -0700 Subject: [PATCH 02/46] linting --- .../ai/navigator/trajectory/trajectory.hpp | 3 +-- src/software/embedded/BUILD | 4 ++-- .../motion_control/pid_controller_test.cpp | 3 ++- src/software/embedded/primitive_executor.cpp | 19 +++++++-------- src/software/embedded/primitive_executor.h | 12 +++++----- src/software/embedded/thunderloop.cpp | 24 +++++++++---------- src/software/embedded/thunderloop.h | 2 +- 7 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 57789684a3..4f970c2e7a 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -67,6 +67,5 @@ class Trajectory * different * @return True if the trajectories are different, false otherwise */ - virtual bool isNew(const Trajectory& other, - double threshold) const = 0; + virtual bool isNew(const Trajectory& other, double threshold) const = 0; }; diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 2ae80efcd7..6bc28d668e 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -34,11 +34,11 @@ cc_library( "//proto/primitive:primitive_msg_factory", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/ai/navigator/trajectory:trajectory_path", + "//software/embedded/motion_control:orientation_controller", + "//software/embedded/motion_control:position_controller", "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", - "//software/embedded/motion_control:orientation_controller", - "//software/embedded/motion_control:position_controller", "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", diff --git a/src/software/embedded/motion_control/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp index 307100d84a..90248c5459 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -43,7 +43,8 @@ TEST(PidControllerTest, OnlyIntegralTermNonZero) // should not accumulate integral term above max_integral EXPECT_DOUBLE_EQ(pid.step(6.7, 1.0), k_i * 5.5); // Sign not swapped from 0.0 to 6.7 - EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0); // Clamped (5.5 + 5.0 = 10.5 -> 10.0) + EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), + k_i * 10.0); // Clamped (5.5 + 5.0 = 10.5 -> 10.0) EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 10.0); } diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 9c1783ef07..fea41e7295 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -12,8 +12,7 @@ PrimitiveExecutor::PrimitiveExecutor( const robot_constants::RobotConstants& robot_constants) - : last_step_time_(std::chrono::steady_clock::now()), - robot_constants_(robot_constants) + : last_step_time_(std::chrono::steady_clock::now()), robot_constants_(robot_constants) { } @@ -49,7 +48,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m const bool is_angular_traj_new = !angular_trajectory_.has_value() || angular_trajectory_->isNew(new_angular_trajectory, - ANGULAR_DESTINATION_THRESHOLD_DEGREES); + ANGULAR_DESTINATION_THRESHOLD_DEGREES); if (is_angular_traj_new) { @@ -115,10 +114,10 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status) { - const auto current_time = std::chrono::steady_clock::now(); + const auto current_time = std::chrono::steady_clock::now(); const auto delta_time_chrono = current_time - last_step_time_; - const Duration delta_time = Duration::fromSeconds( - std::chrono::duration(delta_time_chrono).count()); + const Duration delta_time = + Duration::fromSeconds(std::chrono::duration(delta_time_chrono).count()); time_since_linear_trajectory_creation_ += delta_time_chrono; time_since_angular_trajectory_creation_ += delta_time_chrono; @@ -154,10 +153,10 @@ std::unique_ptr PrimitiveExecutor::stepPrimi prim->direct_control()); } - const Duration elapsed_linear = Duration::fromSeconds( - time_since_linear_trajectory_creation_.count()); - const Duration elapsed_angular = Duration::fromSeconds( - time_since_angular_trajectory_creation_.count()); + const Duration elapsed_linear = + Duration::fromSeconds(time_since_linear_trajectory_creation_.count()); + const Duration elapsed_angular = + Duration::fromSeconds(time_since_angular_trajectory_creation_.count()); const Vector target_linear_velocity_global = position_controller_.step( position_, *trajectory_path_, elapsed_linear, delta_time); diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 952816282a..1be81b2483 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -4,10 +4,10 @@ #include "proto/tbots_software_msgs.pb.h" #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/ai/navigator/trajectory/trajectory_path.h" -#include "software/geom/vector.h" -#include "software/world/robot_state.h" #include "software/embedded/motion_control/orientation_controller.h" #include "software/embedded/motion_control/position_controller.h" +#include "software/geom/vector.h" +#include "software/world/robot_state.h" class PrimitiveExecutor { @@ -77,8 +77,8 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index fca9b69cd1..433b7b0129 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -219,8 +219,8 @@ void Thunderloop::runLoop() } } -inline void Thunderloop::processNetworkPolling(struct timespec& poll_time, - struct timespec& last_primitive_received_time) +inline void Thunderloop::processNetworkPolling( + struct timespec& poll_time, struct timespec& last_primitive_received_time) { struct timespec current_time; TbotsProto::Primitive new_primitive; @@ -296,10 +296,9 @@ inline void Thunderloop::processLocalizationUpdates() robot_localizer_.step(linear_acceleration); - primitive_executor_.updateState(robot_localizer_.getPosition(), - robot_localizer_.getVelocity(), - robot_localizer_.getOrientation(), - robot_localizer_.getAngularVelocity()); + primitive_executor_.updateState( + robot_localizer_.getPosition(), robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity()); } } @@ -371,12 +370,13 @@ inline void Thunderloop::updateAndSendRobotStatus(uint64_t last_handled_primitiv robot_status_.set_robot_id(robot_id_); robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); - *(robot_status_.mutable_time_sent()) = time_sent_; - *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; - *(robot_status_.mutable_motor_status()) = motor_status_.value_or(TbotsProto::MotorStatus()); - *(robot_status_.mutable_power_status()) = power_status_; - *(robot_status_.mutable_network_status()) = network_status_; - *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; + *(robot_status_.mutable_time_sent()) = time_sent_; + *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; + *(robot_status_.mutable_motor_status()) = + motor_status_.value_or(TbotsProto::MotorStatus()); + *(robot_status_.mutable_power_status()) = power_status_; + *(robot_status_.mutable_network_status()) = network_status_; + *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; *(robot_status_.mutable_primitive_executor_status()) = primitive_executor_status_; updateErrorCodes(); diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 347c655d83..8aa4423991 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -145,7 +145,7 @@ class Thunderloop * @param last_primitive_received_time Input for checking timeouts */ inline void processPrimitiveExecution(struct timespec& poll_time, - struct timespec& last_primitive_received_time); + struct timespec& last_primitive_received_time); /** * Updates internal tracking for chipper/kicker firing events. From 1cd0268a7c39655eaf8c6dda3a4eede25213ee39 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 11:00:32 -0700 Subject: [PATCH 03/46] remove auto test --- src/software/embedded/services/BUILD | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index c0d1eff518..1aee8c7016 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -52,21 +52,4 @@ cc_library( ], ) -cc_binary( - name = "robot_auto_test", - srcs = ["robot_auto_test.cpp"], - linkopts = [ - "-lpthread", - "-lrt", - ], - deps = [ - ":motor", - ":power", - "//proto/message_translation:tbots_geometry", - "//proto/primitive:primitive_msg_factory", - "//shared:robot_constants", - "//software/embedded:primitive_executor", - "//software/logger", - "@trinamic", - ], -) + From aff8056f0aa43e6b62f17f1d1a4e5fb9f0024da1 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 11:01:24 -0700 Subject: [PATCH 04/46] remove auto test --- src/software/embedded/services/BUILD | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 1aee8c7016..fa9571a16c 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -51,5 +51,3 @@ cc_library( "@eigen", ], ) - - From 41df92c60cfa3895b55652cdcaa9d874081aa92a Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 14:52:35 -0700 Subject: [PATCH 05/46] test fixture fixes --- src/shared/robot_constants.h | 12 +- .../ball_placement_play_test.py | 4 +- .../kickoff_enemy/kickoff_enemy_play_test.py | 11 +- .../kickoff_friendly_play_fsm.cpp | 2 +- .../kickoff_friendly_play_test.py | 11 +- .../motion_control/orientation_controller.h | 4 +- .../motion_control/position_controller.h | 6 +- src/software/embedded/primitive_executor.cpp | 46 ++- src/software/embedded/primitive_executor.h | 14 +- src/software/embedded/thunderloop.cpp | 12 +- src/software/embedded/thunderloop.h | 6 +- .../simulated_tests/simulated_test_fixture.py | 270 +++++++----------- .../simulation/er_force_simulator.cpp | 16 +- 13 files changed, 168 insertions(+), 246 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 620861d8aa..e4eb334f96 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -159,14 +159,14 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_max_speed_trajectory_m_per_s = 2.5f, - .robot_max_acceleration_m_per_s_2 = 2.0f, - .robot_max_deceleration_m_per_s_2 = 1.5f, + .robot_max_speed_trajectory_m_per_s = 3.0f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 6.0f, - .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f, diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py index 77e4e196ab..ef3132c563 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py @@ -200,7 +200,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, ag_always_validation_sequence_set=[[]], ag_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, - test_timeout_s=[15], + test_timeout_s=15, ) simulated_test_runner.run_test( @@ -209,7 +209,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, ag_always_validation_sequence_set=drop_ball_always_validation_sequence_set, ag_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, - test_timeout_s=[5], + test_timeout_s=5, ) diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py index ce8968de8f..a76fa9b3a7 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py @@ -57,14 +57,6 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.YELLOW ) - # Let robots get ready before starting kickoff - threading.Timer( - 4.0, - lambda: simulated_test_runner.send_gamecontroller_command( - gc_command=Command.Type.NORMAL_START, team=Team.YELLOW - ), - ).start() - simulated_test_runner.set_plays( blue_play=PlayName.KickoffEnemyPlay, yellow_play=PlayName.KickoffFriendlyPlay, @@ -145,6 +137,9 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, + ci_cmd_with_delay=[ + (4.0, Command.Type.NORMAL_START, Team.YELLOW), + ], ) diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp index 7c1afd31f9..a2cb6c42af 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp @@ -42,7 +42,7 @@ void KickoffFriendlyPlayFSM::createKickoffSetupPositions(const WorldPtr& world_p kickoff_setup_positions = { // Robot 1 Point(world_ptr->field().centerPoint() + - Vector(-world_ptr->field().centerCircleRadius(), 0)), + Vector(-0.95 * world_ptr->field().centerCircleRadius(), 0)), // Robot 2 // Goalie positions will be handled by the goalie tactic // Robot 3 diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py index a7e33d5d34..6e85349b02 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py @@ -62,14 +62,6 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.BLUE ) - # Let robots get ready before starting kickoff - threading.Timer( - 4.0, - lambda: simulated_test_runner.send_gamecontroller_command( - gc_command=Command.Type.NORMAL_START, team=Team.BLUE - ), - ).start() - simulated_test_runner.set_plays( blue_play=PlayName.KickoffFriendlyPlay, yellow_play=PlayName.KickoffEnemyPlay, @@ -145,6 +137,9 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, + ci_cmd_with_delay=[ + (4.0, Command.Type.NORMAL_START, Team.BLUE), + ], ) diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 63b90ed6d1..8799380e9b 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -38,7 +38,7 @@ class OrientationController 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}; + PidController w_pid_close_{10.0, 0.0, 4.0, 0.0}; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; }; diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index 12725e09d7..d3fc239d10 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -38,8 +38,8 @@ class PositionController : public MotionController 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}; + PidController x_pid_close_{5.0, 0.0, 0.0, 0.0}; + PidController y_pid_close_{5.0, 0.0, 0.0, 0.0}; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.05; }; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index fea41e7295..66c818dc79 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -12,7 +12,9 @@ PrimitiveExecutor::PrimitiveExecutor( const robot_constants::RobotConstants& robot_constants) - : last_step_time_(std::chrono::steady_clock::now()), robot_constants_(robot_constants) + : time_since_linear_trajectory_creation_(Duration::fromSeconds(0)), + time_since_angular_trajectory_creation_(Duration::fromSeconds(0)), + robot_constants_(robot_constants) { } @@ -35,8 +37,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_linear_traj_new) { trajectory_path_ = new_trajectory_path; - time_since_linear_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } @@ -53,8 +54,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_angular_traj_new) { angular_trajectory_ = new_angular_trajectory; - time_since_angular_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); orientation_controller_.reset(); } } @@ -75,7 +75,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit { const double linear_following_error = (position_ - trajectory_path_->getPosition( - time_since_linear_trajectory_creation_.count())) + time_since_linear_trajectory_creation_.toSeconds())) .length(); if (linear_following_error > LINEAR_STALL_ERROR_MAX_METERS) @@ -85,8 +85,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), position_, velocity_, robot_constants_); - time_since_linear_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); } } @@ -94,7 +93,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit { const double angular_following_error = angular_trajectory_ - ->getPosition(time_since_angular_trajectory_creation_.count()) + ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) .minDiff(orientation_) .toDegrees(); @@ -105,23 +104,16 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit current_primitive_.move().w_traj_params(), orientation_, angular_velocity_, robot_constants_); - time_since_angular_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); } } } std::unique_ptr PrimitiveExecutor::stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status) + TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time) { - const auto current_time = std::chrono::steady_clock::now(); - const auto delta_time_chrono = current_time - last_step_time_; - const Duration delta_time = - Duration::fromSeconds(std::chrono::duration(delta_time_chrono).count()); - - time_since_linear_trajectory_creation_ += delta_time_chrono; - time_since_angular_trajectory_creation_ += delta_time_chrono; - last_step_time_ = current_time; + time_since_linear_trajectory_creation_ = time_since_linear_trajectory_creation_ + delta_time; + time_since_angular_trajectory_creation_ = time_since_angular_trajectory_creation_ + delta_time; status.set_running_primitive(true); @@ -153,16 +145,14 @@ std::unique_ptr PrimitiveExecutor::stepPrimi prim->direct_control()); } - const Duration elapsed_linear = - Duration::fromSeconds(time_since_linear_trajectory_creation_.count()); - const Duration elapsed_angular = - Duration::fromSeconds(time_since_angular_trajectory_creation_.count()); - const Vector target_linear_velocity_global = position_controller_.step( - position_, *trajectory_path_, elapsed_linear, delta_time); + position_, *trajectory_path_, time_since_linear_trajectory_creation_, + delta_time); - const AngularVelocity target_angular_velocity = orientation_controller_.step( - orientation_, *angular_trajectory_, elapsed_angular, delta_time); + const AngularVelocity target_angular_velocity = + orientation_controller_.step(orientation_, *angular_trajectory_, + time_since_angular_trajectory_creation_, + delta_time); Vector target_linear_velocity_local = globalToLocalVelocity(target_linear_velocity_global, orientation_); diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 1be81b2483..bba3069456 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -42,10 +42,11 @@ class PrimitiveExecutor * target wheel velocities * @param status The status of the primitive executor, set to false if current * primitive is a Stop primitive + * @param delta_time The time passed since the last step * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status); + TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time); private: TbotsProto::Primitive current_primitive_; @@ -53,9 +54,8 @@ class PrimitiveExecutor std::optional trajectory_path_; std::optional angular_trajectory_; - std::chrono::time_point last_step_time_; - std::chrono::duration time_since_linear_trajectory_creation_; - std::chrono::duration time_since_angular_trajectory_creation_; + Duration time_since_linear_trajectory_creation_; + Duration time_since_angular_trajectory_creation_; Point position_; Vector velocity_; @@ -77,8 +77,8 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.2; static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.05; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 433b7b0129..aab30afeba 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -192,11 +192,15 @@ void Thunderloop::runLoop() FrameMarkStart(TracyConstants::THUNDERLOOP_FRAME_MARKER); ScopedTimespecTimer iteration_timer(&iteration_time); + const Duration delta_time = Duration::fromSeconds( + getMilliseconds(time_since_prev_iter) * SECONDS_PER_MILLISECOND); + processNetworkPolling(poll_time, last_primitive_received_time); processLocalizationUpdates(); - processPrimitiveExecution(poll_time, last_primitive_received_time); + processPrimitiveExecution(poll_time, last_primitive_received_time, + delta_time); updateChickerStatus(last_chipper_fired, last_kicker_fired); @@ -303,7 +307,8 @@ inline void Thunderloop::processLocalizationUpdates() } inline void Thunderloop::processPrimitiveExecution( - struct timespec& poll_time, struct timespec& last_primitive_received_time) + struct timespec& poll_time, struct timespec& last_primitive_received_time, + const Duration& delta_time) { struct timespec current_time; struct timespec time_since_last_primitive_received; @@ -324,7 +329,8 @@ inline void Thunderloop::processPrimitiveExecution( primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } - direct_control_ = *primitive_executor_.stepPrimitive(primitive_executor_status_); + direct_control_ = + *primitive_executor_.stepPrimitive(primitive_executor_status_, delta_time); } thunderloop_status_.set_primitive_executor_step_time_ms(getMilliseconds(poll_time)); diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 8aa4423991..85464b06de 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -143,9 +143,11 @@ class Thunderloop * * @param poll_time Output for execution duration * @param last_primitive_received_time Input for checking timeouts + * @param delta_time The time passed since the last step */ - inline void processPrimitiveExecution(struct timespec& poll_time, - struct timespec& last_primitive_received_time); + inline void processPrimitiveExecution( + struct timespec& poll_time, struct timespec& last_primitive_received_time, + const Duration& delta_time); /** * Updates internal tracking for chipper/kicker firing events. diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 25e92dce64..ec0659d87f 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -24,10 +24,11 @@ logger = create_logger(__name__) -LAUNCH_DELAY_S = 0.1 -WORLD_BUFFER_TIMEOUT = 0.5 -PROCESS_BUFFER_DELAY_S = 0.01 -PAUSE_AFTER_FAIL_DELAY_S = 3 +LAUNCH_DELAY_S = 2 +WORLD_BUFFER_TIMEOUT = 10 +PROCESS_BUFFER_DELAY_S = 0.1 +PAUSE_AFTER_FAIL_DELAY_S = 5 +SECONDS_PER_MILLISECOND = 0.001 class SimulatedTestRunner(TbotsTestRunner): @@ -101,57 +102,50 @@ def sync_setup(self, setup, param): WorldStateReceivedTrigger, world_state_received_buffer ) - while True: + retries = 0 + MAX_RETRIES = 100 # 10 seconds total at 0.1s sleep + while retries < MAX_RETRIES: setup(param) try: world_state_received_buffer.get( - block=True, timeout=WORLD_BUFFER_TIMEOUT + block=True, timeout=0.1, return_cached=False ) + return except queue.Empty: - # Did not receive a response within timeout period - continue - else: - # Received a response from the simulator - break + retries += 1 + + raise TimeoutError("Timed out waiting for Simulator to receive world state") def runner( self, always_validation_sequence_set, eventually_validation_sequence_set, - test_timeout_s, - tick_duration_s, - ci_cmd_with_delay, - run_till_end, + test_timeout_s=3, + tick_duration_s=1.0 / 60.0, + ci_cmd_with_delay=[], + run_till_end=False, + **kwargs, ): - """Run a test - - :param always_validation_sequence_set: Validation functions that should - hold on every tick - :param eventually_validation_sequence_set: Validation that should - eventually be true, before the test ends - :param test_timeout_s: The timeout for the test, if any eventually_validations - remain after the timeout, the test fails. - :param tick_duration_s: The simulation step duration + """Ticks the simulation forward while running the validations + + :param eventually_validation_sequence_set: validation set that must eventually be true + :param always_validation_sequence_set: validation set that must always be true + :param test_timeout_s: how long the test will run + :param tick_duration_s: how long each simulation step will be + :param run_till_end: if the test should run till the test timeout even if a pass condition is reached :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. - [ - (time, command, team), - (time, command, team), - ... - ] - :param run_till_end: If true, test runs till the end even if eventually validation passes - If false, test stops once eventually validation passes and fails if time out + [(1.0, Command.Type.NORMAL_START, Team.BLUE)] """ time_elapsed_s = 0 - eventually_validation_failure_msg = "Test Timed Out" + eventually_validation_proto_set = None while time_elapsed_s < test_timeout_s: - # get time before we execute the loop - processing_start_time = time.time() + start_time = time.time() # Check for new CI commands at this time step - for delay, cmd, team in ci_cmd_with_delay: + for delay, cmd, team in ci_cmd_with_delay[:]: # If delay matches time if delay <= time_elapsed_s: # send command @@ -163,7 +157,9 @@ def runner( self.simulator_proto_unix_io.send_proto(SimulatorTick, tick) time_elapsed_s += tick_duration_s - while True: + retry_count = 0 + MAX_RETRIES = 5 + while retry_count < MAX_RETRIES: try: world = self.world_buffer.get( block=True, timeout=WORLD_BUFFER_TIMEOUT, return_cached=False @@ -179,24 +175,23 @@ def runner( break except queue.Empty: - # If we timeout, that means full_system missed the last - # wrapper and robot status, lets resend it. - logger.warning("Fullsystem missed last wrapper, resending ...") - - ssl_wrapper = self.ssl_wrapper_buffer.get(block=False) - robot_status = self.robot_status_buffer.get(block=False) - - self.blue_full_system_proto_unix_io.send_proto( - SSL_WrapperPacket, ssl_wrapper + retry_count += 1 + logger.warning( + f"Timeout waiting for world/primitives (retry {retry_count}/{MAX_RETRIES}). Resending SSL Wrapper." ) - self.blue_full_system_proto_unix_io.send_proto( - RobotStatus, robot_status - ) - - # get the time difference after we get the primitive (after any blocking that happened) - processing_time = time.time() - processing_start_time - - # if the time we have blocked is less than a tick, sleep for the remaining time (for Thunderscope only) + # No world or primitives was found within the given timeout. Re-send the SSL wrapper packet and try again. + for packet in self.ssl_wrapper_buffer.get_all(): + self.blue_full_system_proto_unix_io.send_proto( + SSL_WrapperPacket, packet + ) + self.yellow_full_system_proto_unix_io.send_proto( + SSL_WrapperPacket, packet + ) + + if retry_count == MAX_RETRIES: + raise TimeoutError("Timed out waiting for world/primitive updates from AI/Simulator") + + processing_time = time.time() - start_time if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) @@ -248,121 +243,51 @@ def runner( # Check that all eventually validations are eventually valid validation.check_validation(eventually_validation_proto_set) - self.__stopper() @override def run_test( self, - always_validation_sequence_set, - eventually_validation_sequence_set, + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + setup=None, test_timeout_s=3, - tick_duration_s=0.0166, # Default to 60hz - index=0, + tick_duration_s=1.0 / 60.0, ci_cmd_with_delay=[], - run_till_end=True, - **kwargs, - ): - """Helper function to run a test, with thunderscope if enabled - - :param always_validation_sequence_set: validation that should always be true - :param eventually_validation_sequence_set: validation that should eventually be true - :param test_timeout_s: how long the test should run before timing out - :param tick_duration_s: length of a tick - :param index: index of the current test. default is 0 (invariant test) - values can be passed in during aggregate testing for different timeout durations - :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. - [ - (time, command, team), - (time, command, team), - ... - ] - :param run_till_end: If true, test runs till the end even if eventually validation passes - If false, test stops once eventually validation passes and fails if time out - """ - test_timeout_duration = ( - test_timeout_s[index] if type(test_timeout_s) == list else test_timeout_s - ) - - # If thunderscope is enabled, run the test in a thread and show - # thunderscope on this thread. The excepthook is setup to catch - # any test failures and propagate them to the main thread - if self.thunderscope: - run_sim_thread = threading.Thread( - target=self.runner, - daemon=True, - args=[ - always_validation_sequence_set, - eventually_validation_sequence_set, - test_timeout_duration, - tick_duration_s, - ci_cmd_with_delay, - run_till_end, - ], - ) - run_sim_thread.start() - self.thunderscope.show() - run_sim_thread.join() - - if self.last_exception: - pytest.fail(str(self.last_exception)) - - # If thunderscope is disabled, just run the test - else: - self.runner( - always_validation_sequence_set, - eventually_validation_sequence_set, - test_timeout_duration, - tick_duration_s, - ci_cmd_with_delay=ci_cmd_with_delay, - run_till_end=run_till_end, - ) - - -class InvariantTestRunner(SimulatedTestRunner): - """Runs a simulated test only once with a given parameter - - Test passes or fails based on the outcome of this test - """ - - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - - @override - def run_test( - self, - setup=(lambda x: None), - params=[0], - inv_always_validation_sequence_set=[[]], - inv_eventually_validation_sequence_set=[[]], + run_till_end=False, **kwargs, ): - """Run an invariant test + """Begins validating a test based on incoming world protos - :param setup: Function that sets up the World state and the gamecontroller before running the test - :param params: List of parameters for each iteration of the test - (this method only uses the first element) - :param inv_always_validation_sequence_set: Validation functions for invariant testing - that should hold on every tick - :param inv_eventually_validation_sequence_set: Validation functions for invariant testing - that should eventually be true, before the test ends + :param eventually_validation_sequence_set: validation set that must eventually be true + :param always_validation_sequence_set: validation set that must always be true + :param setup: initialization function for this test + :param test_timeout_s: how long the test will run """ - threading.excepthook = self.excepthook - - super().sync_setup(setup, params[0]) - - super().run_test( - inv_always_validation_sequence_set, - inv_eventually_validation_sequence_set, + # Set the hook for exception handling so that we can close the thunderscope + # instance should one exist + sys.excepthook = self.excepthook + + # Only run setup if provided and if we are not being called from AggregateTestRunner + # (which handles its own setup loop) + if setup and "params" not in kwargs: + self.sync_setup(setup, self) + + self.runner( + always_validation_sequence_set=always_validation_sequence_set, + eventually_validation_sequence_set=eventually_validation_sequence_set, + test_timeout_s=test_timeout_s, + tick_duration_s=tick_duration_s, + ci_cmd_with_delay=ci_cmd_with_delay, + run_till_end=run_till_end, **kwargs, ) class AggregateTestRunner(SimulatedTestRunner): - """Runs a simulated test multiple times with different given parameters - - Result of the test is determined by comparing the number of - passing iterations to a predetermined acceptable threshold + """A test runner for aggregate tests. + These tests are a collection of invariant tests. If any of the invariant tests fail, + the aggregate test fails. """ def __init__(self, *args, **kwargs): @@ -377,30 +302,29 @@ def run_test( ag_eventually_validation_sequence_set=[[]], **kwargs, ): - """Run an aggregate test + """Begins validating a test based on incoming world protos. Runs the + invariant test first, then the aggregate test. - :param setup: Function that sets up the World state and the gamecontroller before running the test + :param setup: initialization function for this test :param params: List of parameters for each iteration of the test - :param ag_always_validation_sequence_set: Validation functions for aggregate testing - that should hold on every tick - :param ag_eventually_validation_sequence_set: Validation functions for aggregate testing - that should eventually be true, before the test end + :param ag_eventually_validation_sequence_set: validation set for aggregate test that must eventually be true + :param ag_always_validation_sequence_set: validation set for aggregate test that must always be true """ - threading.excepthook = self.excepthook + sys.excepthook = self.excepthook failed_tests = 0 - # Runs the test once for each given parameter - # Catches Assertion Error thrown by failing test and increments counter - # Calculates overall results and prints them + # Create a copy of kwargs without 'params' to avoid double-setup in SimulatedTestRunner + clean_kwargs = {k: v for k, v in kwargs.items() if k != "params"} + for x in range(len(params)): super().sync_setup(setup, params[x]) try: super().run_test( - ag_always_validation_sequence_set, - ag_eventually_validation_sequence_set, - **kwargs, + always_validation_sequence_set=ag_always_validation_sequence_set, + eventually_validation_sequence_set=ag_eventually_validation_sequence_set, + **clean_kwargs, ) except AssertionError: failed_tests += 1 @@ -494,6 +418,12 @@ def load_command_line_arguments(allow_unrecognized: bool = False): default=False, help="Use realism in the simulator", ) + parser.add_argument( + "--ci_mode", + action="store_true", + default=False, + help="Run in CI mode (faster execution)", + ) return parser.parse_known_args()[0] if allow_unrecognized else parser.parse_args() @@ -542,14 +472,14 @@ def simulated_test_runner(): args.debug_blue_full_system, False, should_restart_on_crash=False, - running_in_realtime=args.enable_thunderscope, + running_in_realtime=not args.ci_mode, ) as blue_fs, FullSystem( "software/unix_full_system", f"{args.yellow_full_system_runtime_dir}/test/{test_name}", args.debug_yellow_full_system, True, should_restart_on_crash=False, - running_in_realtime=args.enable_thunderscope, + running_in_realtime=not args.ci_mode, ) as yellow_fs: with Gamecontroller( suppress_logs=(not args.show_gamecontroller_logs) @@ -580,7 +510,9 @@ def simulated_test_runner(): layout_path=args.layout, ) - time.sleep(LAUNCH_DELAY_S) + # Even in CI mode, give a small delay for processes to start up + actual_launch_delay = 0.5 if args.ci_mode else LAUNCH_DELAY_S + time.sleep(actual_launch_delay) runner = None @@ -595,7 +527,7 @@ def simulated_test_runner(): gamecontroller, ) else: - runner = InvariantTestRunner( + runner = SimulatedTestRunner( current_test, tscope, simulator_proto_unix_io, diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7593b87ce1..b0fa2cad84 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -395,16 +395,18 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( TbotsProto::PrimitiveExecutorStatus status; // Added for compilation if (ramping) { - auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); - direct_control = getRampedVelocityPrimitive( - globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), - std::get<1>(current_state.at(robot_id))), - std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, - primitive_executor_time_step_s); + auto direct_control_no_ramp = primitive_executor->stepPrimitive( + status, Duration::fromSeconds(primitive_executor_time_step_s)); + direct_control = getRampedVelocityPrimitive( + globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), + std::get<1>(current_state.at(robot_id))), + std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, + primitive_executor_time_step_s); } else { - direct_control = primitive_executor->stepPrimitive(status); + direct_control = primitive_executor->stepPrimitive( + status, Duration::fromSeconds(primitive_executor_time_step_s)); } auto command = *getRobotCommandFromDirectControl( From a67712551148112f5c4f8756ac5095e3c058790a Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 14:58:28 -0700 Subject: [PATCH 06/46] test fixture fixes linting --- .../kickoff_enemy/kickoff_enemy_play_test.py | 2 -- .../kickoff_friendly_play_test.py | 2 -- src/software/embedded/primitive_executor.cpp | 17 +++++++++-------- src/software/embedded/thunderloop.cpp | 2 +- src/software/embedded/thunderloop.h | 6 +++--- .../simulated_tests/simulated_test_fixture.py | 5 +++-- 6 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py index a76fa9b3a7..7b31b26718 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py @@ -1,5 +1,3 @@ -import threading - import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py index 6e85349b02..9510feae33 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py @@ -1,5 +1,3 @@ -import threading - import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 66c818dc79..07587da036 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -36,7 +36,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_linear_traj_new) { - trajectory_path_ = new_trajectory_path; + trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } @@ -53,7 +53,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_angular_traj_new) { - angular_trajectory_ = new_angular_trajectory; + angular_trajectory_ = new_angular_trajectory; time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); orientation_controller_.reset(); } @@ -112,8 +112,10 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time) { - time_since_linear_trajectory_creation_ = time_since_linear_trajectory_creation_ + delta_time; - time_since_angular_trajectory_creation_ = time_since_angular_trajectory_creation_ + delta_time; + time_since_linear_trajectory_creation_ = + time_since_linear_trajectory_creation_ + delta_time; + time_since_angular_trajectory_creation_ = + time_since_angular_trajectory_creation_ + delta_time; status.set_running_primitive(true); @@ -149,10 +151,9 @@ std::unique_ptr PrimitiveExecutor::stepPrimi position_, *trajectory_path_, time_since_linear_trajectory_creation_, delta_time); - const AngularVelocity target_angular_velocity = - orientation_controller_.step(orientation_, *angular_trajectory_, - time_since_angular_trajectory_creation_, - delta_time); + const AngularVelocity target_angular_velocity = orientation_controller_.step( + orientation_, *angular_trajectory_, + time_since_angular_trajectory_creation_, delta_time); Vector target_linear_velocity_local = globalToLocalVelocity(target_linear_velocity_global, orientation_); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index aab30afeba..a3d89e1fe0 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -200,7 +200,7 @@ void Thunderloop::runLoop() processLocalizationUpdates(); processPrimitiveExecution(poll_time, last_primitive_received_time, - delta_time); + delta_time); updateChickerStatus(last_chipper_fired, last_kicker_fired); diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 85464b06de..434035c933 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -145,9 +145,9 @@ class Thunderloop * @param last_primitive_received_time Input for checking timeouts * @param delta_time The time passed since the last step */ - inline void processPrimitiveExecution( - struct timespec& poll_time, struct timespec& last_primitive_received_time, - const Duration& delta_time); + inline void processPrimitiveExecution(struct timespec& poll_time, + struct timespec& last_primitive_received_time, + const Duration& delta_time); /** * Updates internal tracking for chipper/kicker firing events. diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index ec0659d87f..87557a278a 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -1,4 +1,3 @@ -import threading import queue import argparse import time @@ -189,7 +188,9 @@ def runner( ) if retry_count == MAX_RETRIES: - raise TimeoutError("Timed out waiting for world/primitive updates from AI/Simulator") + raise TimeoutError( + "Timed out waiting for world/primitive updates from AI/Simulator" + ) processing_time = time.time() - start_time if self.thunderscope and tick_duration_s > processing_time: From 4e4a2c4be48cf3ef74df925f8447a504a19cc322 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 15:04:45 -0700 Subject: [PATCH 07/46] tuning constants pid --- .../embedded/motion_control/orientation_controller.h | 4 ++-- src/software/embedded/motion_control/position_controller.h | 6 +++--- src/software/embedded/primitive_executor.h | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 8799380e9b..63b90ed6d1 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -38,7 +38,7 @@ class OrientationController private: // TODO(#3737): tune constants PidController w_pid_{0.7, 0.0, 2.0, 0.0}; - PidController w_pid_close_{10.0, 0.0, 4.0, 0.0}; + PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index d3fc239d10..12725e09d7 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -38,8 +38,8 @@ class PositionController : public MotionController 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_{5.0, 0.0, 0.0, 0.0}; - PidController y_pid_close_{5.0, 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.05; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; }; diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index bba3069456..a8db26d346 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -77,8 +77,8 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.2; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.05; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; From cc994bef3b27b1520d8778e887cd2c17254595f3 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 15:21:13 -0700 Subject: [PATCH 08/46] tuning constants pid --- src/software/embedded/primitive_executor.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index a8db26d346..d432af01c9 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -77,8 +77,6 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.4; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13.0; }; From 8639b2c258b67cebfdd9ab7d1ea5fb725afd80dc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 29 May 2026 22:34:31 +0000 Subject: [PATCH 09/46] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/primitive_executor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index d432af01c9..26b400c637 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -77,6 +77,6 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.4; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13.0; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.4; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13.0; }; From 573dc741e6255a21e41631d65d871b675c05f130 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 15:54:26 -0700 Subject: [PATCH 10/46] removed old pid --- src/software/util/pid/BUILD | 8 --- src/software/util/pid/pid_controller.hpp | 82 ------------------------ 2 files changed, 90 deletions(-) delete mode 100644 src/software/util/pid/BUILD delete mode 100644 src/software/util/pid/pid_controller.hpp diff --git a/src/software/util/pid/BUILD b/src/software/util/pid/BUILD deleted file mode 100644 index 156e74085e..0000000000 --- a/src/software/util/pid/BUILD +++ /dev/null @@ -1,8 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "pid", - hdrs = [ - "pid_controller.hpp", - ], -) diff --git a/src/software/util/pid/pid_controller.hpp b/src/software/util/pid/pid_controller.hpp deleted file mode 100644 index d3e673efc2..0000000000 --- a/src/software/util/pid/pid_controller.hpp +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once - -#include - -namespace controls -{ -template -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(T k_p, T k_i, T k_d, T max_integral); - - /** - * Given an error, returns the control effort to minimize it. - * - * @param deltaTime The time passed since last step, for calculating integrator and - *derivative. If this function is calling in invariant intervals, deltaTime can be set - *to 1 and any effects it would have can be handled by tuning kD. - **/ - T step(T error, T delta_time = 1.0f); - - /** - * Resets the integrator and clears the last error used for derivative calculation. - **/ - void reset(); - - T k_p; - T k_i; - T k_d; - T max_integral; - - protected: - T integral; - std::optional last_error = std::nullopt; -}; -} // namespace controls - -template -controls::PIDController::PIDController(T k_p, T k_i, T k_d, T max_integral) - : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral){}; - -template -T controls::PIDController::step(T error, T delta_time) -{ - // If sign of error swaps, reset integrator - if (last_error.value_or(0) * error < 0) - { - integral = 0; - } - - integral += std::max(std::min(error * delta_time, max_integral), -max_integral); - // set derivative, if no last_error, just set to 0 - const T derivative = (last_error.value_or(error) - error) / delta_time; - last_error = error; - return error * k_p + integral * k_i + derivative * k_d; -} - -template -void controls::PIDController::reset() -{ - integral = 0; - last_error = std::nullopt; -} From 39d7d7b0e33a5aa4ef01bfe41e61b307bb8f446b Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Fri, 29 May 2026 16:52:30 -0700 Subject: [PATCH 11/46] tuning fixes --- src/software/embedded/BUILD | 1 - .../embedded/motion_control/orientation_controller.h | 4 ++-- .../motor_controller/stspin_motor_controller.cpp | 4 ++-- src/software/embedded/services/motor.cpp | 10 +++++----- src/software/embedded/thunderloop.cpp | 5 +++-- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 6bc28d668e..e56929c45f 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -39,7 +39,6 @@ cc_library( "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", - "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", ], diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 63b90ed6d1..ddd74f03d3 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -37,8 +37,8 @@ class OrientationController 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}; + PidController w_pid_{0.7, 0.0, 0.02, 0.0}; + PidController w_pid_close_{4.0, 0.0, 0.2, 0.0}; static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 7324b0a6a4..d28a38f390 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -189,8 +189,8 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, void StSpinMotorController::updateEuclideanVelocity( EuclideanSpace_t target_euclidean_velocity) { - const Vector local_velocity(target_euclidean_velocity[1], - -target_euclidean_velocity[0]); + const Vector local_velocity(target_euclidean_velocity[0], + target_euclidean_velocity[1]); if (local_velocity.length() <= MINIMUM_SPEED_FOR_FEED_FORWARD) { diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 6c838b0c5a..f2fefbba19 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -171,9 +171,9 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor euclidean_to_four_wheel_.getEuclideanVelocity(current_wheel_velocities); motor_status.mutable_local_velocity()->set_x_component_meters( - current_euclidean_velocity[1]); + current_euclidean_velocity[0]); motor_status.mutable_local_velocity()->set_y_component_meters( - -current_euclidean_velocity[0]); + current_euclidean_velocity[1]); motor_status.mutable_angular_velocity()->set_radians_per_second( current_euclidean_velocity[2]); @@ -194,8 +194,8 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor const auto& direct_velocity = motor_control.direct_velocity_control(); const EuclideanSpace_t target_euclidean_velocity = { - -direct_velocity.velocity().y_component_meters(), direct_velocity.velocity().x_component_meters(), + direct_velocity.velocity().y_component_meters(), direct_velocity.angular_velocity().radians_per_second()}; motor_controller_->updateEuclideanVelocity(target_euclidean_velocity); @@ -213,9 +213,9 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor euclidean_to_four_wheel_.getEuclideanVelocity(target_wheel_velocities_); motor_status.mutable_target_local_velocity()->set_x_component_meters( - target_euclidean_velocity[1]); + target_euclidean_velocity[0]); motor_status.mutable_target_local_velocity()->set_y_component_meters( - -target_euclidean_velocity[0]); + target_euclidean_velocity[1]); motor_status.mutable_target_angular_velocity()->set_radians_per_second( target_euclidean_velocity[2]); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index a3d89e1fe0..7433ed2306 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -254,7 +254,8 @@ inline void Thunderloop::processNetworkPolling( const Point position = createPoint(primitive_.move().xy_traj_params().start_position()); const Angle orientation = - createAngle(primitive_.move().w_traj_params().start_angle()); + createAngle(primitive_.move().w_traj_params().start_angle()) + + Angle::half(); robot_localizer_.update( RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); @@ -295,7 +296,7 @@ inline void Thunderloop::processLocalizationUpdates() if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) { const auto accel = imu_poll->linear_acceleration.value(); - linear_acceleration = Vector(accel[1], accel[0]); + linear_acceleration = Vector(accel[0], accel[1]); } robot_localizer_.step(linear_acceleration); From 24a50acf02543247a3f86f189b84e399b67571b4 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Fri, 29 May 2026 16:52:47 -0700 Subject: [PATCH 12/46] tuning fixes --- src/shared/robot_constants.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index e4eb334f96..4e60d0ee90 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -159,14 +159,14 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_max_speed_trajectory_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_trajectory_m_per_s = 2.5f, + .robot_max_acceleration_m_per_s_2 = 2.0f, + .robot_max_deceleration_m_per_s_2 = 1.5f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 6.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, .wheel_radius_meters = 0.03f, @@ -174,7 +174,7 @@ constexpr RobotConstants createRobotConstants() // Kalman filter variances for robot localizer .kalman_process_noise_variance_rad_per_s_4 = 1.0f, - .kalman_vision_noise_variance_rad_2 = 0.01f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5}; } #elif CHECK_VERSION(2021) From acedc5a2ea6b7e4a06fb7418e45b3aff08fbf168 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sun, 31 May 2026 13:05:50 -0700 Subject: [PATCH 13/46] refactor and localizer fixes --- .../trajectory/bang_bang_trajectory_1d.h | 14 +++---- .../bang_bang_trajectory_1d_angular.h | 14 +++---- .../ai/navigator/trajectory/trajectory.hpp | 11 ++--- .../ai/navigator/trajectory/trajectory_2d.h | 14 +++---- src/software/embedded/primitive_executor.cpp | 10 ++--- src/software/embedded/robot_localizer.cpp | 41 ++++++++++++------- .../field_tests/field_test_fixture.py | 30 +++++--------- 7 files changed, 69 insertions(+), 65 deletions(-) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index 4095faea25..4bce8c76df 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -105,16 +105,16 @@ class BangBangTrajectory1D : public Trajectory size_t getNumTrajectoryParts() const; /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise */ - bool isNew(const Trajectory& other, - double threshold) const override + bool equals(const Trajectory& other, + double threshold) const override { - return std::abs(getDestination() - other.getDestination()) > threshold; + return std::abs(getDestination() - other.getDestination()) <= threshold; } private: diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h index 859034685e..3c87b039c2 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h @@ -84,16 +84,16 @@ class BangBangTrajectory1DAngular double getTotalTime() const override; /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different in degrees. - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal in degrees. + * @return True if the trajectories are equal, false otherwise */ - bool isNew(const Trajectory& other, - double threshold) const override + bool equals(const Trajectory& other, + double threshold) const override { - return getDestination().minDiff(other.getDestination()).toDegrees() > threshold; + return getDestination().minDiff(other.getDestination()).toDegrees() <= threshold; } private: diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 4f970c2e7a..b147f24684 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -61,11 +61,12 @@ class Trajectory } /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise */ - virtual bool isNew(const Trajectory& other, double threshold) const = 0; + virtual bool equals(const Trajectory& other, + double threshold) const = 0; }; diff --git a/src/software/ai/navigator/trajectory/trajectory_2d.h b/src/software/ai/navigator/trajectory/trajectory_2d.h index 4f21a9a25b..e40b367f58 100644 --- a/src/software/ai/navigator/trajectory/trajectory_2d.h +++ b/src/software/ai/navigator/trajectory/trajectory_2d.h @@ -17,15 +17,15 @@ class Trajectory2D : virtual public Trajectory virtual std::vector getBoundingBoxes() const = 0; /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise */ - bool isNew(const Trajectory& other, - double threshold) const override + bool equals(const Trajectory& other, + double threshold) const override { - return distance(getDestination(), other.getDestination()) > threshold; + return distance(getDestination(), other.getDestination()) <= threshold; } }; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 07587da036..831a5e6af6 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -31,12 +31,12 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m const bool is_linear_traj_new = (new_trajectory_path.has_value() != trajectory_path_.has_value()) || (new_trajectory_path.has_value() && - trajectory_path_->isNew(*new_trajectory_path, - LINEAR_DESTINATION_THRESHOLD_METERS)); + !trajectory_path_->equals(*new_trajectory_path, + LINEAR_DESTINATION_THRESHOLD_METERS)); if (is_linear_traj_new) { - trajectory_path_ = new_trajectory_path; + trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } @@ -48,8 +48,8 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m const bool is_angular_traj_new = !angular_trajectory_.has_value() || - angular_trajectory_->isNew(new_angular_trajectory, - ANGULAR_DESTINATION_THRESHOLD_DEGREES); + !angular_trajectory_->equals(new_angular_trajectory, + ANGULAR_DESTINATION_THRESHOLD_DEGREES); if (is_angular_traj_new) { diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 41744aa047..73f24164eb 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -139,45 +139,58 @@ void RobotLocalizer::update(const VisionData& data) const auto current_time = std::chrono::steady_clock::now(); const auto sample_age = std::chrono::duration(data.age_seconds); - const auto rollback_point = std::find_if( + auto rollback_point = std::find_if( history.begin(), history.end(), - [&](const FilterStep& step) { return step.time - current_time >= sample_age; }); + [&](const FilterStep& step) { return (current_time - step.time) >= sample_age; }); if (rollback_point == history.begin()) { - // All history predates the sample, no need to rollback + // All history predates the sample, or is exactly at the same time. + // No need to rollback, just apply to the current state. updateFilterWithVision(data.position, data.orientation); + history.clear(); // Safe to clear, since all history is older than current state return; } - auto replay_iter = std::make_reverse_iterator(rollback_point); + if (rollback_point == history.end()) + { + // The vision sample is older than our entire history. + // Roll back as far as we can (to the oldest step). + rollback_point = std::prev(history.end()); + } - // Truncate history after the rollback point - history.erase(rollback_point, history.end()); + // 1. Save the state from the rollback point + filter_.state_estimate = rollback_point->state_estimate; + filter_.state_covariance = rollback_point->state_covariance; - filter_.state_estimate = replay_iter->state_estimate; - filter_.state_covariance = replay_iter->state_covariance; + // 2. Truncate history (erase rollback_point and everything older) + history.erase(rollback_point, history.end()); + // 3. Apply the delayed vision measurement updateFilterWithVision(data.position, data.orientation); - // Replay from the rollback point back to the current estimate - for (; replay_iter != history.rbegin(); --replay_iter) + // 4. Replay the remaining history (from oldest to newest) + for (auto it = history.rbegin(); it != history.rend(); ++it) { - if (replay_iter->prediction.has_value()) + if (it->prediction.has_value()) { - const auto& prediction = replay_iter->prediction.value(); + const auto& prediction = it->prediction.value(); filter_.process_model = prediction.process_model; filter_.process_covariance = prediction.process_covariance; filter_.control_model = prediction.control_model; filter_.predict(prediction.control_input); } - if (replay_iter->update.has_value()) + if (it->update.has_value()) { - const auto& update = replay_iter->update.value(); + const auto& update = it->update.value(); filter_.measurement_model = update.measurement_model; filter_.update(update.measurement); } + + // IMPORTANT: Update the history with the recomputed state so future rollbacks are correct + it->state_estimate = filter_.state_estimate; + it->state_covariance = filter_.state_covariance; } } diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index 9535e727d1..e4c857d15a 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -363,13 +363,6 @@ def load_command_line_arguments(): help="Disables checking for estop plugged in (ONLY USE FOR LOCAL TESTING)", ) - parser.add_argument( - "--no_visualization", - action="store_true", - default=False, - help="Disables the Thunderscope GUI", - ) - return parser.parse_args() @@ -436,17 +429,15 @@ def field_test_runner(): simulator_proto_unix_io=simulator_proto_unix_io, ) # Inject the proto unix ios into thunderscope and start the test - tscope = None - if not args.no_visualization: - tscope = Thunderscope( - configure_field_test_view( - simulator_proto_unix_io=simulator_proto_unix_io, - blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, - yellow_is_friendly=args.run_yellow, - ), - layout_path=None, - ) + tscope = Thunderscope( + configure_field_test_view( + simulator_proto_unix_io=simulator_proto_unix_io, + blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, + yellow_is_friendly=args.run_yellow, + ), + layout_path=None, + ) # Set control mode for all robots to AI so that packets are sent to the robots for robot_id in range(MAX_ROBOT_IDS_PER_SIDE): @@ -457,8 +448,7 @@ def field_test_runner(): # connect the keyboard estop toggle to the key event if needed if estop_mode == EstopMode.KEYBOARD_ESTOP: - if tscope: - tscope.keyboard_estop_shortcut.activated.connect( + tscope.keyboard_estop_shortcut.activated.connect( rc_friendly.toggle_keyboard_estop ) # we call this method to enable estop automatically when a field test starts From 244f17c28cda434ae0cf348ab30947dc229ee679 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 14:56:18 -0700 Subject: [PATCH 14/46] sim tests --- .../simulated_tests/simulated_test_fixture.py | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 87557a278a..edd312b7e8 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -23,10 +23,10 @@ logger = create_logger(__name__) -LAUNCH_DELAY_S = 2 -WORLD_BUFFER_TIMEOUT = 10 +LAUNCH_DELAY_S = 0.1 +WORLD_BUFFER_TIMEOUT = 0.5 PROCESS_BUFFER_DELAY_S = 0.1 -PAUSE_AFTER_FAIL_DELAY_S = 5 +PAUSE_AFTER_FAIL_DELAY_S = 3 SECONDS_PER_MILLISECOND = 0.001 @@ -122,7 +122,7 @@ def runner( eventually_validation_sequence_set, test_timeout_s=3, tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=[], + ci_cmd_with_delay=None, run_till_end=False, **kwargs, ): @@ -136,6 +136,7 @@ def runner( :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. [(1.0, Command.Type.NORMAL_START, Team.BLUE)] """ + ci_cmd_with_delay = list(ci_cmd_with_delay) if ci_cmd_with_delay else [] time_elapsed_s = 0 eventually_validation_failure_msg = "Test Timed Out" eventually_validation_proto_set = None @@ -249,12 +250,12 @@ def runner( @override def run_test( self, - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], + always_validation_sequence_set=None, + eventually_validation_sequence_set=None, setup=None, test_timeout_s=3, tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=[], + ci_cmd_with_delay=None, run_till_end=False, **kwargs, ): @@ -265,6 +266,8 @@ def run_test( :param setup: initialization function for this test :param test_timeout_s: how long the test will run """ + always_validation_sequence_set = always_validation_sequence_set or [[]] + eventually_validation_sequence_set = eventually_validation_sequence_set or [[]] # Set the hook for exception handling so that we can close the thunderscope # instance should one exist sys.excepthook = self.excepthook @@ -298,9 +301,9 @@ def __init__(self, *args, **kwargs): def run_test( self, setup=(lambda arg: None), - params=[], - ag_always_validation_sequence_set=[[]], - ag_eventually_validation_sequence_set=[[]], + params=None, + ag_always_validation_sequence_set=None, + ag_eventually_validation_sequence_set=None, **kwargs, ): """Begins validating a test based on incoming world protos. Runs the @@ -311,6 +314,9 @@ def run_test( :param ag_eventually_validation_sequence_set: validation set for aggregate test that must eventually be true :param ag_always_validation_sequence_set: validation set for aggregate test that must always be true """ + params = params or [] + ag_always_validation_sequence_set = ag_always_validation_sequence_set or [[]] + ag_eventually_validation_sequence_set = ag_eventually_validation_sequence_set or [[]] sys.excepthook = self.excepthook failed_tests = 0 From 96c20c1663388686b971fa820a7f4aade1e1df3b Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 14:57:25 -0700 Subject: [PATCH 15/46] linting --- src/software/ai/navigator/trajectory/trajectory.hpp | 3 +-- src/software/embedded/primitive_executor.cpp | 2 +- src/software/embedded/robot_localizer.cpp | 5 +++-- src/software/field_tests/field_test_fixture.py | 4 ++-- src/software/simulated_tests/simulated_test_fixture.py | 4 +++- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index b147f24684..caffd6b86c 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -67,6 +67,5 @@ class Trajectory * equal * @return True if the trajectories are equal, false otherwise */ - virtual bool equals(const Trajectory& other, - double threshold) const = 0; + virtual bool equals(const Trajectory& other, double threshold) const = 0; }; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 831a5e6af6..89f329753e 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -36,7 +36,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_linear_traj_new) { - trajectory_path_ = new_trajectory_path; + trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 73f24164eb..a095983d90 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -148,7 +148,7 @@ void RobotLocalizer::update(const VisionData& data) // All history predates the sample, or is exactly at the same time. // No need to rollback, just apply to the current state. updateFilterWithVision(data.position, data.orientation); - history.clear(); // Safe to clear, since all history is older than current state + history.clear(); // Safe to clear, since all history is older than current state return; } @@ -188,7 +188,8 @@ void RobotLocalizer::update(const VisionData& data) filter_.update(update.measurement); } - // IMPORTANT: Update the history with the recomputed state so future rollbacks are correct + // IMPORTANT: Update the history with the recomputed state so future rollbacks are + // correct it->state_estimate = filter_.state_estimate; it->state_covariance = filter_.state_covariance; } diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index e4c857d15a..a67bc341a4 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -449,8 +449,8 @@ def field_test_runner(): # connect the keyboard estop toggle to the key event if needed if estop_mode == EstopMode.KEYBOARD_ESTOP: tscope.keyboard_estop_shortcut.activated.connect( - rc_friendly.toggle_keyboard_estop - ) + rc_friendly.toggle_keyboard_estop + ) # we call this method to enable estop automatically when a field test starts rc_friendly.toggle_keyboard_estop() logger.warning( diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index edd312b7e8..4d048d995a 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -316,7 +316,9 @@ def run_test( """ params = params or [] ag_always_validation_sequence_set = ag_always_validation_sequence_set or [[]] - ag_eventually_validation_sequence_set = ag_eventually_validation_sequence_set or [[]] + ag_eventually_validation_sequence_set = ( + ag_eventually_validation_sequence_set or [[]] + ) sys.excepthook = self.excepthook failed_tests = 0 From 1bd2cd9002e7bdefb14065305c3e60bbff1f52f6 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 15:47:10 -0700 Subject: [PATCH 16/46] test fixes --- .../message_translation/tbots_protobuf.cpp | 2 +- src/shared/robot_constants.h | 14 ++++++------ src/software/ai/evaluation/intercept.cpp | 2 +- src/software/embedded/primitive_executor.cpp | 5 +++++ src/software/embedded/thunderloop.cpp | 22 +++++++++---------- src/software/world/robot.cpp | 2 +- 6 files changed, 26 insertions(+), 21 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 23e7d0c51c..2ba0c2c6b9 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( switch (max_allowed_speed_mode) { case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT: - return robot_constants.robot_max_speed_trajectory_m_per_s; + return robot_constants.robot_trajectory_max_speed_m_per_s; case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND: return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND - STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND; diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 4e60d0ee90..0ab068eded 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -90,7 +90,7 @@ struct RobotConstants // The maximum speed that the trajectory planner is allowed to command the robot to // move at, while still leaving headroom for the PID to apply correction on lag. [m/s] - float robot_max_speed_trajectory_m_per_s; + float robot_trajectory_max_speed_m_per_s; // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -159,14 +159,14 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_max_speed_trajectory_m_per_s = 2.5f, - .robot_max_acceleration_m_per_s_2 = 2.0f, - .robot_max_deceleration_m_per_s_2 = 1.5f, + .robot_trajectory_max_speed_m_per_s = 3.0f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 6.0f, - .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f, diff --git a/src/software/ai/evaluation/intercept.cpp b/src/software/ai/evaluation/intercept.cpp index 2c729dd516..ea4fa9099c 100644 --- a/src/software/ai/evaluation/intercept.cpp +++ b/src/software/ai/evaluation/intercept.cpp @@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p Point best_position = intercept_position; double final_speed = step_speed; bool finished = false; - double max_speed = robot.robotConstants().robot_max_speed_trajectory_m_per_s; + double max_speed = robot.robotConstants().robot_trajectory_max_speed_m_per_s; double max_acc = robot.robotConstants().robot_max_acceleration_m_per_s_2; while (!finished) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 89f329753e..691889c4a7 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -69,6 +69,11 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit orientation_ = orientation; angular_velocity_ = angular_velocity; + if (!current_primitive_.has_move()) + { + return; + } + // If we are lagging behind trajectory too much, we have stalled! We need to // regenerate trajectory. if (trajectory_path_.has_value()) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 7433ed2306..b23926e9fe 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -291,20 +291,20 @@ inline void Thunderloop::processLocalizationUpdates() localToGlobalVelocity(createVector(status.local_velocity()), robot_localizer_.getOrientation()), createAngularVelocity(status.angular_velocity())}); + } - Vector linear_acceleration; - if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) - { - const auto accel = imu_poll->linear_acceleration.value(); - linear_acceleration = Vector(accel[0], accel[1]); - } + Vector linear_acceleration; + if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) + { + const auto accel = imu_poll->linear_acceleration.value(); + linear_acceleration = Vector(accel[0], accel[1]); + } - robot_localizer_.step(linear_acceleration); + robot_localizer_.step(linear_acceleration); - primitive_executor_.updateState( - robot_localizer_.getPosition(), robot_localizer_.getVelocity(), - robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity()); - } + primitive_executor_.updateState( + robot_localizer_.getPosition(), robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity()); } inline void Thunderloop::processPrimitiveExecution( diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 5a13d519a6..2b987e7b60 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -219,7 +219,7 @@ Duration Robot::getTimeToPosition(const Point& destination, double final_velocity_1d = final_velocity.dot(dist_vector.normalize()); return getTimeToTravelDistance(dist, - robot_constants_.robot_max_speed_trajectory_m_per_s, + robot_constants_.robot_trajectory_max_speed_m_per_s, robot_constants_.robot_max_acceleration_m_per_s_2, initial_velocity_1d, final_velocity_1d); } From 4b62eaebc9afe5ee8c8d8bde64d08934e169e1b1 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 16:43:05 -0700 Subject: [PATCH 17/46] more euclid change --- .../embedded/motor_controller/stspin_motor_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index d28a38f390..e28ccfb402 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -189,8 +189,8 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, void StSpinMotorController::updateEuclideanVelocity( EuclideanSpace_t target_euclidean_velocity) { - const Vector local_velocity(target_euclidean_velocity[0], - target_euclidean_velocity[1]); + const Vector local_velocity(target_euclidean_velocity[1], + target_euclidean_velocity[0]); if (local_velocity.length() <= MINIMUM_SPEED_FOR_FEED_FORWARD) { From 9335cb7d1edd22716365dbc6cc894293525882f0 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 16:48:46 -0700 Subject: [PATCH 18/46] more euclid change --- src/software/physics/euclidean_to_wheel.cpp | 8 +-- .../physics/euclidean_to_wheel_test.cpp | 58 +++++++++---------- 2 files changed, 33 insertions(+), 33 deletions(-) diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 63c24d9666..815077f402 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -39,10 +39,10 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ // clang-format off euclidean_to_wheel_velocity_D_ << - std::cos(b_fr), std::sin(b_fr), (fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)), - std::cos(b_fl), std::sin(b_fl), (fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)), - std::cos(b_bl), std::sin(b_bl), (bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)), - std::cos(b_br), std::sin(b_br), (br_x * std::sin(b_br) - br_y * std::cos(b_br)); + -std::cos(b_fr), -std::sin(b_fr), -(fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)), + -std::cos(b_fl), -std::sin(b_fl), -(fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)), + -std::cos(b_bl), -std::sin(b_bl), -(bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)), + -std::cos(b_br), -std::sin(b_br), -(br_x * std::sin(b_br) - br_y * std::cos(b_br)); // clang-format on // Inverse of euclidean to wheel matrix (D) for converting wheel velocity to euclidean diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index 0890f6cde9..ff4fb52d1d 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -48,11 +48,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_x) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be - velocity, left wheels must be + velocity. - EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Right wheels must be + velocity, left wheels must be - velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); // Right wheels must have same velocity magnitude as left wheels, but opposite sign. @@ -69,11 +69,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_x) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be + velocity, left wheels must be - velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Right wheels must be - velocity, left wheels must be + velocity. + EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); // Right wheels must have same velocity magnitude as left wheels, but opposite sign. EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], @@ -89,11 +89,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_y) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Back wheels must be + velocity, front wheels must be - velocity. - EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Back wheels must be - velocity, front wheels must be + velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) @@ -103,11 +103,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Back wheels must be - velocity, front wheels must be + velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Back wheels must be + velocity, front wheels must be - velocity. + EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_sign) @@ -117,11 +117,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_sign) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be + velocity, Left wheels must be - velocity. - EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Right wheels must be - velocity, Left wheels must be + velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_sign) @@ -132,10 +132,10 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_sign) euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); // Right wheels must be + velocity, Left wheels must be - velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) From d1f3efb6c74a517b5e700bc71bf6f5948216be79 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 13:58:21 -0700 Subject: [PATCH 19/46] temporary fix for ai vs ai --- src/software/simulation/er_force_simulator.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index b0fa2cad84..a0b3c9ee51 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -311,8 +311,8 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const auto& [position, orientation, velocity, angular_velocity] = robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, - world_proto, position, orientation, velocity, - angular_velocity); + world_proto, -position, orientation + Angle::half(), + velocity, angular_velocity); } } } @@ -384,6 +384,12 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { const auto& sim_robots = sim_state.yellow_robots(); current_state = getRobotIdToStateMap(sim_robots); + for (auto& [robot_id, state] : current_state) + { + auto& [position, orientation, velocity, angular_velocity] = state; + position = -position; + orientation = orientation + Angle::half(); + } } for (auto& primitive_executor_with_id : robot_primitive_executor_map) From 6e0bc08f08c9eb2e2f9a7edcdd4af19cc43f2bef Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Thu, 4 Jun 2026 11:45:36 -0700 Subject: [PATCH 20/46] reseting --- .../ball_placement_play_test.py | 4 +- .../kickoff_enemy/kickoff_enemy_play_test.py | 13 +- .../kickoff_friendly_play_fsm.cpp | 2 +- .../kickoff_friendly_play_test.py | 13 +- .../simulated_tests/simulated_test_fixture.py | 281 +++++++++++------- 5 files changed, 193 insertions(+), 120 deletions(-) diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py index ef3132c563..77e4e196ab 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py @@ -200,7 +200,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, ag_always_validation_sequence_set=[[]], ag_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, - test_timeout_s=15, + test_timeout_s=[15], ) simulated_test_runner.run_test( @@ -209,7 +209,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, ag_always_validation_sequence_set=drop_ball_always_validation_sequence_set, ag_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, - test_timeout_s=5, + test_timeout_s=[5], ) diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py index 7b31b26718..ce8968de8f 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py @@ -1,3 +1,5 @@ +import threading + import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName @@ -55,6 +57,14 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.YELLOW ) + # Let robots get ready before starting kickoff + threading.Timer( + 4.0, + lambda: simulated_test_runner.send_gamecontroller_command( + gc_command=Command.Type.NORMAL_START, team=Team.YELLOW + ), + ).start() + simulated_test_runner.set_plays( blue_play=PlayName.KickoffEnemyPlay, yellow_play=PlayName.KickoffFriendlyPlay, @@ -135,9 +145,6 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, - ci_cmd_with_delay=[ - (4.0, Command.Type.NORMAL_START, Team.YELLOW), - ], ) diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp index a2cb6c42af..7c1afd31f9 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp @@ -42,7 +42,7 @@ void KickoffFriendlyPlayFSM::createKickoffSetupPositions(const WorldPtr& world_p kickoff_setup_positions = { // Robot 1 Point(world_ptr->field().centerPoint() + - Vector(-0.95 * world_ptr->field().centerCircleRadius(), 0)), + Vector(-world_ptr->field().centerCircleRadius(), 0)), // Robot 2 // Goalie positions will be handled by the goalie tactic // Robot 3 diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py index 9510feae33..a7e33d5d34 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py @@ -1,3 +1,5 @@ +import threading + import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName @@ -60,6 +62,14 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.BLUE ) + # Let robots get ready before starting kickoff + threading.Timer( + 4.0, + lambda: simulated_test_runner.send_gamecontroller_command( + gc_command=Command.Type.NORMAL_START, team=Team.BLUE + ), + ).start() + simulated_test_runner.set_plays( blue_play=PlayName.KickoffFriendlyPlay, yellow_play=PlayName.KickoffEnemyPlay, @@ -135,9 +145,6 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, - ci_cmd_with_delay=[ - (4.0, Command.Type.NORMAL_START, Team.BLUE), - ], ) diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 4d048d995a..25e92dce64 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -1,3 +1,4 @@ +import threading import queue import argparse import time @@ -25,9 +26,8 @@ LAUNCH_DELAY_S = 0.1 WORLD_BUFFER_TIMEOUT = 0.5 -PROCESS_BUFFER_DELAY_S = 0.1 +PROCESS_BUFFER_DELAY_S = 0.01 PAUSE_AFTER_FAIL_DELAY_S = 3 -SECONDS_PER_MILLISECOND = 0.001 class SimulatedTestRunner(TbotsTestRunner): @@ -101,51 +101,57 @@ def sync_setup(self, setup, param): WorldStateReceivedTrigger, world_state_received_buffer ) - retries = 0 - MAX_RETRIES = 100 # 10 seconds total at 0.1s sleep - while retries < MAX_RETRIES: + while True: setup(param) try: world_state_received_buffer.get( - block=True, timeout=0.1, return_cached=False + block=True, timeout=WORLD_BUFFER_TIMEOUT ) - return except queue.Empty: - retries += 1 - - raise TimeoutError("Timed out waiting for Simulator to receive world state") + # Did not receive a response within timeout period + continue + else: + # Received a response from the simulator + break def runner( self, always_validation_sequence_set, eventually_validation_sequence_set, - test_timeout_s=3, - tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=None, - run_till_end=False, - **kwargs, + test_timeout_s, + tick_duration_s, + ci_cmd_with_delay, + run_till_end, ): - """Ticks the simulation forward while running the validations - - :param eventually_validation_sequence_set: validation set that must eventually be true - :param always_validation_sequence_set: validation set that must always be true - :param test_timeout_s: how long the test will run - :param tick_duration_s: how long each simulation step will be - :param run_till_end: if the test should run till the test timeout even if a pass condition is reached + """Run a test + + :param always_validation_sequence_set: Validation functions that should + hold on every tick + :param eventually_validation_sequence_set: Validation that should + eventually be true, before the test ends + :param test_timeout_s: The timeout for the test, if any eventually_validations + remain after the timeout, the test fails. + :param tick_duration_s: The simulation step duration :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. - [(1.0, Command.Type.NORMAL_START, Team.BLUE)] + [ + (time, command, team), + (time, command, team), + ... + ] + :param run_till_end: If true, test runs till the end even if eventually validation passes + If false, test stops once eventually validation passes and fails if time out """ - ci_cmd_with_delay = list(ci_cmd_with_delay) if ci_cmd_with_delay else [] time_elapsed_s = 0 + eventually_validation_failure_msg = "Test Timed Out" - eventually_validation_proto_set = None while time_elapsed_s < test_timeout_s: - start_time = time.time() + # get time before we execute the loop + processing_start_time = time.time() # Check for new CI commands at this time step - for delay, cmd, team in ci_cmd_with_delay[:]: + for delay, cmd, team in ci_cmd_with_delay: # If delay matches time if delay <= time_elapsed_s: # send command @@ -157,9 +163,7 @@ def runner( self.simulator_proto_unix_io.send_proto(SimulatorTick, tick) time_elapsed_s += tick_duration_s - retry_count = 0 - MAX_RETRIES = 5 - while retry_count < MAX_RETRIES: + while True: try: world = self.world_buffer.get( block=True, timeout=WORLD_BUFFER_TIMEOUT, return_cached=False @@ -175,25 +179,24 @@ def runner( break except queue.Empty: - retry_count += 1 - logger.warning( - f"Timeout waiting for world/primitives (retry {retry_count}/{MAX_RETRIES}). Resending SSL Wrapper." + # If we timeout, that means full_system missed the last + # wrapper and robot status, lets resend it. + logger.warning("Fullsystem missed last wrapper, resending ...") + + ssl_wrapper = self.ssl_wrapper_buffer.get(block=False) + robot_status = self.robot_status_buffer.get(block=False) + + self.blue_full_system_proto_unix_io.send_proto( + SSL_WrapperPacket, ssl_wrapper ) - # No world or primitives was found within the given timeout. Re-send the SSL wrapper packet and try again. - for packet in self.ssl_wrapper_buffer.get_all(): - self.blue_full_system_proto_unix_io.send_proto( - SSL_WrapperPacket, packet - ) - self.yellow_full_system_proto_unix_io.send_proto( - SSL_WrapperPacket, packet - ) - - if retry_count == MAX_RETRIES: - raise TimeoutError( - "Timed out waiting for world/primitive updates from AI/Simulator" - ) + self.blue_full_system_proto_unix_io.send_proto( + RobotStatus, robot_status + ) + + # get the time difference after we get the primitive (after any blocking that happened) + processing_time = time.time() - processing_start_time - processing_time = time.time() - start_time + # if the time we have blocked is less than a tick, sleep for the remaining time (for Thunderscope only) if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) @@ -245,53 +248,121 @@ def runner( # Check that all eventually validations are eventually valid validation.check_validation(eventually_validation_proto_set) + self.__stopper() @override def run_test( self, - always_validation_sequence_set=None, - eventually_validation_sequence_set=None, - setup=None, + always_validation_sequence_set, + eventually_validation_sequence_set, test_timeout_s=3, - tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=None, - run_till_end=False, + tick_duration_s=0.0166, # Default to 60hz + index=0, + ci_cmd_with_delay=[], + run_till_end=True, + **kwargs, + ): + """Helper function to run a test, with thunderscope if enabled + + :param always_validation_sequence_set: validation that should always be true + :param eventually_validation_sequence_set: validation that should eventually be true + :param test_timeout_s: how long the test should run before timing out + :param tick_duration_s: length of a tick + :param index: index of the current test. default is 0 (invariant test) + values can be passed in during aggregate testing for different timeout durations + :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. + [ + (time, command, team), + (time, command, team), + ... + ] + :param run_till_end: If true, test runs till the end even if eventually validation passes + If false, test stops once eventually validation passes and fails if time out + """ + test_timeout_duration = ( + test_timeout_s[index] if type(test_timeout_s) == list else test_timeout_s + ) + + # If thunderscope is enabled, run the test in a thread and show + # thunderscope on this thread. The excepthook is setup to catch + # any test failures and propagate them to the main thread + if self.thunderscope: + run_sim_thread = threading.Thread( + target=self.runner, + daemon=True, + args=[ + always_validation_sequence_set, + eventually_validation_sequence_set, + test_timeout_duration, + tick_duration_s, + ci_cmd_with_delay, + run_till_end, + ], + ) + run_sim_thread.start() + self.thunderscope.show() + run_sim_thread.join() + + if self.last_exception: + pytest.fail(str(self.last_exception)) + + # If thunderscope is disabled, just run the test + else: + self.runner( + always_validation_sequence_set, + eventually_validation_sequence_set, + test_timeout_duration, + tick_duration_s, + ci_cmd_with_delay=ci_cmd_with_delay, + run_till_end=run_till_end, + ) + + +class InvariantTestRunner(SimulatedTestRunner): + """Runs a simulated test only once with a given parameter + + Test passes or fails based on the outcome of this test + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + @override + def run_test( + self, + setup=(lambda x: None), + params=[0], + inv_always_validation_sequence_set=[[]], + inv_eventually_validation_sequence_set=[[]], **kwargs, ): - """Begins validating a test based on incoming world protos + """Run an invariant test - :param eventually_validation_sequence_set: validation set that must eventually be true - :param always_validation_sequence_set: validation set that must always be true - :param setup: initialization function for this test - :param test_timeout_s: how long the test will run + :param setup: Function that sets up the World state and the gamecontroller before running the test + :param params: List of parameters for each iteration of the test + (this method only uses the first element) + :param inv_always_validation_sequence_set: Validation functions for invariant testing + that should hold on every tick + :param inv_eventually_validation_sequence_set: Validation functions for invariant testing + that should eventually be true, before the test ends """ - always_validation_sequence_set = always_validation_sequence_set or [[]] - eventually_validation_sequence_set = eventually_validation_sequence_set or [[]] - # Set the hook for exception handling so that we can close the thunderscope - # instance should one exist - sys.excepthook = self.excepthook - - # Only run setup if provided and if we are not being called from AggregateTestRunner - # (which handles its own setup loop) - if setup and "params" not in kwargs: - self.sync_setup(setup, self) - - self.runner( - always_validation_sequence_set=always_validation_sequence_set, - eventually_validation_sequence_set=eventually_validation_sequence_set, - test_timeout_s=test_timeout_s, - tick_duration_s=tick_duration_s, - ci_cmd_with_delay=ci_cmd_with_delay, - run_till_end=run_till_end, + threading.excepthook = self.excepthook + + super().sync_setup(setup, params[0]) + + super().run_test( + inv_always_validation_sequence_set, + inv_eventually_validation_sequence_set, **kwargs, ) class AggregateTestRunner(SimulatedTestRunner): - """A test runner for aggregate tests. - These tests are a collection of invariant tests. If any of the invariant tests fail, - the aggregate test fails. + """Runs a simulated test multiple times with different given parameters + + Result of the test is determined by comparing the number of + passing iterations to a predetermined acceptable threshold """ def __init__(self, *args, **kwargs): @@ -301,39 +372,35 @@ def __init__(self, *args, **kwargs): def run_test( self, setup=(lambda arg: None), - params=None, - ag_always_validation_sequence_set=None, - ag_eventually_validation_sequence_set=None, + params=[], + ag_always_validation_sequence_set=[[]], + ag_eventually_validation_sequence_set=[[]], **kwargs, ): - """Begins validating a test based on incoming world protos. Runs the - invariant test first, then the aggregate test. + """Run an aggregate test - :param setup: initialization function for this test + :param setup: Function that sets up the World state and the gamecontroller before running the test :param params: List of parameters for each iteration of the test - :param ag_eventually_validation_sequence_set: validation set for aggregate test that must eventually be true - :param ag_always_validation_sequence_set: validation set for aggregate test that must always be true + :param ag_always_validation_sequence_set: Validation functions for aggregate testing + that should hold on every tick + :param ag_eventually_validation_sequence_set: Validation functions for aggregate testing + that should eventually be true, before the test end """ - params = params or [] - ag_always_validation_sequence_set = ag_always_validation_sequence_set or [[]] - ag_eventually_validation_sequence_set = ( - ag_eventually_validation_sequence_set or [[]] - ) - sys.excepthook = self.excepthook + threading.excepthook = self.excepthook failed_tests = 0 - # Create a copy of kwargs without 'params' to avoid double-setup in SimulatedTestRunner - clean_kwargs = {k: v for k, v in kwargs.items() if k != "params"} - + # Runs the test once for each given parameter + # Catches Assertion Error thrown by failing test and increments counter + # Calculates overall results and prints them for x in range(len(params)): super().sync_setup(setup, params[x]) try: super().run_test( - always_validation_sequence_set=ag_always_validation_sequence_set, - eventually_validation_sequence_set=ag_eventually_validation_sequence_set, - **clean_kwargs, + ag_always_validation_sequence_set, + ag_eventually_validation_sequence_set, + **kwargs, ) except AssertionError: failed_tests += 1 @@ -427,12 +494,6 @@ def load_command_line_arguments(allow_unrecognized: bool = False): default=False, help="Use realism in the simulator", ) - parser.add_argument( - "--ci_mode", - action="store_true", - default=False, - help="Run in CI mode (faster execution)", - ) return parser.parse_known_args()[0] if allow_unrecognized else parser.parse_args() @@ -481,14 +542,14 @@ def simulated_test_runner(): args.debug_blue_full_system, False, should_restart_on_crash=False, - running_in_realtime=not args.ci_mode, + running_in_realtime=args.enable_thunderscope, ) as blue_fs, FullSystem( "software/unix_full_system", f"{args.yellow_full_system_runtime_dir}/test/{test_name}", args.debug_yellow_full_system, True, should_restart_on_crash=False, - running_in_realtime=not args.ci_mode, + running_in_realtime=args.enable_thunderscope, ) as yellow_fs: with Gamecontroller( suppress_logs=(not args.show_gamecontroller_logs) @@ -519,9 +580,7 @@ def simulated_test_runner(): layout_path=args.layout, ) - # Even in CI mode, give a small delay for processes to start up - actual_launch_delay = 0.5 if args.ci_mode else LAUNCH_DELAY_S - time.sleep(actual_launch_delay) + time.sleep(LAUNCH_DELAY_S) runner = None @@ -536,7 +595,7 @@ def simulated_test_runner(): gamecontroller, ) else: - runner = SimulatedTestRunner( + runner = InvariantTestRunner( current_test, tscope, simulator_proto_unix_io, From c13928fe19e530f0f3d8dd2e6468c1968ca96d03 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 12 Jun 2026 14:43:30 -0700 Subject: [PATCH 21/46] pid --- src/software/embedded/motion_control/BUILD | 3 - .../motion_control/orientation_controller.h | 2 +- .../motion_control/pid_controller.cpp | 27 -------- .../embedded/motion_control/pid_controller.h | 62 ++++++++++++++----- .../motion_control/pid_controller_test.cpp | 28 +++++---- .../motion_control/position_controller.h | 4 +- 6 files changed, 65 insertions(+), 61 deletions(-) delete mode 100644 src/software/embedded/motion_control/pid_controller.cpp diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD index 018c8ddb19..8f9d68cd7f 100644 --- a/src/software/embedded/motion_control/BUILD +++ b/src/software/embedded/motion_control/BUILD @@ -45,9 +45,6 @@ cc_library( cc_library( name = "pid_controller", - srcs = [ - "pid_controller.cpp", - ], hdrs = [ "pid_controller.h", ], diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 1e01b81782..d7ee19ac95 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -37,7 +37,7 @@ class OrientationController private: // TODO(#3737): tune constants - PidController w_pid_{0.4, 0.0, 0.0, 0.0}; + PidController w_pid_{0.4, 0.0, 0.0, 0.0}; static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 5; }; diff --git a/src/software/embedded/motion_control/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp deleted file mode 100644 index fe19d403dd..0000000000 --- a/src/software/embedded/motion_control/pid_controller.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#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 index 6282e41317..16c10b9393 100644 --- a/src/software/embedded/motion_control/pid_controller.h +++ b/src/software/embedded/motion_control/pid_controller.h @@ -1,21 +1,25 @@ #pragma once +#include +#include #include +/** + * A PID controller is used to calculate corrections based on error values over + * time as the difference between a desired value and the actual measured value. + * This PID controller also limits integral windup using a max_integral value. + * + * Resources: + * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ + * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ + */ +template class PidController { public: /** * Constructs a new PID controller. * - * A PID controller is used to calculate corrections based on error values over - * time as the difference between a desired value and the actual measured value. - * This PID controller also limits integral windup using a max_integral value. - * - * Resources: - * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ - * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ - * * @pre max_integral must be >= 0.0 * * @param k_p The proportional gain. @@ -24,7 +28,11 @@ class PidController * @param max_integral The maximum absolute value that the integrator term can * accumulate to. **/ - PidController(double k_p, double k_i, double k_d, double max_integral); + PidController(T k_p, T k_i, T k_d, T max_integral) + : k_p_(k_p), k_i_(k_i), k_d_(k_d), max_integral_(max_integral) + { + assert(max_integral >= T(0.0)); + } /** * Given an error, returns the control effort to minimize it. @@ -33,19 +41,39 @@ class PidController * @param delta_time The time passed since last step, for calculating the integrator * and derivative. **/ - double step(double error, double delta_time); + T step(T error, T delta_time = T(1.0)) + { + // If sign of error swaps, reset integrator + if (last_error_.has_value() && (last_error_.value() * error < T(0.0))) + { + integral_ = T(0.0); + } + + integral_ = + std::clamp(integral_ + error * delta_time, -max_integral_, max_integral_); + + const T derivative = (error - last_error_.value_or(error)) / delta_time; + + last_error_ = error; + + return error * k_p_ + integral_ * k_i_ + derivative * k_d_; + } /** * Resets the integrator and clears the last error used for derivative calculation. **/ - void reset(); + void reset() + { + integral_ = T(0.0); + last_error_ = std::nullopt; + } private: - double k_p_; - double k_i_; - double k_d_; - double max_integral_; + T k_p_; + T k_i_; + T k_d_; + T max_integral_; - double integral_ = 0.0; - std::optional last_error_ = std::nullopt; + T integral_ = T(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 index c7c0db12b3..90248c5459 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -4,7 +4,7 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) { - PidController pid{1.0, 0.0, 0.0, 0.0}; + 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); @@ -13,7 +13,7 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) 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}; + 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); @@ -26,27 +26,32 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) TEST(PidControllerTest, OnlyIntegralTermNonZero) { constexpr double k_i = 2.0; - PidController pid{0.0, k_i, 0.0, 10.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); + // Sign swap should reset integrator to 0, then add -0.2 + 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); + // Sign swap back to 0.0? No, 0.0 doesn't swap sign usually (0*X is 0, not < 0) + // My implementation: last_error_.value() * error < T(0.0) + // If error is 0, it's not < 0. + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * -1.2); // 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(6.7, 1.0), k_i * 5.5); // Sign not swapped from 0.0 to 6.7 + EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), + k_i * 10.0); // Clamped (5.5 + 5.0 = 10.5 -> 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}; + 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); @@ -64,12 +69,13 @@ TEST(PidControllerTest, GeneralApplication) 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}; + 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); + // Sign swap reset: integral becomes 0 + -2.0*1.0 = -2.0 + EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0); } diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index 08440c3829..01e8c332c3 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -35,8 +35,8 @@ class PositionController : public MotionController x_pid_{0.2, 0.0, 0.0, 0.0}; + PidController y_pid_{0.2, 0.0, 0.0, 0.0}; static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; }; From 57f787cd6c79f4ded65d7fc5dc17e94bd786b82e Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 12 Jun 2026 16:10:30 -0700 Subject: [PATCH 22/46] localizer --- src/shared/robot_constants.h | 21 +- src/software/embedded/BUILD | 1 + src/software/embedded/thunderloop.cpp | 347 +++++++++++++++----------- src/software/embedded/thunderloop.h | 43 ++++ 4 files changed, 271 insertions(+), 141 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 673b0469c4..be674b45b5 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -116,6 +116,13 @@ struct RobotConstants // Found by sqrt(x^2 + y^2) of a wheel. // Front wheel arm = Rear wheel arm. See ASCII image above. float expected_lever_arm; + + // Various variances for the robot localizer Kalman filter + float kalman_process_noise_variance_rad_per_s_4; + + float kalman_vision_noise_variance_rad_2; + + float kalman_motor_sensor_noise_variance_rad_per_s_2; }; /** @@ -163,7 +170,12 @@ constexpr RobotConstants createRobotConstants() .wheel_radius_meters = 0.03f, - .expected_lever_arm = 0.0749f}; + .expected_lever_arm = 0.0749f, + + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 1.0f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f}; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() @@ -194,7 +206,12 @@ constexpr RobotConstants createRobotConstants() .robot_trajectory_max_ang_speed_rad_per_s = 7.0f, .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, - .wheel_radius_meters = 0.03f}; + .wheel_radius_meters = 0.03f, + + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 0.5f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f * 0.5f}; } #endif diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 7f5e9f1078..9f3e81f15c 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -60,6 +60,7 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded:robot_localizer", "//software/embedded/services:imu", "//software/embedded/services:motor", "//software/embedded/services:power", diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index e76d5eae13..3356a21b56 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -16,6 +16,7 @@ #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" +#include "software/physics/velocity_conversion_util.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" #include "software/world/robot_state.h" @@ -80,7 +81,11 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, std::stoi(toml_config_client_->get(ROBOT_MULTICAST_CHANNEL_CONFIG_KEY))), network_interface_(toml_config_client_->get(ROBOT_NETWORK_INTERFACE_CONFIG_KEY)), loop_hz_(loop_hz), - primitive_executor_(robot_constants) + primitive_executor_(robot_constants), + robot_localizer_(RobotLocalizer::RobotLocalizerConfig{ + robot_constants.kalman_process_noise_variance_rad_per_s_4, + robot_constants.kalman_vision_noise_variance_rad_2, + robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2}) { waitForNetworkUp(); @@ -139,6 +144,11 @@ Thunderloop::~Thunderloop() {} /* * Run the main robot loop! + * + * Each iteration reads from the sensors and network, fuses them into a robot state + * estimate, steps the active primitive, and drives the actuators. The body is kept + * as a short sequence of named stages so the high-level control flow stays readable; + * each stage is implemented in its own helper below. */ void Thunderloop::runLoop() { @@ -152,9 +162,6 @@ void Thunderloop::runLoop() struct timespec last_kicker_fired; struct timespec prev_iter_start_time; - // Input buffer - TbotsProto::Primitive new_primitive; - // Loop interval int interval = static_cast(1.0f / static_cast(loop_hz_) * NANOSECONDS_PER_SECOND); @@ -168,14 +175,12 @@ void Thunderloop::runLoop() clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); clock_gettime(CLOCK_MONOTONIC, &prev_iter_start_time); + // Initial version setup std::string thunderloop_hash, thunderloop_date_flashed; - std::ifstream hashFile("~/thunderbots_hashes/thunderloop.hash"); std::ifstream dateFile("~/thunderbots_hashes/thunderloop.date"); - std::getline(hashFile, thunderloop_hash); std::getline(dateFile, thunderloop_date_flashed); - hashFile.close(); dateFile.close(); @@ -184,14 +189,6 @@ void Thunderloop::runLoop() *(robot_status_.mutable_motor_status()) = TbotsProto::MotorStatus(); *(robot_status_.mutable_power_status()) = TbotsProto::PowerStatus(); - std::optional imu_poll = imu_service_->poll(); - - // TODO (3725): Replace with actual IMU data usage - if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) - { - LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); - } - for (;;) { struct timespec time_since_prev_iter; @@ -210,86 +207,23 @@ void Thunderloop::runLoop() ScopedTimespecTimer iteration_timer(&iteration_time); + const Duration delta_time = Duration::fromSeconds( + getMilliseconds(time_since_prev_iter) * SECONDS_PER_MILLISECOND); + // Network Service: receive newest primitives and send out the last // robot status - { - ScopedTimespecTimer timer(&poll_time); - - ZoneNamedN(_tracy_network_poll, "Thunderloop: Poll NetworkService", true); + processNetworkPolling(poll_time, last_primitive_received_time); - new_primitive = network_service_->poll(robot_status_); - } + // Robot Localizer: fuse sensor measurements into a robot state estimate + // and hand it to the primitive executor + processLocalizationUpdates(); - thunderloop_status_.set_network_service_poll_time_ms( - getMilliseconds(poll_time)); - - uint64_t last_handled_primitive_set = primitive_.sequence_number(); - - // Updating primitives with newly received data - // and setting the correct time elasped since last primitive - - struct timespec time_since_last_primitive_received; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, - &last_primitive_received_time, - &time_since_last_primitive_received); - network_status_.set_ms_since_last_primitive_received( - getMilliseconds(time_since_last_primitive_received)); - - // If the primitive msg is new, update the internal buffer - // and start the new primitive. - if (new_primitive.time_sent().epoch_timestamp_seconds() > - primitive_.time_sent().epoch_timestamp_seconds()) - { - // Save new primitive - primitive_ = new_primitive; - - // Update primitive executor's primitive set - { - clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); - - // Start new primitive - { - ScopedTimespecTimer timer(&poll_time); - primitive_executor_.updatePrimitive(primitive_); - } - - thunderloop_status_.set_primitive_executor_start_time_ms( - getMilliseconds(poll_time)); - } - } - - // TODO (#3725): use robot localizer to pass in position and orientation - primitive_executor_.updateState(RobotState( - Point(), createVector(robot_status_.motor_status().local_velocity()), - Angle(), - createAngularVelocity(robot_status_.motor_status().angular_velocity()))); - - // Timeout Overrides for Primitives - // These should be after the new primitive update section above - - // If primitive not received in a while, stop robot // Primitive Executor: run the last primitive if we have not timed out - { - ScopedTimespecTimer timer(&poll_time); - - ZoneNamedN(_tracy_step_primitive, "Thunderloop: Step Primitive", true); + processPrimitiveExecution(poll_time, last_primitive_received_time, + delta_time); - // Handle emergency stop override - auto nanoseconds_elapsed_since_last_primitive = - getNanoseconds(time_since_last_primitive_received); - - if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) - { - primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); - } - - direct_control_ = *primitive_executor_.stepPrimitive( - primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); - } - - thunderloop_status_.set_primitive_executor_step_time_ms( - getMilliseconds(poll_time)); + // Chicker: track time since the last kick/chip event + updateChickerStatus(last_chipper_fired, last_kicker_fired); // Motor Service: execute the motor control command pollMotorService(poll_time, time_since_prev_iter); @@ -301,56 +235,8 @@ void Thunderloop::runLoop() thunderloop_status_.set_power_service_poll_time_ms( getMilliseconds(poll_time)); - struct timespec time_since_kicker_fired; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, &last_kicker_fired, - &time_since_kicker_fired); - chipper_kicker_status_.set_ms_since_kicker_fired( - getMilliseconds(time_since_kicker_fired)); - - struct timespec time_since_chipper_fired; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, &last_chipper_fired, - &time_since_chipper_fired); - chipper_kicker_status_.set_ms_since_chipper_fired( - getMilliseconds(time_since_chipper_fired)); - - // if a kick proto is sent or if autokick is on - if (direct_control_.power_control().chicker().has_kick_speed_m_per_s() || - direct_control_.power_control() - .chicker() - .auto_chip_or_kick() - .has_autokick_speed_m_per_s()) - { - clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); - } - // if a chip proto is sent or if autochip is on - else if (direct_control_.power_control() - .chicker() - .has_chip_distance_meters() || - direct_control_.power_control() - .chicker() - .auto_chip_or_kick() - .has_autochip_distance_meters()) - { - clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); - } - - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - time_sent_.set_epoch_timestamp_seconds( - static_cast(current_time.tv_sec)); - - // Update Robot Status with poll responses - robot_status_.set_robot_id(robot_id_); - robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); - *(robot_status_.mutable_time_sent()) = time_sent_; - *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; - *(robot_status_.mutable_network_status()) = network_status_; - *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; - *(robot_status_.mutable_primitive_executor_status()) = - primitive_executor_status_; - - updateErrorCodes(); + // Robot Status: aggregate poll responses and broadcast + updateAndSendRobotStatus(primitive_.sequence_number()); } auto loop_duration_ns = getNanoseconds(iteration_time); @@ -365,6 +251,189 @@ void Thunderloop::runLoop() } } +inline void Thunderloop::processNetworkPolling( + struct timespec& poll_time, struct timespec& last_primitive_received_time) +{ + struct timespec current_time; + TbotsProto::Primitive new_primitive; + + // Network Service: receive newest primitives and send out the last robot status + { + ScopedTimespecTimer timer(&poll_time); + + ZoneNamedN(_tracy_network_poll, "Thunderloop: Poll NetworkService", true); + + new_primitive = network_service_->poll(robot_status_); + } + + thunderloop_status_.set_network_service_poll_time_ms(getMilliseconds(poll_time)); + + // Update the time elapsed since the last received primitive + struct timespec time_since_last_primitive_received; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + ScopedTimespecTimer::timespecDiff(¤t_time, &last_primitive_received_time, + &time_since_last_primitive_received); + network_status_.set_ms_since_last_primitive_received( + getMilliseconds(time_since_last_primitive_received)); + + // If the primitive msg is new, update the internal buffer and start the new + // primitive. + if (new_primitive.time_sent().epoch_timestamp_seconds() > + primitive_.time_sent().epoch_timestamp_seconds()) + { + // Save new primitive + primitive_ = new_primitive; + + // Feed the trajectory's starting pose to the localizer as a vision update. + // The start angle is offset by half a turn to match the robot's convention. + if (primitive_.has_move()) + { + const Point position = + createPoint(primitive_.move().xy_traj_params().start_position()); + const Angle orientation = + createAngle(primitive_.move().w_traj_params().start_angle()) + + Angle::half(); + + robot_localizer_.update( + RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); + } + + clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); + + // Start new primitive + { + ScopedTimespecTimer timer(&poll_time); + primitive_executor_.updatePrimitive(primitive_); + } + + thunderloop_status_.set_primitive_executor_start_time_ms( + getMilliseconds(poll_time)); + } +} + +inline void Thunderloop::processLocalizationUpdates() +{ + const std::optional imu_poll = imu_service_->poll(); + + // IMU: feed the measured angular velocity to the localizer + if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) + { + robot_localizer_.update( + RobotLocalizer::ImuData{imu_poll->angular_velocity.value()}); + } + + // Motors: feed the measured wheel velocities (rotated into the global frame) to + // the localizer + if (robot_status_.has_motor_status()) + { + const auto status = robot_status_.motor_status(); + + robot_localizer_.update(RobotLocalizer::MotorData{ + localToGlobalVelocity(createVector(status.local_velocity()), + robot_localizer_.getOrientation()), + createAngularVelocity(status.angular_velocity())}); + } + + // Step the localizer forward using the measured linear acceleration + Vector linear_acceleration; + if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) + { + const auto accel = imu_poll->linear_acceleration.value(); + linear_acceleration = Vector(accel[0], accel[1]); + } + + robot_localizer_.step(linear_acceleration); + + // Hand the fused state estimate to the primitive executor + primitive_executor_.updateState(RobotState( + robot_localizer_.getPosition(), robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity())); +} + +inline void Thunderloop::processPrimitiveExecution( + struct timespec& poll_time, struct timespec& last_primitive_received_time, + const Duration& delta_time) +{ + struct timespec current_time; + struct timespec time_since_last_primitive_received; + + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + ScopedTimespecTimer::timespecDiff(¤t_time, &last_primitive_received_time, + &time_since_last_primitive_received); + + { + ScopedTimespecTimer timer(&poll_time); + + ZoneNamedN(_tracy_step_primitive, "Thunderloop: Step Primitive", true); + + // If primitive not received in a while, stop the robot + auto nanoseconds_elapsed_since_last_primitive = + getNanoseconds(time_since_last_primitive_received); + + if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) + { + primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); + } + + direct_control_ = + *primitive_executor_.stepPrimitive(primitive_executor_status_, delta_time); + } + + thunderloop_status_.set_primitive_executor_step_time_ms(getMilliseconds(poll_time)); +} + +inline void Thunderloop::updateChickerStatus(struct timespec& last_chipper_fired, + struct timespec& last_kicker_fired) +{ + struct timespec current_time; + struct timespec time_diff; + + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + ScopedTimespecTimer::timespecDiff(¤t_time, &last_kicker_fired, &time_diff); + chipper_kicker_status_.set_ms_since_kicker_fired(getMilliseconds(time_diff)); + + ScopedTimespecTimer::timespecDiff(¤t_time, &last_chipper_fired, &time_diff); + chipper_kicker_status_.set_ms_since_chipper_fired(getMilliseconds(time_diff)); + + // if a kick proto is sent or if autokick is on + if (direct_control_.power_control().chicker().has_kick_speed_m_per_s() || + direct_control_.power_control() + .chicker() + .auto_chip_or_kick() + .has_autokick_speed_m_per_s()) + { + clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); + } + // if a chip proto is sent or if autochip is on + else if (direct_control_.power_control().chicker().has_chip_distance_meters() || + direct_control_.power_control() + .chicker() + .auto_chip_or_kick() + .has_autochip_distance_meters()) + { + clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); + } +} + +inline void Thunderloop::updateAndSendRobotStatus(uint64_t last_handled_primitive_set) +{ + struct timespec current_time; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + time_sent_.set_epoch_timestamp_seconds(static_cast(current_time.tv_sec)); + + // Update Robot Status with poll responses + robot_status_.set_robot_id(robot_id_); + robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); + *(robot_status_.mutable_time_sent()) = time_sent_; + *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; + *(robot_status_.mutable_network_status()) = network_status_; + *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; + *(robot_status_.mutable_primitive_executor_status()) = primitive_executor_status_; + + updateErrorCodes(); +} + double Thunderloop::getMilliseconds(timespec time) { return (static_cast(time.tv_sec) * MILLISECONDS_PER_SECOND) + diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 50ce9e2978..842b4b2ed7 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -10,6 +10,7 @@ #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/robot_localizer.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" @@ -121,6 +122,45 @@ class Thunderloop */ void waitForNetworkUp(); + /** + * Polls the network service for new primitives and updates timing status. + * + * @param poll_time Output for polling duration + * @param last_primitive_received_time Input/Output for tracking last received packet + */ + inline void processNetworkPolling(struct timespec& poll_time, + struct timespec& last_primitive_received_time); + + /** + * Processes robot localization updates from sensors (IMU, Motors) and feeds the + * resulting state estimate to the primitive executor. + */ + inline void processLocalizationUpdates(); + + /** + * Steps the primitive executor and handles timeouts. + * + * @param poll_time Output for execution duration + * @param last_primitive_received_time Input for checking timeouts + * @param delta_time The time passed since the last step + */ + inline void processPrimitiveExecution(struct timespec& poll_time, + struct timespec& last_primitive_received_time, + const Duration& delta_time); + + /** + * Updates internal tracking for chipper/kicker firing events. + */ + inline void updateChickerStatus(struct timespec& last_chipper_fired, + struct timespec& last_kicker_fired); + + /** + * Aggregates and sends the final robot status message. + * + * @param last_handled_primitive_set The sequence number of the last handled primitive + */ + inline void updateAndSendRobotStatus(uint64_t last_handled_primitive_set); + // Input Msg Buffers TbotsProto::World world_; @@ -146,6 +186,9 @@ class Thunderloop // Primitive Executor PrimitiveExecutor primitive_executor_; + // Robot localization model + RobotLocalizer robot_localizer_; + // 500 millisecond timeout on receiving primitives before we stop the robots const double PACKET_TIMEOUT_NS = 500.0 * NANOSECONDS_PER_MILLISECOND; From 9eb4d05dcd6f663679a38910d0cc974dab8961c3 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 13 Jun 2026 12:55:21 -0700 Subject: [PATCH 23/46] test fixes --- src/proto/message_translation/tbots_protobuf_test.cpp | 4 ++-- .../embedded/motor_controller/stspin_motor_controller.h | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 2735d57a3b..0f82de69d6 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -209,8 +209,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) *(params.add_sub_destinations()) = sub_destination_proto; } - auto converted_trajectory_path_opt = createTrajectoryPathFromParams( - params, createPoint(params.start_position()), initial_velocity, robot_constants); + auto converted_trajectory_path_opt = + createTrajectoryPathFromParams(params, initial_velocity, robot_constants); ASSERT_TRUE(converted_trajectory_path_opt.has_value()); TrajectoryPath converted_trajectory_path = converted_trajectory_path_opt.value(); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 88a6c778ae..13b2d3be7d 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "software/embedded/gpio/gpio.h" #include "software/embedded/motor_controller/motor_controller.h" #include "software/embedded/motor_controller/motor_fault_indicator.h" @@ -58,8 +60,8 @@ class StSpinMotorController : public MotorController static constexpr int RESET_GPIO_PIN = 12; - static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 700; - static constexpr int SPEED_PID_INTEGRAL_GAIN = 30; + static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 2000; + static constexpr int SPEED_PID_INTEGRAL_GAIN = 200; static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300; From 9c8e12fba931e0e30fa7a287707ec57af00da769 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 13 Jun 2026 18:17:10 -0700 Subject: [PATCH 24/46] bugs fixed --- src/software/embedded/primitive_executor.cpp | 30 ++++++++++++++++++-- src/software/embedded/primitive_executor.h | 16 +++++++++++ 2 files changed, 43 insertions(+), 3 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 4b2fe348e4..b7a9158cc9 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -60,7 +60,7 @@ Vector PrimitiveExecutor::stepTargetLinearVelocity(Duration delta_time) static_cast(robot_constants_.robot_max_speed_m_per_s))); const Vector local_acceleration = - (target_v_local - state_.localVelocity()) / delta_time.toSeconds(); + (target_v_local - prev_target_local_velocity_) / delta_time.toSeconds(); if (local_acceleration.length() > robot_constants_.robot_max_acceleration_m_per_s_2) { @@ -68,6 +68,7 @@ Vector PrimitiveExecutor::stepTargetLinearVelocity(Duration delta_time) << "m/s^2."; } + prev_target_local_velocity_ = target_v_local; return target_v_local; } @@ -83,13 +84,14 @@ AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(Duration delta_time target_w = AngularVelocity::fromRadians(clamped_w); const auto angular_acceleration = - (target_w - state_.angularVelocity()) / delta_time.toSeconds(); - if (angular_acceleration.toRadians() > + (target_w - prev_target_angular_velocity_) / delta_time.toSeconds(); + if (std::abs(angular_acceleration.toRadians()) > robot_constants_.robot_max_ang_acceleration_rad_per_s_2) { LOG(WARNING) << "Robot trying to angular accelerate at " << angular_acceleration.toRadians() << "rads/s^2."; } + prev_target_angular_velocity_ = target_w; return target_w; } @@ -110,10 +112,23 @@ std::unique_ptr PrimitiveExecutor::stepPrimi auto output = std::make_unique( prim->direct_control()); status.set_running_primitive(false); + setPrevCommandedVelocity(Vector(), AngularVelocity()); return output; } case TbotsProto::Primitive::kDirectControl: { + const auto& motor_control = current_primitive_.direct_control().motor_control(); + if (motor_control.has_direct_velocity_control()) + { + setPrevCommandedVelocity( + createVector(motor_control.direct_velocity_control().velocity()), + createAngularVelocity( + motor_control.direct_velocity_control().angular_velocity())); + } + else + { + setPrevCommandedVelocity(Vector(), AngularVelocity()); + } return std::make_unique( current_primitive_.direct_control()); } @@ -127,6 +142,7 @@ std::unique_ptr PrimitiveExecutor::stepPrimi prim->direct_control()); LOG(INFO) << "Not moving because trajectory_path_ or angular_trajectory_ is not set"; + setPrevCommandedVelocity(Vector(), AngularVelocity()); return output; } @@ -154,9 +170,17 @@ std::unique_ptr PrimitiveExecutor::stepPrimi // LOG(DEBUG) << "No primitive set!"; } } + setPrevCommandedVelocity(Vector(), AngularVelocity()); return std::make_unique(); } +void PrimitiveExecutor::setPrevCommandedVelocity(const Vector& local_velocity, + const AngularVelocity& angular_velocity) +{ + prev_target_local_velocity_ = local_velocity; + prev_target_angular_velocity_ = angular_velocity; +} + void PrimitiveExecutor::sendLinearMotionToPlotJuggler(const Vector& target_local_velocity, Duration delta_time) { diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 0173abf2f6..b3f39efe78 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -74,6 +74,17 @@ class PrimitiveExecutor void sendLinearMotionToPlotJuggler(const Vector& target_local_velocity, Duration delta_time); + /** + * Records the velocities commanded this step so the next step can measure the + * commanded (tick-to-tick) acceleration. Call on every code path that commands a + * velocity without going through stepTargetLinearVelocity/stepTargetAngularVelocity. + * + * @param local_velocity The local velocity commanded this step + * @param angular_velocity The angular velocity commanded this step + */ + void setPrevCommandedVelocity(const Vector& local_velocity, + const AngularVelocity& angular_velocity); + RobotState state_; TbotsProto::Primitive current_primitive_; robot_constants::RobotConstants robot_constants_; @@ -87,6 +98,11 @@ class PrimitiveExecutor PositionController position_controller_; OrientationController orientation_controller_; + // The velocities commanded on the previous step. Used to measure the commanded + // (tick-to-tick) acceleration + Vector prev_target_local_velocity_; + AngularVelocity prev_target_angular_velocity_; + // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; From bc13c3c88d32739e55efa802826720003cbe94e8 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 13 Jun 2026 18:19:21 -0700 Subject: [PATCH 25/46] linting --- src/software/embedded/primitive_executor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index b7a9158cc9..4715d236c5 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -117,7 +117,8 @@ std::unique_ptr PrimitiveExecutor::stepPrimi } case TbotsProto::Primitive::kDirectControl: { - const auto& motor_control = current_primitive_.direct_control().motor_control(); + const auto& motor_control = + current_primitive_.direct_control().motor_control(); if (motor_control.has_direct_velocity_control()) { setPrevCommandedVelocity( From 66b994ade01c9ac3fcaed76daf67c7ac50ddfe1c Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 13 Jun 2026 19:42:47 -0700 Subject: [PATCH 26/46] flags added --- src/cli/cli_params.py | 14 ++++++++++ src/software/embedded/BUILD | 39 ++++++++++++++++++++++++++- src/software/embedded/thunderloop.cpp | 13 +++++++++ src/tbots.py | 22 +++++++++++++++ 4 files changed, 87 insertions(+), 1 deletion(-) diff --git a/src/cli/cli_params.py b/src/cli/cli_params.py index 831585bdff..3b7da886ed 100644 --- a/src/cli/cli_params.py +++ b/src/cli/cli_params.py @@ -192,6 +192,20 @@ class DebugBinary(str, Enum): ), ] +# Deploy robot software "Options:" checkbox. Each option compiles Thunderloop +# with a preprocessor flag that disables the corresponding service, letting +# Thunderloop run on a robot whose powerboard or motorboard is unavailable. +DEPLOY_ROBOT_SOFTWARE_OPTION_CHOICES = [ + questionary.Choice( + title="DISABLE_POWER_SERVICE", + description="Compile Thunderloop without the Power Service (no powerboard)", + ), + questionary.Choice( + title="DISABLE_MOTOR_SERVICE", + description="Compile Thunderloop without the Motor Service (no motorboard)", + ), +] + # Marker value returned by the DEBUG_POWERLOOP playbook choice. It maps to the # deploy_powerboard.yml playbook but additionally compiles powerloop_main with # the DEBUG_POWERLOOP flag, swapping in bare setup()/loop() stubs so arbitrary diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 9f3e81f15c..4e275f39c7 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -1,7 +1,37 @@ -load("@bazel_skylib//rules:common_settings.bzl", "string_flag") +load("@bazel_skylib//rules:common_settings.bzl", "bool_flag", "string_flag") package(default_visibility = ["//visibility:public"]) +# When enabled, compiles Thunderloop with the DISABLE_POWER_SERVICE flag, which +# skips initializing and polling the PowerService so Thunderloop can be run on a +# robot without a functioning powerboard. +bool_flag( + name = "disable_power_service", + build_setting_default = False, +) + +config_setting( + name = "disable_power_service_enabled", + flag_values = { + ":disable_power_service": "True", + }, +) + +# When enabled, compiles Thunderloop with the DISABLE_MOTOR_SERVICE flag, which +# skips initializing and polling the MotorService so Thunderloop can be run on a +# robot without a functioning motorboard. +bool_flag( + name = "disable_motor_service", + build_setting_default = False, +) + +config_setting( + name = "disable_motor_service_enabled", + flag_values = { + ":disable_motor_service": "True", + }, +) + config_setting( name = "build_trinamic", flag_values = { @@ -57,6 +87,13 @@ cc_library( name = "thunderloop", srcs = ["thunderloop.cpp"], hdrs = ["thunderloop.h"], + local_defines = select({ + ":disable_power_service_enabled": ["DISABLE_POWER_SERVICE"], + "//conditions:default": [], + }) + select({ + ":disable_motor_service_enabled": ["DISABLE_MOTOR_SERVICE"], + "//conditions:default": [], + }), deps = [ ":primitive_executor", "//proto:tbots_cc_proto", diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 3356a21b56..7f249ed13e 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -117,18 +117,27 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; +#ifndef DISABLE_POWER_SERVICE power_service_ = std::make_unique( std::stod(toml_config_client_->get(ROBOT_KICK_EXP_COEFF_CONFIG_KEY)), std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY)), std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))); LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; +#else + LOG(INFO) + << "THUNDERLOOP: Power Service DISABLED! Next initializing Motor Service"; +#endif +#ifndef DISABLE_MOTOR_SERVICE motor_service_ = std::make_unique(robot_constants); g_motor_service = motor_service_.get(); motor_service_->setup(); LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service"; +#else + LOG(INFO) << "THUNDERLOOP: Motor Service DISABLED! Next initializing IMU Service"; +#endif imu_service_ = std::make_unique(); LOG(INFO) << "THUNDERLOOP: IMU Service initialized!"; @@ -225,15 +234,19 @@ void Thunderloop::runLoop() // Chicker: track time since the last kick/chip event updateChickerStatus(last_chipper_fired, last_kicker_fired); +#ifndef DISABLE_MOTOR_SERVICE // Motor Service: execute the motor control command pollMotorService(poll_time, time_since_prev_iter); thunderloop_status_.set_motor_service_poll_time_ms( getMilliseconds(poll_time)); +#endif +#ifndef DISABLE_POWER_SERVICE // Power Service: execute the power control command pollPowerService(poll_time); thunderloop_status_.set_power_service_poll_time_ms( getMilliseconds(poll_time)); +#endif // Robot Status: aggregate poll responses and broadcast updateAndSendRobotStatus(primitive_.sequence_number()); diff --git a/src/tbots.py b/src/tbots.py index 6ffe8cb151..37554d5031 100755 --- a/src/tbots.py +++ b/src/tbots.py @@ -15,6 +15,7 @@ from cli.cli_params import ( CATEGORY_CHOICES, DEBUG_POWERLOOP_PLAYBOOK, + DEPLOY_ROBOT_SOFTWARE_OPTION_CHOICES, INTERACTIVE_STYLE, LAUNCH_MODE_CHOICES, PLAYBOOK_CHOICES, @@ -61,6 +62,8 @@ class BuildConfig: robot_name: str | None = None ansible_playbook: str | None = None debug_powerloop: bool = False + disable_power_service: bool = False + disable_motor_service: bool = False class BazelFlag(tuple, Enum): @@ -71,6 +74,8 @@ class BazelFlag(tuple, Enum): THUNDERSCOPE = ("--spawn_strategy=local", "--test_env=DISPLAY=:0") NO_CACHE_TESTS = ("--cache_test_results=false",) DEBUG_POWERLOOP = ("--//software/power:debug_powerloop",) + DISABLE_POWER_SERVICE = ("--//software/embedded:disable_power_service",) + DISABLE_MOTOR_SERVICE = ("--//software/embedded:disable_motor_service",) app = Typer() @@ -209,6 +214,8 @@ def create_command(config: BuildConfig, extra_args: list[str]) -> list[str]: BazelFlag.THUNDERSCOPE: config.enable_thunderscope, BazelFlag.NO_CACHE_TESTS: config.action == ActionArgument.test, BazelFlag.DEBUG_POWERLOOP: config.debug_powerloop, + BazelFlag.DISABLE_POWER_SERVICE: config.disable_power_service, + BazelFlag.DISABLE_MOTOR_SERVICE: config.disable_motor_service, } for flag, condition in flag_conditions.items(): if condition: @@ -376,6 +383,21 @@ def start_interactive_cli(): config.debug_powerloop = True else: config.ansible_playbook = playbook_choice + # The deploy_robot_software playbook can compile Thunderloop with + # services disabled so it can run on a robot missing a powerboard or + # motorboard. Each selected option maps to a Bazel flag that defines + # the matching preprocessor macro in thunderloop.cpp. + if config.ansible_playbook == "deploy_robot_software.yml": + selected = ( + questionary.checkbox( + "Options:", + choices=DEPLOY_ROBOT_SOFTWARE_OPTION_CHOICES, + style=INTERACTIVE_STYLE, + ).ask() + or [] + ) + config.disable_power_service = "DISABLE_POWER_SERVICE" in selected + config.disable_motor_service = "DISABLE_MOTOR_SERVICE" in selected config.robot_name = questionary.text( "Robot name?", style=INTERACTIVE_STYLE ).ask() From 88afd0bb7e50f926a236ca3d3b882c14b931329b Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 13 Jun 2026 19:43:30 -0700 Subject: [PATCH 27/46] linting --- src/software/embedded/thunderloop.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 7f249ed13e..50018a9d1f 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -125,8 +125,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; #else - LOG(INFO) - << "THUNDERLOOP: Power Service DISABLED! Next initializing Motor Service"; + LOG(INFO) << "THUNDERLOOP: Power Service DISABLED! Next initializing Motor Service"; #endif #ifndef DISABLE_MOTOR_SERVICE From 3821ee29a255f7bf4936fd6d6b9a35dd83a3c145 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sat, 13 Jun 2026 19:49:12 -0700 Subject: [PATCH 28/46] renamed motors --- .../ansible/playbooks/deploy_motor_firmware.yml | 4 ++-- .../embedded/drivers/flash_motor_drivers.py | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml b/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml index b674569c6e..2f2ba7b95f 100644 --- a/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml +++ b/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml @@ -108,10 +108,10 @@ failed_when: false changed_when: false - - name: Run flashing script for Motor Drivers (A B C D) + - name: Run flashing script for Motor Drivers (FL FR BL BR) ansible.builtin.shell: > set -o pipefail; - echo {{ ansible_sudo_pass }} | sudo -S python3 flash_motor_drivers.py A B C D + echo {{ ansible_sudo_pass }} | sudo -S python3 flash_motor_drivers.py FL FR BL BR args: chdir: "{{ remote_dir }}" executable: /bin/bash diff --git a/src/software/embedded/drivers/flash_motor_drivers.py b/src/software/embedded/drivers/flash_motor_drivers.py index d2e50b703f..e1e1b15ea1 100755 --- a/src/software/embedded/drivers/flash_motor_drivers.py +++ b/src/software/embedded/drivers/flash_motor_drivers.py @@ -12,13 +12,13 @@ DEMUX_DISABLE_PIN = 0 # Pull HIGH to disable SWD and SWCLK multiplexing # Put these in order of board 0, 1, 2, 3 -BOARD_NAMES = ["A", "B", "C", "D"] +BOARD_NAMES = ["FL", "FR", "BL", "BR"] class MotorDriverFlasher: - def __init__(self, board_letter, drivers): - switch_case_num = BOARD_NAMES.index(board_letter) - self.board_letter = board_letter + def __init__(self, board_name, drivers): + switch_case_num = BOARD_NAMES.index(board_name) + self.board_name = board_name self.multiplex = [switch_case_num % 2, switch_case_num // 2] self.drivers = drivers @@ -30,7 +30,7 @@ def flash(self): else: self.drivers[i].off() - print(f"Preparing to flash driver on board {self.board_letter}...") + print(f"Preparing to flash driver on board {self.board_name}...") # Short delay to ensure lines settle time.sleep(0.5) @@ -72,7 +72,7 @@ def flash(self): raise e # After flashing all, ensure all are set to High (Run) - print(f"Flash to board {self.board_letter} complete.") + print(f"Flash to board {self.board_name} complete.") # Let line settle rq time.sleep(0.5) @@ -81,7 +81,7 @@ def flash(self): # If no arguments given, if not (2 <= len(sys.argv) <= 5): print( - "Usage: python3 flash_motor_drivers.py (A to D valid)" + "Usage: python3 flash_motor_drivers.py (FL, FR, BL, BR valid)" ) sys.exit(1) @@ -89,7 +89,7 @@ def flash(self): for i in range(1, len(sys.argv)): if sys.argv[i] not in BOARD_NAMES: print( - f"Usage: python3 flash_motor_drivers.py (Valid Letters: {BOARD_NAMES})" + f"Usage: python3 flash_motor_drivers.py (Valid Names: {BOARD_NAMES})" ) sys.exit(1) From ea757d1cbd94a2b89339b8c20e4412f5a2024071 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sun, 14 Jun 2026 22:53:25 -0700 Subject: [PATCH 29/46] prim --- src/software/embedded/primitive_executor.cpp | 115 +++++++++++++++++-- src/software/embedded/primitive_executor.h | 37 +++++- 2 files changed, 141 insertions(+), 11 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 4715d236c5..7f71fa93b5 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -1,5 +1,7 @@ #include "software/embedded/primitive_executor.h" +#include + #include "proto/message_translation/tbots_geometry.h" #include "proto/message_translation/tbots_protobuf.h" #include "proto/primitive.pb.h" @@ -16,6 +18,91 @@ PrimitiveExecutor::PrimitiveExecutor( { } +bool PrimitiveExecutor::isLinearTrajectoryNew( + const std::optional& new_trajectory) const +{ + if (new_trajectory.has_value() != trajectory_path_.has_value()) + { + // We either don't have a trajectory and the new one does, or we have one and + // the new one doesn't. + return true; + } + + if (!new_trajectory.has_value()) + { + return false; + } + + // Regenerate if the destination moved meaningfully. This also catches changes + // beyond the comparison horizon below (e.g. a far-away destination shifting). + if (!trajectory_path_->equals(*new_trajectory, LINEAR_DESTINATION_THRESHOLD_METERS)) + { + return true; + } + + // The destinations match, but the path to get there may have changed (e.g. a + // newly-detected obstacle forces a detour). Compare the two plans over the same + // upcoming time window, anchored at where we currently are on the old trajectory, + // so timing/speed differences are accounted for (a purely spatial comparison would + // not). Regenerate if the plans ever diverge by more than the deviation threshold. + const double old_time = time_since_linear_trajectory_creation_.toSeconds(); + const double new_time = VISION_TO_ROBOT_DELAY_S; + const double horizon = + std::min(std::max(trajectory_path_->getTotalTime() - old_time, + new_trajectory->getTotalTime() - new_time), + TRAJECTORY_DEVIATION_HORIZON_S); + + for (double tau = 0.0; tau <= horizon; tau += TRAJECTORY_DEVIATION_TIME_STEP_S) + { + const Point old_pos = trajectory_path_->getPosition(old_time + tau); + const Point new_pos = new_trajectory->getPosition(new_time + tau); + if (distance(old_pos, new_pos) > LINEAR_TRAJECTORY_DEVIATION_THRESHOLD_METERS) + { + return true; + } + } + + return false; +} + +bool PrimitiveExecutor::isAngularTrajectoryNew( + const BangBangTrajectory1DAngular& new_trajectory) const +{ + if (!angular_trajectory_.has_value()) + { + return true; + } + + // Regenerate if the destination rotated meaningfully (also catches changes beyond + // the comparison horizon below). + if (!angular_trajectory_->equals(new_trajectory, ANGULAR_DESTINATION_THRESHOLD_DEGREES)) + { + return true; + } + + // Compare the two plans over the same upcoming time window (see isLinearTrajectoryNew + // for rationale), regenerating if they ever diverge by more than the threshold. + const double old_time = time_since_angular_trajectory_creation_.toSeconds(); + const double new_time = VISION_TO_ROBOT_DELAY_S; + const double horizon = + std::min(std::max(angular_trajectory_->getTotalTime() - old_time, + new_trajectory.getTotalTime() - new_time), + TRAJECTORY_DEVIATION_HORIZON_S); + + for (double tau = 0.0; tau <= horizon; tau += TRAJECTORY_DEVIATION_TIME_STEP_S) + { + const Angle old_orientation = angular_trajectory_->getPosition(old_time + tau); + const Angle new_orientation = new_trajectory.getPosition(new_time + tau); + if (old_orientation.minDiff(new_orientation).toDegrees() > + ANGULAR_TRAJECTORY_DEVIATION_THRESHOLD_DEGREES) + { + return true; + } + } + + return false; +} + void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_msg) { current_primitive_ = primitive_msg; @@ -26,19 +113,29 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), state_.velocity(), robot_constants_); + // Only regenerate the trajectory (and reset the controller/clock) when the + // destination has meaningfully changed. The AI re-sends the same primitive + // every tick; regenerating unconditionally would pin the robot to the start + // of a fresh trajectory forever, so it would never accelerate along it. + if (isLinearTrajectoryNew(new_trajectory_path)) + { + trajectory_path_ = new_trajectory_path; + position_controller_.reset(); + time_since_linear_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + } + const auto new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), state_.angularVelocity(), robot_constants_); - trajectory_path_ = new_trajectory_path; - position_controller_.reset(); - time_since_linear_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); - - angular_trajectory_ = new_angular_trajectory; - orientation_controller_.reset(); - time_since_angular_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + if (isAngularTrajectoryNew(new_angular_trajectory)) + { + angular_trajectory_ = new_angular_trajectory; + orientation_controller_.reset(); + time_since_angular_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + } } } diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index b3f39efe78..f7699b3484 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -48,6 +48,26 @@ class PrimitiveExecutor TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time); private: + /** + * Returns whether the given linear trajectory is meaningfully different from the + * one currently being followed (i.e. its destination has moved by more than + * LINEAR_DESTINATION_THRESHOLD_METERS), and so should replace it. + * + * @param new_trajectory The newly generated linear trajectory + * @return true if the trajectory should be regenerated, false to keep the current one + */ + bool isLinearTrajectoryNew(const std::optional& new_trajectory) const; + + /** + * Returns whether the given angular trajectory is meaningfully different from the + * one currently being followed (i.e. its destination has rotated by more than + * ANGULAR_DESTINATION_THRESHOLD_DEGREES), and so should replace it. + * + * @param new_trajectory The newly generated angular trajectory + * @return true if the trajectory should be regenerated, false to keep the current one + */ + bool isAngularTrajectoryNew(const BangBangTrajectory1DAngular& new_trajectory) const; + /* * Compute the next target linear _local_ velocity the robot should have. * @param delta_time The elapsed time since last time step @@ -110,8 +130,21 @@ class PrimitiveExecutor // to avoid jittering around the destination. static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; - // If distance between current linear trajectory destination and new one is larger - // than this, we change trajectories. + // If the destination of the current and newly-generated trajectory differ by more + // than these, we regenerate the trajectory. static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; + + // Even when the destination is unchanged, the path to it may change (e.g. a newly + // detected obstacle forces a detour). We compare the current and newly-generated + // trajectories over an upcoming time window and regenerate if they ever diverge by + // more than these thresholds. + static constexpr double LINEAR_TRAJECTORY_DEVIATION_THRESHOLD_METERS = 0.05; + static constexpr double ANGULAR_TRAJECTORY_DEVIATION_THRESHOLD_DEGREES = 10.0; + + // The time window (and sampling step) over which the current and newly-generated + // trajectories are compared, anchored at the robot's current point on the + // trajectory. Capped so comparison stays cheap for long trajectories. + static constexpr double TRAJECTORY_DEVIATION_HORIZON_S = 1.0; + static constexpr double TRAJECTORY_DEVIATION_TIME_STEP_S = 0.1; }; From 83e8911eaa0ee2656763c6316755ea7700e1a5af Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Mon, 15 Jun 2026 06:00:37 +0000 Subject: [PATCH 30/46] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/primitive_executor.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 7f71fa93b5..f3d2a6087a 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -47,10 +47,9 @@ bool PrimitiveExecutor::isLinearTrajectoryNew( // not). Regenerate if the plans ever diverge by more than the deviation threshold. const double old_time = time_since_linear_trajectory_creation_.toSeconds(); const double new_time = VISION_TO_ROBOT_DELAY_S; - const double horizon = - std::min(std::max(trajectory_path_->getTotalTime() - old_time, - new_trajectory->getTotalTime() - new_time), - TRAJECTORY_DEVIATION_HORIZON_S); + const double horizon = std::min(std::max(trajectory_path_->getTotalTime() - old_time, + new_trajectory->getTotalTime() - new_time), + TRAJECTORY_DEVIATION_HORIZON_S); for (double tau = 0.0; tau <= horizon; tau += TRAJECTORY_DEVIATION_TIME_STEP_S) { @@ -75,7 +74,8 @@ bool PrimitiveExecutor::isAngularTrajectoryNew( // Regenerate if the destination rotated meaningfully (also catches changes beyond // the comparison horizon below). - if (!angular_trajectory_->equals(new_trajectory, ANGULAR_DESTINATION_THRESHOLD_DEGREES)) + if (!angular_trajectory_->equals(new_trajectory, + ANGULAR_DESTINATION_THRESHOLD_DEGREES)) { return true; } From e7cea8a9a294c0cef73d5ea73e50c8a6a85453a8 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Mon, 15 Jun 2026 17:18:52 -0700 Subject: [PATCH 31/46] localizer tests --- src/MODULE.bazel | 2 +- src/software/embedded/BUILD | 11 ++ src/software/embedded/primitive_executor.cpp | 115 ++---------------- src/software/embedded/primitive_executor.h | 37 +----- src/software/embedded/robot_localizer.cpp | 38 +++++- .../embedded/robot_localizer_test.cpp | 114 +++++++++++++++++ src/software/embedded/thunderloop.cpp | 4 +- 7 files changed, 171 insertions(+), 150 deletions(-) create mode 100644 src/software/embedded/robot_localizer_test.cpp diff --git a/src/MODULE.bazel b/src/MODULE.bazel index 21c9aca242..6ec0c832df 100644 --- a/src/MODULE.bazel +++ b/src/MODULE.bazel @@ -219,7 +219,7 @@ git_repository( name = "mdv6_firmware", build_file = "@//extlibs:mdv6_firmware.bzl", # Pinning to the exact commit guarantees Bazel will rebuild when this commit SHA is updated - commit = "97e06b9ce8db1940a033b536cd55b4059943d601", # latest commit on master branch + commit = "0f14b9482985159dd132c6fa012970783b0263c3", # latest commit on master branch remote = "https://github.com/UBC-Thunderbots/MDv6_Firmware.git", ) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 4e275f39c7..50e182e817 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -165,3 +165,14 @@ cc_library( "@eigen", ], ) + +cc_test( + name = "robot_localizer_test", + srcs = ["robot_localizer_test.cpp"], + deps = [ + ":robot_localizer", + "//shared:constants", + "//shared/test_util:tbots_gtest_main", + "//software/physics:velocity_conversion_util", + ], +) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 7f71fa93b5..4715d236c5 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -1,7 +1,5 @@ #include "software/embedded/primitive_executor.h" -#include - #include "proto/message_translation/tbots_geometry.h" #include "proto/message_translation/tbots_protobuf.h" #include "proto/primitive.pb.h" @@ -18,91 +16,6 @@ PrimitiveExecutor::PrimitiveExecutor( { } -bool PrimitiveExecutor::isLinearTrajectoryNew( - const std::optional& new_trajectory) const -{ - if (new_trajectory.has_value() != trajectory_path_.has_value()) - { - // We either don't have a trajectory and the new one does, or we have one and - // the new one doesn't. - return true; - } - - if (!new_trajectory.has_value()) - { - return false; - } - - // Regenerate if the destination moved meaningfully. This also catches changes - // beyond the comparison horizon below (e.g. a far-away destination shifting). - if (!trajectory_path_->equals(*new_trajectory, LINEAR_DESTINATION_THRESHOLD_METERS)) - { - return true; - } - - // The destinations match, but the path to get there may have changed (e.g. a - // newly-detected obstacle forces a detour). Compare the two plans over the same - // upcoming time window, anchored at where we currently are on the old trajectory, - // so timing/speed differences are accounted for (a purely spatial comparison would - // not). Regenerate if the plans ever diverge by more than the deviation threshold. - const double old_time = time_since_linear_trajectory_creation_.toSeconds(); - const double new_time = VISION_TO_ROBOT_DELAY_S; - const double horizon = - std::min(std::max(trajectory_path_->getTotalTime() - old_time, - new_trajectory->getTotalTime() - new_time), - TRAJECTORY_DEVIATION_HORIZON_S); - - for (double tau = 0.0; tau <= horizon; tau += TRAJECTORY_DEVIATION_TIME_STEP_S) - { - const Point old_pos = trajectory_path_->getPosition(old_time + tau); - const Point new_pos = new_trajectory->getPosition(new_time + tau); - if (distance(old_pos, new_pos) > LINEAR_TRAJECTORY_DEVIATION_THRESHOLD_METERS) - { - return true; - } - } - - return false; -} - -bool PrimitiveExecutor::isAngularTrajectoryNew( - const BangBangTrajectory1DAngular& new_trajectory) const -{ - if (!angular_trajectory_.has_value()) - { - return true; - } - - // Regenerate if the destination rotated meaningfully (also catches changes beyond - // the comparison horizon below). - if (!angular_trajectory_->equals(new_trajectory, ANGULAR_DESTINATION_THRESHOLD_DEGREES)) - { - return true; - } - - // Compare the two plans over the same upcoming time window (see isLinearTrajectoryNew - // for rationale), regenerating if they ever diverge by more than the threshold. - const double old_time = time_since_angular_trajectory_creation_.toSeconds(); - const double new_time = VISION_TO_ROBOT_DELAY_S; - const double horizon = - std::min(std::max(angular_trajectory_->getTotalTime() - old_time, - new_trajectory.getTotalTime() - new_time), - TRAJECTORY_DEVIATION_HORIZON_S); - - for (double tau = 0.0; tau <= horizon; tau += TRAJECTORY_DEVIATION_TIME_STEP_S) - { - const Angle old_orientation = angular_trajectory_->getPosition(old_time + tau); - const Angle new_orientation = new_trajectory.getPosition(new_time + tau); - if (old_orientation.minDiff(new_orientation).toDegrees() > - ANGULAR_TRAJECTORY_DEVIATION_THRESHOLD_DEGREES) - { - return true; - } - } - - return false; -} - void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_msg) { current_primitive_ = primitive_msg; @@ -113,29 +26,19 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), state_.velocity(), robot_constants_); - // Only regenerate the trajectory (and reset the controller/clock) when the - // destination has meaningfully changed. The AI re-sends the same primitive - // every tick; regenerating unconditionally would pin the robot to the start - // of a fresh trajectory forever, so it would never accelerate along it. - if (isLinearTrajectoryNew(new_trajectory_path)) - { - trajectory_path_ = new_trajectory_path; - position_controller_.reset(); - time_since_linear_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); - } - const auto new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), state_.angularVelocity(), robot_constants_); - if (isAngularTrajectoryNew(new_angular_trajectory)) - { - angular_trajectory_ = new_angular_trajectory; - orientation_controller_.reset(); - time_since_angular_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); - } + trajectory_path_ = new_trajectory_path; + position_controller_.reset(); + time_since_linear_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + + angular_trajectory_ = new_angular_trajectory; + orientation_controller_.reset(); + time_since_angular_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); } } diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index f7699b3484..b3f39efe78 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -48,26 +48,6 @@ class PrimitiveExecutor TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time); private: - /** - * Returns whether the given linear trajectory is meaningfully different from the - * one currently being followed (i.e. its destination has moved by more than - * LINEAR_DESTINATION_THRESHOLD_METERS), and so should replace it. - * - * @param new_trajectory The newly generated linear trajectory - * @return true if the trajectory should be regenerated, false to keep the current one - */ - bool isLinearTrajectoryNew(const std::optional& new_trajectory) const; - - /** - * Returns whether the given angular trajectory is meaningfully different from the - * one currently being followed (i.e. its destination has rotated by more than - * ANGULAR_DESTINATION_THRESHOLD_DEGREES), and so should replace it. - * - * @param new_trajectory The newly generated angular trajectory - * @return true if the trajectory should be regenerated, false to keep the current one - */ - bool isAngularTrajectoryNew(const BangBangTrajectory1DAngular& new_trajectory) const; - /* * Compute the next target linear _local_ velocity the robot should have. * @param delta_time The elapsed time since last time step @@ -130,21 +110,8 @@ class PrimitiveExecutor // to avoid jittering around the destination. static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; - // If the destination of the current and newly-generated trajectory differ by more - // than these, we regenerate the trajectory. + // If distance between current linear trajectory destination and new one is larger + // than this, we change trajectories. static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; - - // Even when the destination is unchanged, the path to it may change (e.g. a newly - // detected obstacle forces a detour). We compare the current and newly-generated - // trajectories over an upcoming time window and regenerate if they ever diverge by - // more than these thresholds. - static constexpr double LINEAR_TRAJECTORY_DEVIATION_THRESHOLD_METERS = 0.05; - static constexpr double ANGULAR_TRAJECTORY_DEVIATION_THRESHOLD_DEGREES = 10.0; - - // The time window (and sampling step) over which the current and newly-generated - // trajectories are compared, anchored at the robot's current point on the - // trajectory. Capped so comparison stays cheap for long trajectories. - static constexpr double TRAJECTORY_DEVIATION_HORIZON_S = 1.0; - static constexpr double TRAJECTORY_DEVIATION_TIME_STEP_S = 0.1; }; diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index a095983d90..1f780e926e 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -159,17 +159,23 @@ void RobotLocalizer::update(const VisionData& data) rollback_point = std::prev(history.end()); } - // 1. Save the state from the rollback point + // 1. Roll the filter back to the state from just before the rollback point's + // operation (its stored state is captured pre-operation). filter_.state_estimate = rollback_point->state_estimate; filter_.state_covariance = rollback_point->state_covariance; - // 2. Truncate history (erase rollback_point and everything older) - history.erase(rollback_point, history.end()); + // 2. Drop everything strictly older than the rollback point, but KEEP the rollback + // point itself so its operation is replayed after the vision measurement is + // inserted. (Erasing the rollback point too would silently drop one + // predict/update step on every vision frame, making the position estimate lag + // and inflating the velocity estimate through the position/velocity covariance.) + history.erase(std::next(rollback_point), history.end()); - // 3. Apply the delayed vision measurement + // 3. Apply the delayed vision measurement at the rolled-back time. updateFilterWithVision(data.position, data.orientation); - // 4. Replay the remaining history (from oldest to newest) + // 4. Replay the remaining history (from oldest to newest), including the rollback + // point's own operation. for (auto it = history.rbegin(); it != history.rend(); ++it) { if (it->prediction.has_value()) @@ -225,7 +231,29 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, static_cast(MeasurementIndex::VISION_ORIENTATION), static_cast(StateIndex::ORIENTATION)) = 1; + // Vision observes pose (position + orientation), not velocity. The velocity states + // are observed directly by the wheel encoders (and IMU for angular velocity), which + // are accurate. Letting the vision position/orientation correction also adjust the + // velocity states through the position<->velocity covariance coupling is unstable + // here: the tight vision noise leaves the position correction lagging, and that + // persistent lag is fed into the velocity estimate every frame, inflating it (a + // robot moving 1 m/s was estimated at ~1.8 m/s, and worse without delay + // compensation). So we hold the velocity states fixed across the vision update. + const double x_velocity = + filter_.state_estimate(static_cast(StateIndex::X_VELOCITY)); + const double y_velocity = + filter_.state_estimate(static_cast(StateIndex::Y_VELOCITY)); + const double angular_velocity = + filter_.state_estimate(static_cast(StateIndex::ANGULAR_VELOCITY)); + filter_.update(measurement); + + filter_.state_estimate(static_cast(StateIndex::X_VELOCITY)) = + x_velocity; + filter_.state_estimate(static_cast(StateIndex::Y_VELOCITY)) = + y_velocity; + filter_.state_estimate(static_cast(StateIndex::ANGULAR_VELOCITY)) = + angular_velocity; } void RobotLocalizer::update(const MotorData& data) diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp new file mode 100644 index 0000000000..772743349f --- /dev/null +++ b/src/software/embedded/robot_localizer_test.cpp @@ -0,0 +1,114 @@ +#include "software/embedded/robot_localizer.h" + +#include + +#include + +#include "shared/constants.h" +#include "software/physics/velocity_conversion_util.h" + +namespace +{ +// Mirror the values thunderloop constructs the localizer with (DivB constants). +RobotLocalizer::RobotLocalizerConfig makeConfig() +{ + return RobotLocalizer::RobotLocalizerConfig{ + /*process_noise_variance=*/1.0, + /*vision_noise_variance=*/0.01 * 0.01, + /*motor_sensor_noise_variance=*/0.5}; +} + +constexpr double LOOP_HZ = 300.0; +constexpr double DT = 1.0 / LOOP_HZ; + +// Drives the localizer through one second of a robot moving straight along global +X at +// a constant 1 m/s (heading along +X, not rotating), feeding it the same inputs +// thunderloop does each tick. If feed_vision is false, only the initial vision seed is +// provided (isolates whether the periodic vision fix corrupts the velocity estimate). +RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / 2) +{ + RobotLocalizer localizer(makeConfig()); + + const Vector true_velocity(1.0, 0.0); + const Angle true_orientation = Angle::zero(); + Point true_position(0.0, 0.0); + + localizer.update( + RobotLocalizer::VisionData{true_position, true_orientation, RTT_S / 2}); + + for (int tick = 0; tick < static_cast(LOOP_HZ); ++tick) + { + true_position += true_velocity * DT; + + localizer.update(RobotLocalizer::ImuData{AngularVelocity::zero()}); + + const Vector local_velocity = + globalToLocalVelocity(true_velocity, true_orientation); + localizer.update(RobotLocalizer::MotorData{ + localToGlobalVelocity(local_velocity, localizer.getOrientation()), + AngularVelocity::zero()}); + + localizer.step(Vector(0.0, 0.0)); + + // Periodic vision fix (~60 Hz). Feed the position from RTT_S/2 ago, consistent + // with the reported age. + if (feed_vision && tick % 5 == 0) + { + localizer.update(RobotLocalizer::VisionData{ + true_position - true_velocity * vision_age, true_orientation, + vision_age}); + } + } + + return localizer; +} +} // namespace + +// Regression test mirroring the (fixed) thunderloop wiring: orientation, velocity, and +// position should all track a constant-velocity robot. +TEST(RobotLocalizer, tracks_constant_forward_velocity) +{ + const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/true); + + std::cerr << "[motor+vision] pos=(" << localizer.getPosition().x() << ", " + << localizer.getPosition().y() << ") vel=(" << localizer.getVelocity().x() + << ", " << localizer.getVelocity().y() + << ") orient=" << localizer.getOrientation().toDegrees() << "deg\n"; + + EXPECT_NEAR(localizer.getOrientation().toDegrees(), 0.0, 10.0); + EXPECT_NEAR(localizer.getVelocity().x(), 1.0, 0.2) + << "Forward velocity estimate does not track"; + EXPECT_NEAR(localizer.getVelocity().y(), 0.0, 0.2); + EXPECT_NEAR(localizer.getPosition().x(), 1.0, 0.2); + EXPECT_NEAR(localizer.getPosition().y(), 0.0, 0.2); +} + +// Diagnostic: with no periodic vision fix, the velocity estimate comes purely from the +// motor measurements (a constant 1 m/s). If this tracks but the test above does not, +// the periodic vision update is what corrupts the velocity estimate. +TEST(RobotLocalizer, velocity_tracks_from_motors_without_vision) +{ + const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/false); + + std::cerr << "[motor only] vel=(" << localizer.getVelocity().x() << ", " + << localizer.getVelocity().y() << ")\n"; + + EXPECT_NEAR(localizer.getVelocity().x(), 1.0, 0.2); + EXPECT_NEAR(localizer.getVelocity().y(), 0.0, 0.2); +} + +// Diagnostic: feed vision with a near-zero age, which takes the non-rollback path +// (apply vision to the current state, clear history). If velocity tracks here but not +// with a realistic age, the rollback/replay machinery is the culprit; if it's still +// wrong, the vision measurement's position->velocity covariance coupling is. +TEST(RobotLocalizer, velocity_with_zero_age_vision) +{ + const RobotLocalizer localizer = + runConstantVelocity(/*feed_vision=*/true, /*vision_age=*/1e-6); + + std::cerr << "[zero-age vision] vel=(" << localizer.getVelocity().x() << ", " + << localizer.getVelocity().y() << ")\n"; + + EXPECT_NEAR(localizer.getVelocity().x(), 1.0, 0.2); + EXPECT_NEAR(localizer.getVelocity().y(), 0.0, 0.2); +} diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 50018a9d1f..d29bd376bc 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -297,14 +297,12 @@ inline void Thunderloop::processNetworkPolling( primitive_ = new_primitive; // Feed the trajectory's starting pose to the localizer as a vision update. - // The start angle is offset by half a turn to match the robot's convention. if (primitive_.has_move()) { const Point position = createPoint(primitive_.move().xy_traj_params().start_position()); const Angle orientation = - createAngle(primitive_.move().w_traj_params().start_angle()) + - Angle::half(); + createAngle(primitive_.move().w_traj_params().start_angle()); robot_localizer_.update( RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); From 7f2ceae70906ef4830218c96ecd0457b87dbdc3a Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Mon, 15 Jun 2026 18:05:24 -0700 Subject: [PATCH 32/46] merge fixes --- src/software/embedded/primitive_executor.cpp | 113 ++----------------- 1 file changed, 9 insertions(+), 104 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index c634691a5c..4715d236c5 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -16,91 +16,6 @@ PrimitiveExecutor::PrimitiveExecutor( { } -bool PrimitiveExecutor::isLinearTrajectoryNew( - const std::optional& new_trajectory) const -{ - if (new_trajectory.has_value() != trajectory_path_.has_value()) - { - // We either don't have a trajectory and the new one does, or we have one and - // the new one doesn't. - return true; - } - - if (!new_trajectory.has_value()) - { - return false; - } - - // Regenerate if the destination moved meaningfully. This also catches changes - // beyond the comparison horizon below (e.g. a far-away destination shifting). - if (!trajectory_path_->equals(*new_trajectory, LINEAR_DESTINATION_THRESHOLD_METERS)) - { - return true; - } - - // The destinations match, but the path to get there may have changed (e.g. a - // newly-detected obstacle forces a detour). Compare the two plans over the same - // upcoming time window, anchored at where we currently are on the old trajectory, - // so timing/speed differences are accounted for (a purely spatial comparison would - // not). Regenerate if the plans ever diverge by more than the deviation threshold. - const double old_time = time_since_linear_trajectory_creation_.toSeconds(); - const double new_time = VISION_TO_ROBOT_DELAY_S; - const double horizon = std::min(std::max(trajectory_path_->getTotalTime() - old_time, - new_trajectory->getTotalTime() - new_time), - TRAJECTORY_DEVIATION_HORIZON_S); - - for (double tau = 0.0; tau <= horizon; tau += TRAJECTORY_DEVIATION_TIME_STEP_S) - { - const Point old_pos = trajectory_path_->getPosition(old_time + tau); - const Point new_pos = new_trajectory->getPosition(new_time + tau); - if (distance(old_pos, new_pos) > LINEAR_TRAJECTORY_DEVIATION_THRESHOLD_METERS) - { - return true; - } - } - - return false; -} - -bool PrimitiveExecutor::isAngularTrajectoryNew( - const BangBangTrajectory1DAngular& new_trajectory) const -{ - if (!angular_trajectory_.has_value()) - { - return true; - } - - // Regenerate if the destination rotated meaningfully (also catches changes beyond - // the comparison horizon below). - if (!angular_trajectory_->equals(new_trajectory, - ANGULAR_DESTINATION_THRESHOLD_DEGREES)) - { - return true; - } - - // Compare the two plans over the same upcoming time window (see isLinearTrajectoryNew - // for rationale), regenerating if they ever diverge by more than the threshold. - const double old_time = time_since_angular_trajectory_creation_.toSeconds(); - const double new_time = VISION_TO_ROBOT_DELAY_S; - const double horizon = - std::min(std::max(angular_trajectory_->getTotalTime() - old_time, - new_trajectory.getTotalTime() - new_time), - TRAJECTORY_DEVIATION_HORIZON_S); - - for (double tau = 0.0; tau <= horizon; tau += TRAJECTORY_DEVIATION_TIME_STEP_S) - { - const Angle old_orientation = angular_trajectory_->getPosition(old_time + tau); - const Angle new_orientation = new_trajectory.getPosition(new_time + tau); - if (old_orientation.minDiff(new_orientation).toDegrees() > - ANGULAR_TRAJECTORY_DEVIATION_THRESHOLD_DEGREES) - { - return true; - } - } - - return false; -} - void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_msg) { current_primitive_ = primitive_msg; @@ -111,29 +26,19 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), state_.velocity(), robot_constants_); - // Only regenerate the trajectory (and reset the controller/clock) when the - // destination has meaningfully changed. The AI re-sends the same primitive - // every tick; regenerating unconditionally would pin the robot to the start - // of a fresh trajectory forever, so it would never accelerate along it. - if (isLinearTrajectoryNew(new_trajectory_path)) - { - trajectory_path_ = new_trajectory_path; - position_controller_.reset(); - time_since_linear_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); - } - const auto new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), state_.angularVelocity(), robot_constants_); - if (isAngularTrajectoryNew(new_angular_trajectory)) - { - angular_trajectory_ = new_angular_trajectory; - orientation_controller_.reset(); - time_since_angular_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); - } + trajectory_path_ = new_trajectory_path; + position_controller_.reset(); + time_since_linear_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + + angular_trajectory_ = new_angular_trajectory; + orientation_controller_.reset(); + time_since_angular_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); } } From 39a2c16dae456d6a3e40a26362a1280fc18f39ca Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 15 Jun 2026 21:14:12 -0700 Subject: [PATCH 33/46] pi values --- src/shared/robot_constants.h | 4 ++-- .../motor_controller/stspin_motor_controller.cpp | 9 +++++++++ .../embedded/motor_controller/stspin_motor_controller.h | 7 +++++-- src/software/embedded/services/motor.cpp | 2 ++ src/software/thunderscope/wifi_communication_manager.py | 8 ++++---- 5 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index be674b45b5..7a93c64cb9 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -159,8 +159,8 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_trajectory_max_speed_m_per_s = 2.5f, - .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_trajectory_max_speed_m_per_s = 1.5f, + .robot_max_acceleration_m_per_s_2 = 2.0f, .robot_max_deceleration_m_per_s_2 = 2.0f, // Robot's angular movement constants diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index e28ccfb402..490127c590 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -47,6 +47,8 @@ void StSpinMotorController::setup() { sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = SPEED_PID_PROPORTIONAL_GAIN, .ki = SPEED_PID_INTEGRAL_GAIN}); + sendAndReceiveFrame(motor, SetPidTorqueKpKiFrame{.kp = TORQUE_PID_PROPORTIONAL_GAIN, + .ki = TORQUE_PID_INTEGRAL_GAIN}); } } @@ -189,6 +191,7 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, void StSpinMotorController::updateEuclideanVelocity( EuclideanSpace_t target_euclidean_velocity) { +#if 0 const Vector local_velocity(target_euclidean_velocity[1], target_euclidean_velocity[0]); @@ -237,6 +240,12 @@ void StSpinMotorController::updateEuclideanVelocity( SetSpeedFeedForwardKsFrame{.ks = back_right_ks}); sendAndReceiveFrame(MotorIndex::BACK_LEFT, SetSpeedFeedForwardKsFrame{.ks = back_left_ks}); +#endif + sendMotorStatusToPlotJuggler(MotorIndex::FRONT_LEFT); + sendMotorStatusToPlotJuggler(MotorIndex::FRONT_RIGHT); + sendMotorStatusToPlotJuggler(MotorIndex::BACK_LEFT); + sendMotorStatusToPlotJuggler(MotorIndex::BACK_RIGHT); + } void StSpinMotorController::immediatelyDisable() diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 13b2d3be7d..6655ab7841 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -60,8 +60,11 @@ class StSpinMotorController : public MotorController static constexpr int RESET_GPIO_PIN = 12; - static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 2000; - static constexpr int SPEED_PID_INTEGRAL_GAIN = 200; + static constexpr int SPEED_PID_PROPORTIONAL_GAIN = 1620; + static constexpr int SPEED_PID_INTEGRAL_GAIN = 883; + + static constexpr int TORQUE_PID_PROPORTIONAL_GAIN = 309; + static constexpr int TORQUE_PID_INTEGRAL_GAIN = 156; static constexpr int MAX_SPEED_FEED_FORWARD_STATIC_GAIN = 750; static constexpr int MIN_SPEED_FEED_FORWARD_STATIC_GAIN = 300; diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 7e53672fce..d5d6ee8e24 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -138,6 +138,7 @@ void MotorService::poll(const TbotsProto::DirectControlPrimitive& primitive, TbotsProto::MotorStatus motor_status = createMotorStatus(current_wheel_velocities, dribbler_rpm); + #if 0 if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) @@ -166,6 +167,7 @@ void MotorService::poll(const TbotsProto::DirectControlPrimitive& primitive, motor_controller_->immediatelyDisable(); LOG(FATAL) << "Back right motor runaway"; } +#endif // Convert to Euclidean velocity_delta const EuclideanSpace_t current_euclidean_velocity = diff --git a/src/software/thunderscope/wifi_communication_manager.py b/src/software/thunderscope/wifi_communication_manager.py index 46ad98669d..39423aeab1 100644 --- a/src/software/thunderscope/wifi_communication_manager.py +++ b/src/software/thunderscope/wifi_communication_manager.py @@ -410,7 +410,7 @@ def send_primitive(self, robot_id: int, primitive: Primitive) -> None: primitive_sender = self.primitive_senders[robot_id][1] if primitive_sender is not None: primitive_sender.send_proto(primitive, True) - else: - logger.warning( - f"Robot {robot_id} is not connected. Unable to send a primitive to it." - ) + # else: + # logger.warning( + # f"Robot {robot_id} is not connected. Unable to send a primitive to it." + # ) From e870f9a32e975f57a69bdfcfb2446cb70c318f5c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Tue, 16 Jun 2026 04:21:53 +0000 Subject: [PATCH 34/46] [pre-commit.ci lite] apply automatic fixes --- .../motor_controller/stspin_motor_controller.cpp | 6 +++--- src/software/embedded/robot_localizer_test.cpp | 13 ++++++------- src/software/embedded/services/motor.cpp | 2 +- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 490127c590..37fa83ace2 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -47,8 +47,9 @@ void StSpinMotorController::setup() { sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = SPEED_PID_PROPORTIONAL_GAIN, .ki = SPEED_PID_INTEGRAL_GAIN}); - sendAndReceiveFrame(motor, SetPidTorqueKpKiFrame{.kp = TORQUE_PID_PROPORTIONAL_GAIN, - .ki = TORQUE_PID_INTEGRAL_GAIN}); + sendAndReceiveFrame(motor, + SetPidTorqueKpKiFrame{.kp = TORQUE_PID_PROPORTIONAL_GAIN, + .ki = TORQUE_PID_INTEGRAL_GAIN}); } } @@ -245,7 +246,6 @@ void StSpinMotorController::updateEuclideanVelocity( sendMotorStatusToPlotJuggler(MotorIndex::FRONT_RIGHT); sendMotorStatusToPlotJuggler(MotorIndex::BACK_LEFT); sendMotorStatusToPlotJuggler(MotorIndex::BACK_RIGHT); - } void StSpinMotorController::immediatelyDisable() diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp index 772743349f..3021025fdf 100644 --- a/src/software/embedded/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer_test.cpp @@ -12,10 +12,9 @@ namespace // Mirror the values thunderloop constructs the localizer with (DivB constants). RobotLocalizer::RobotLocalizerConfig makeConfig() { - return RobotLocalizer::RobotLocalizerConfig{ - /*process_noise_variance=*/1.0, - /*vision_noise_variance=*/0.01 * 0.01, - /*motor_sensor_noise_variance=*/0.5}; + return RobotLocalizer::RobotLocalizerConfig{/*process_noise_variance=*/1.0, + /*vision_noise_variance=*/0.01 * 0.01, + /*motor_sensor_noise_variance=*/0.5}; } constexpr double LOOP_HZ = 300.0; @@ -54,9 +53,9 @@ RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / // with the reported age. if (feed_vision && tick % 5 == 0) { - localizer.update(RobotLocalizer::VisionData{ - true_position - true_velocity * vision_age, true_orientation, - vision_age}); + localizer.update( + RobotLocalizer::VisionData{true_position - true_velocity * vision_age, + true_orientation, vision_age}); } } diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index d5d6ee8e24..ab650af672 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -138,7 +138,7 @@ void MotorService::poll(const TbotsProto::DirectControlPrimitive& primitive, TbotsProto::MotorStatus motor_status = createMotorStatus(current_wheel_velocities, dribbler_rpm); - #if 0 +#if 0 if (std::abs(current_wheel_velocities[FRONT_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) From 6957006fef23a8402b54bc6dfdbcd7418ad77f87 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 16 Jun 2026 18:53:29 -0700 Subject: [PATCH 35/46] breakout --- src/shared/robot_constants.h | 4 +- .../ai/hl/stp/play/ball_placement/BUILD | 2 + .../ai/hl/stp/play/kickoff_enemy/BUILD | 2 + src/software/ai/navigator/trajectory/BUILD | 1 - .../trajectory/bang_bang_trajectory_1d.h | 13 ----- .../bang_bang_trajectory_1d_angular.h | 13 ----- .../ai/navigator/trajectory/trajectory.hpp | 9 ---- .../ai/navigator/trajectory/trajectory_2d.h | 14 ----- src/software/embedded/primitive_executor.cpp | 31 ++++++----- src/software/embedded/primitive_executor.h | 2 +- .../embedded/robot_localizer_test.cpp | 10 +++- src/software/multithreading/BUILD | 3 ++ src/software/power/charger.cpp | 10 ++++ src/software/power/charger.h | 16 ++++-- src/software/power/control_executor.cpp | 52 +++++++++++-------- src/software/power/pins.h | 10 ++-- 16 files changed, 94 insertions(+), 98 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 7a93c64cb9..be674b45b5 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -159,8 +159,8 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_trajectory_max_speed_m_per_s = 1.5f, - .robot_max_acceleration_m_per_s_2 = 2.0f, + .robot_trajectory_max_speed_m_per_s = 2.5f, + .robot_max_acceleration_m_per_s_2 = 3.0f, .robot_max_deceleration_m_per_s_2 = 2.0f, // Robot's angular movement constants diff --git a/src/software/ai/hl/stp/play/ball_placement/BUILD b/src/software/ai/hl/stp/play/ball_placement/BUILD index 0666696e7c..8e2e08621e 100644 --- a/src/software/ai/hl/stp/play/ball_placement/BUILD +++ b/src/software/ai/hl/stp/play/ball_placement/BUILD @@ -34,6 +34,8 @@ cc_library( py_test( name = "ball_placement_play_test", timeout = "long", + # Flaky: multi-robot simulated play sensitive to sim timing; passes on retry. + flaky = True, srcs = [ "ball_placement_play_test.py", ], diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/BUILD b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD index 0a5a92203e..eba9a7d517 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/BUILD +++ b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD @@ -28,6 +28,8 @@ cc_library( py_test( name = "kickoff_enemy_play_test", + # Flaky: multi-robot simulated play sensitive to sim timing; passes on retry. + flaky = True, srcs = ["kickoff_enemy_play_test.py"], deps = [ "//software:conftest", diff --git a/src/software/ai/navigator/trajectory/BUILD b/src/software/ai/navigator/trajectory/BUILD index 9c1e7870d8..6a5e23ecf1 100644 --- a/src/software/ai/navigator/trajectory/BUILD +++ b/src/software/ai/navigator/trajectory/BUILD @@ -21,7 +21,6 @@ cc_library( deps = [ ":trajectory", "//software/geom:point", - "//software/geom/algorithms", ], ) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index 4bce8c76df..3f0fdf9c03 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -104,19 +104,6 @@ class BangBangTrajectory1D : public Trajectory */ size_t getNumTrajectoryParts() const; - /** - * Check if this trajectory is meaningfully equal to another trajectory. - * @param other The other trajectory to compare to - * @param threshold The threshold below which the trajectories are considered - * equal - * @return True if the trajectories are equal, false otherwise - */ - bool equals(const Trajectory& other, - double threshold) const override - { - return std::abs(getDestination() - other.getDestination()) <= threshold; - } - private: /** * Generate a trapezoidal trajectory and fill in `trajectory_parts`. diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h index 3c87b039c2..2ffbff5ec2 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h @@ -83,19 +83,6 @@ class BangBangTrajectory1DAngular */ double getTotalTime() const override; - /** - * Check if this trajectory is meaningfully equal to another trajectory. - * @param other The other trajectory to compare to - * @param threshold The threshold below which the trajectories are considered - * equal in degrees. - * @return True if the trajectories are equal, false otherwise - */ - bool equals(const Trajectory& other, - double threshold) const override - { - return getDestination().minDiff(other.getDestination()).toDegrees() <= threshold; - } - private: BangBangTrajectory1D trajectory; }; diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index caffd6b86c..4e7a7ee77e 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -59,13 +59,4 @@ class Trajectory { return getPosition(getTotalTime()); } - - /** - * Check if this trajectory is meaningfully equal to another trajectory. - * @param other The other trajectory to compare to - * @param threshold The threshold below which the trajectories are considered - * equal - * @return True if the trajectories are equal, false otherwise - */ - virtual bool equals(const Trajectory& other, double threshold) const = 0; }; diff --git a/src/software/ai/navigator/trajectory/trajectory_2d.h b/src/software/ai/navigator/trajectory/trajectory_2d.h index e40b367f58..9699f07c17 100644 --- a/src/software/ai/navigator/trajectory/trajectory_2d.h +++ b/src/software/ai/navigator/trajectory/trajectory_2d.h @@ -1,7 +1,6 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory.hpp" -#include "software/geom/algorithms/distance.h" #include "software/geom/point.h" #include "software/geom/rectangle.h" @@ -15,17 +14,4 @@ class Trajectory2D : virtual public Trajectory * @return bounding boxes which this trajectory passes through */ virtual std::vector getBoundingBoxes() const = 0; - - /** - * Check if this trajectory is meaningfully equal to another trajectory. - * @param other The other trajectory to compare to - * @param threshold The threshold below which the trajectories are considered - * equal - * @return True if the trajectories are equal, false otherwise - */ - bool equals(const Trajectory& other, - double threshold) const override - { - return distance(getDestination(), other.getDestination()) <= threshold; - } }; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 4715d236c5..0e625f79c4 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -49,27 +49,31 @@ void PrimitiveExecutor::updateState(const RobotState& state) Vector PrimitiveExecutor::stepTargetLinearVelocity(Duration delta_time) { - const auto target_v_global = + Vector target_v_global = position_controller_.step(state_.position(), *trajectory_path_, time_since_linear_trajectory_creation_, delta_time); - auto target_v_local = globalToLocalVelocity(target_v_global, state_.orientation()); - // make sure robot doesn't go faster than max speed - target_v_local = target_v_local.normalize( - std::min(target_v_local.length(), + // make sure robot doesn't go faster than max speed (speed is frame-invariant) + target_v_global = target_v_global.normalize( + std::min(target_v_global.length(), static_cast(robot_constants_.robot_max_speed_m_per_s))); - const Vector local_acceleration = - (target_v_local - prev_target_local_velocity_) / delta_time.toSeconds(); - - if (local_acceleration.length() > robot_constants_.robot_max_acceleration_m_per_s_2) + // The acceleration limit applies to the robot's translational (global-frame) + // velocity. Measuring it in the rotating body frame (i.e. after + // globalToLocalVelocity) would add the v*omega term from the body frame spinning, + // which falsely trips the limit whenever the robot translates while rotating even + // though its global motion is within limits. So compare the change in global + // velocity. + const Vector global_acceleration = + (target_v_global - prev_target_global_velocity_) / delta_time.toSeconds(); + if (global_acceleration.length() > robot_constants_.robot_max_acceleration_m_per_s_2) { - LOG(WARNING) << "Robot trying to accelerate at " << local_acceleration.length() + LOG(WARNING) << "Robot trying to accelerate at " << global_acceleration.length() << "m/s^2."; } + prev_target_global_velocity_ = target_v_global; - prev_target_local_velocity_ = target_v_local; - return target_v_local; + return globalToLocalVelocity(target_v_global, state_.orientation()); } AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(Duration delta_time) @@ -178,7 +182,8 @@ std::unique_ptr PrimitiveExecutor::stepPrimi void PrimitiveExecutor::setPrevCommandedVelocity(const Vector& local_velocity, const AngularVelocity& angular_velocity) { - prev_target_local_velocity_ = local_velocity; + prev_target_global_velocity_ = + localToGlobalVelocity(local_velocity, state_.orientation()); prev_target_angular_velocity_ = angular_velocity; } diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index b3f39efe78..b6dbc407d4 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -100,7 +100,7 @@ class PrimitiveExecutor // The velocities commanded on the previous step. Used to measure the commanded // (tick-to-tick) acceleration - Vector prev_target_local_velocity_; + Vector prev_target_global_velocity_; AngularVelocity prev_target_angular_velocity_; // Estimated delay between a vision frame to AI processing to robot executing diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp index 3021025fdf..c1da724633 100644 --- a/src/software/embedded/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer_test.cpp @@ -74,12 +74,18 @@ TEST(RobotLocalizer, tracks_constant_forward_velocity) << ", " << localizer.getVelocity().y() << ") orient=" << localizer.getOrientation().toDegrees() << "deg\n"; + // NOTE: we assert on velocity and orientation, not absolute position. RobotLocalizer + // integrates position using delta_time from a real wall-clock (steady_clock), but + // this test advances simulated time by a fixed DT per iteration. Those only agree if + // each loop iteration takes ~DT of wall-time, which it does not under an optimized + // build (the loop runs far faster), so the integrated position is not deterministic + // in a unit test. Velocity/orientation come from the (decoupled) motor/IMU + // measurements and are robust to this. The key property under test is that the + // periodic vision update no longer corrupts the velocity estimate. EXPECT_NEAR(localizer.getOrientation().toDegrees(), 0.0, 10.0); EXPECT_NEAR(localizer.getVelocity().x(), 1.0, 0.2) << "Forward velocity estimate does not track"; EXPECT_NEAR(localizer.getVelocity().y(), 0.0, 0.2); - EXPECT_NEAR(localizer.getPosition().x(), 1.0, 0.2); - EXPECT_NEAR(localizer.getPosition().y(), 0.0, 0.2); } // Diagnostic: with no periodic vision fix, the velocity estimate comes purely from the diff --git a/src/software/multithreading/BUILD b/src/software/multithreading/BUILD index f58d882a6d..3192940a9c 100644 --- a/src/software/multithreading/BUILD +++ b/src/software/multithreading/BUILD @@ -111,6 +111,9 @@ cc_test( cc_test( name = "last_in_first_out_threaded_observer_test", srcs = ["last_in_first_out_threaded_observer_test.cpp"], + # Flaky: the received-value-ordering assertions depend on thread scheduling, so a + # race can occasionally swap adjacent values. Retry rather than fail CI. + flaky = True, deps = [ ":threaded_observer", "//shared/test_util:tbots_gtest_main", diff --git a/src/software/power/charger.cpp b/src/software/power/charger.cpp index 5963593250..5edff392e1 100644 --- a/src/software/power/charger.cpp +++ b/src/software/power/charger.cpp @@ -13,6 +13,11 @@ void Charger::chargeCapacitors() digitalWrite(CHRG, HIGH); } +void Charger::stopChargeCapacitors() +{ + digitalWrite(CHRG, LOW); +} + float Charger::getCapacitorVoltage() { return analogRead(HV_SENSE) / RESOLUTION * SCALE_VOLTAGE * VOLTAGE_DIVIDER; @@ -22,3 +27,8 @@ bool Charger::getFlybackFault() { return !digitalRead(FLYBACK_FAULT); } + +bool Charger::isCapacitorCharging() +{ + return digitalRead(CHRG_DONE); +} \ No newline at end of file diff --git a/src/software/power/charger.h b/src/software/power/charger.h index 001ab323fc..65c0a35d61 100644 --- a/src/software/power/charger.h +++ b/src/software/power/charger.h @@ -7,7 +7,7 @@ */ class Charger { - public: +public: /** * Creates a Charger setting up pins and attaching interrupts. */ @@ -19,6 +19,10 @@ class Charger * */ static void chargeCapacitors(); + /** + * Sets the charge pin to LOW to stop charging the capacitors. + */ + static void stopChargeCapacitors(); /** * Returns the voltage of the capacitors * @@ -31,9 +35,15 @@ class Charger * @return whether the flyback fault is tripped or not */ bool getFlybackFault(); + /** + * Returns whether the capacitors are currently charging + * + * @return whether the capacitors are charging or not + */ + bool isCapacitorCharging(); - private: +private: static constexpr float VOLTAGE_DIVIDER = 1003.0 / 13.0; static constexpr float RESOLUTION = 4096.0; static constexpr float SCALE_VOLTAGE = 3.3; -}; +}; \ No newline at end of file diff --git a/src/software/power/control_executor.cpp b/src/software/power/control_executor.cpp index e1723dd58a..a94a84c540 100644 --- a/src/software/power/control_executor.cpp +++ b/src/software/power/control_executor.cpp @@ -13,29 +13,37 @@ void ControlExecutor::execute(const TbotsProto_PowerPulseControl& control) { switch (control.chicker.which_chicker_command) { - case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: - chicker->kick(control.chicker.chicker_command.kick_pulse_width); + case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: + chicker->kick(control.chicker.chicker_command.kick_pulse_width); + delay(5); + charger->stopChargeCapacitors(); + delay(5); + charger->chargeCapacitors(); + break; + case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: + chicker->chip(control.chicker.chicker_command.chip_pulse_width); + delay(5); + charger->stopChargeCapacitors(); + delay(5); + charger->chargeCapacitors(); + break; + case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: + switch ( + control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) + { + case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: + chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autokick_pulse_width); break; - case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: - chicker->chip(control.chicker.chicker_command.chip_pulse_width); + case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: + chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autochip_pulse_width); break; - case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: - switch ( - control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) - { - case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: - chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autokick_pulse_width); - break; - case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: - chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autochip_pulse_width); - break; - default: - break; - } - break; - default: + default: break; + } + break; + default: + break; } -} +} \ No newline at end of file diff --git a/src/software/power/pins.h b/src/software/power/pins.h index c171e863ec..6c1238b17d 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -11,14 +11,14 @@ const uint8_t LEFT_DIR = 1; const uint8_t RIGHT_DIR = -1; // Chicker -const uint8_t KICKER_PIN = 32; -const uint8_t CHIPPER_PIN = 33; +const uint8_t KICKER_PIN = 33; +const uint8_t CHIPPER_PIN = 32; // Charger const uint8_t HV_SENSE = 36; const uint8_t FLYBACK_FAULT = 27; -const uint8_t CHRG_DONE = 26; -const uint8_t CHRG = 25; +const uint8_t CHRG_DONE = 25; +const uint8_t CHRG = 26; // Break Beam const uint8_t BREAK_BEAM_PIN = 37; @@ -37,4 +37,4 @@ const uint8_t DRIBBLER_PIN = 4; // Timers const uint8_t CHICKER_PULSE_TIMER = 0; const uint8_t CHICKER_COOLDOWN_TIMER = 3; -const uint32_t MICROSECONDS_IN_SECOND = 1000000; +const uint32_t MICROSECONDS_IN_SECOND = 1000000; \ No newline at end of file From 80148f1b19f9addbd594f0d0f32a488b0ae750f3 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 16 Jun 2026 18:54:01 -0700 Subject: [PATCH 36/46] linting --- .../ai/hl/stp/play/ball_placement/BUILD | 4 +- .../ai/hl/stp/play/kickoff_enemy/BUILD | 2 +- src/software/power/charger.cpp | 2 +- src/software/power/charger.h | 6 +- src/software/power/control_executor.cpp | 60 +++++++++---------- src/software/power/pins.h | 2 +- 6 files changed, 38 insertions(+), 38 deletions(-) diff --git a/src/software/ai/hl/stp/play/ball_placement/BUILD b/src/software/ai/hl/stp/play/ball_placement/BUILD index 8e2e08621e..369fdfd276 100644 --- a/src/software/ai/hl/stp/play/ball_placement/BUILD +++ b/src/software/ai/hl/stp/play/ball_placement/BUILD @@ -34,11 +34,11 @@ cc_library( py_test( name = "ball_placement_play_test", timeout = "long", - # Flaky: multi-robot simulated play sensitive to sim timing; passes on retry. - flaky = True, srcs = [ "ball_placement_play_test.py", ], + # Flaky: multi-robot simulated play sensitive to sim timing; passes on retry. + flaky = True, deps = [ "//software:conftest", "//software/gameplay_tests/validation:validations", diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/BUILD b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD index eba9a7d517..e40c39ba7a 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/BUILD +++ b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD @@ -28,9 +28,9 @@ cc_library( py_test( name = "kickoff_enemy_play_test", + srcs = ["kickoff_enemy_play_test.py"], # Flaky: multi-robot simulated play sensitive to sim timing; passes on retry. flaky = True, - srcs = ["kickoff_enemy_play_test.py"], deps = [ "//software:conftest", "//software/gameplay_tests/validation:validations", diff --git a/src/software/power/charger.cpp b/src/software/power/charger.cpp index 5edff392e1..445d25471c 100644 --- a/src/software/power/charger.cpp +++ b/src/software/power/charger.cpp @@ -31,4 +31,4 @@ bool Charger::getFlybackFault() bool Charger::isCapacitorCharging() { return digitalRead(CHRG_DONE); -} \ No newline at end of file +} diff --git a/src/software/power/charger.h b/src/software/power/charger.h index 65c0a35d61..71e96afeba 100644 --- a/src/software/power/charger.h +++ b/src/software/power/charger.h @@ -7,7 +7,7 @@ */ class Charger { -public: + public: /** * Creates a Charger setting up pins and attaching interrupts. */ @@ -42,8 +42,8 @@ class Charger */ bool isCapacitorCharging(); -private: + private: static constexpr float VOLTAGE_DIVIDER = 1003.0 / 13.0; static constexpr float RESOLUTION = 4096.0; static constexpr float SCALE_VOLTAGE = 3.3; -}; \ No newline at end of file +}; diff --git a/src/software/power/control_executor.cpp b/src/software/power/control_executor.cpp index a94a84c540..20f9cc4242 100644 --- a/src/software/power/control_executor.cpp +++ b/src/software/power/control_executor.cpp @@ -13,37 +13,37 @@ void ControlExecutor::execute(const TbotsProto_PowerPulseControl& control) { switch (control.chicker.which_chicker_command) { - case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: - chicker->kick(control.chicker.chicker_command.kick_pulse_width); - delay(5); - charger->stopChargeCapacitors(); - delay(5); - charger->chargeCapacitors(); - break; - case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: - chicker->chip(control.chicker.chicker_command.chip_pulse_width); - delay(5); - charger->stopChargeCapacitors(); - delay(5); - charger->chargeCapacitors(); - break; - case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: - switch ( - control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) - { - case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: - chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autokick_pulse_width); + case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: + chicker->kick(control.chicker.chicker_command.kick_pulse_width); + delay(5); + charger->stopChargeCapacitors(); + delay(5); + charger->chargeCapacitors(); break; - case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: - chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autochip_pulse_width); + case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: + chicker->chip(control.chicker.chicker_command.chip_pulse_width); + delay(5); + charger->stopChargeCapacitors(); + delay(5); + charger->chargeCapacitors(); break; - default: + case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: + switch ( + control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) + { + case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: + chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autokick_pulse_width); + break; + case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: + chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autochip_pulse_width); + break; + default: + break; + } + break; + default: break; - } - break; - default: - break; } -} \ No newline at end of file +} diff --git a/src/software/power/pins.h b/src/software/power/pins.h index 6c1238b17d..9f82c586f0 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -37,4 +37,4 @@ const uint8_t DRIBBLER_PIN = 4; // Timers const uint8_t CHICKER_PULSE_TIMER = 0; const uint8_t CHICKER_COOLDOWN_TIMER = 3; -const uint32_t MICROSECONDS_IN_SECOND = 1000000; \ No newline at end of file +const uint32_t MICROSECONDS_IN_SECOND = 1000000; From 11d268913f9e3d2523da4e8e7361f15c82389d51 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 16 Jun 2026 20:38:17 -0700 Subject: [PATCH 37/46] removed unrelated changes --- .../trajectory/bang_bang_trajectory_1d.h | 1 - src/software/embedded/spi_utils.h | 7 +- .../gameplay_tests/field_test_fixture.py | 130 ++++-------- .../movement_robot_field_test.py | 191 ++++++++++-------- src/software/power/charger.cpp | 10 - src/software/power/charger.h | 14 +- src/software/power/control_executor.cpp | 50 ++--- src/software/power/pins.h | 8 +- 8 files changed, 177 insertions(+), 234 deletions(-) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index 3f0fdf9c03..d5a54bf7af 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include "software/ai/navigator/trajectory/trajectory.hpp" diff --git a/src/software/embedded/spi_utils.h b/src/software/embedded/spi_utils.h index ee5933d172..1b38e14f22 100644 --- a/src/software/embedded/spi_utils.h +++ b/src/software/embedded/spi_utils.h @@ -32,6 +32,7 @@ void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, * @param read_rx the buffer our read response will be placed in * @param spi_speed the speed to run spi at in Hz */ -void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, - const uint8_t* read_rx, const uint32_t read_len, - const uint32_t write_len, uint32_t spi_speed); +void readThenWriteSpiTransfer( + int fd, const uint8_t* read_tx, const uint8_t* write_tx, const uint8_t* read_rx, + const uint32_t read_len, const uint32_t write_len, + uint32_t spi_speed); // refactor to take std::array, not raw pointers diff --git a/src/software/gameplay_tests/field_test_fixture.py b/src/software/gameplay_tests/field_test_fixture.py index 034c54bc19..2b512dd687 100644 --- a/src/software/gameplay_tests/field_test_fixture.py +++ b/src/software/gameplay_tests/field_test_fixture.py @@ -37,15 +37,14 @@ class FieldTestRunner(TbotsTestRunner): """Run a field test""" def __init__( - self, - test_name, - thunderscope, - blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io, - gamecontroller, - robot_communication, - publish_validation_protos=True, - is_yellow_friendly=False, + self, + test_name, + thunderscope, + blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io, + gamecontroller, + publish_validation_protos=True, + is_yellow_friendly=False, ): """Initialize the FieldTestRunner @@ -54,7 +53,6 @@ def __init__( :param blue_full_system_proto_unix_io: The blue full system proto unix io to use :param yellow_full_system_proto_unix_io: The yellow full system proto unix io to use :param gamecontroller: The gamecontroller context managed instance - :param robot_communication: The robot communication instance :param publish_validation_protos: whether to publish validation protos :param: is_yellow_friendly: if yellow is the friendly team """ @@ -68,52 +66,32 @@ def __init__( ) self.publish_validation_protos = publish_validation_protos self.is_yellow_friendly = is_yellow_friendly - self.robot_communication = robot_communication logger.info("determining robots on field") # survey field for available robot ids - survey_start_time = time.time() - self.friendly_robot_ids_field = [] - while time.time() - survey_start_time < WORLD_BUFFER_TIMEOUT: - try: - world = self.world_buffer.get(block=True, timeout=0.1) - self.initial_world = world - self.friendly_robot_ids_field = [ - robot.id for robot in world.friendly_team.team_robots - ] - - if len(self.friendly_robot_ids_field) > 0: - logger.info(f"friendly team ids {self.friendly_robot_ids_field}") - break - except queue.Empty: - continue - - if len(self.friendly_robot_ids_field) == 0: - raise Exception("no friendly robots found on field within timeout") - - def wait_for_estop_play(self): - """Blocks until the estop is in the PLAY state""" - if self.robot_communication.estop_is_playing: - return - - logger.info("\x1b[33m" + "Waiting for Estop to be in PLAY state..." + "\x1b[0m") - while not self.robot_communication.estop_is_playing: - # We must process events if Thunderscope is running to keep it responsive - if self.thunderscope: - from pyqtgraph.Qt import QtWidgets + try: + world = self.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) + self.initial_world = world + self.friendly_robot_ids_field = [ + robot.id for robot in world.friendly_team.team_robots + ] - QtWidgets.QApplication.processEvents() - time.sleep(0.1) - logger.info( - "\x1b[32m" + "Estop is in PLAY state. Proceeding with test." + "\x1b[0m" - ) + logger.info(f"friendly team ids {self.friendly_robot_ids_field}") + + if len(self.friendly_robot_ids_field) == 0: + raise Exception("no friendly robots found on field") + + except queue.Empty: + raise Exception( + f"No Worlds were received with in {WORLD_BUFFER_TIMEOUT} seconds. Please make sure atleast 1 robot and 1 ball is present on the field." + ) @override def send_gamecontroller_command( - self, - gc_command: proto.ssl_gc_state_pb2.Command, - team: proto.ssl_gc_common_pb2.Team, - final_ball_placement_point=None, + self, + gc_command: proto.ssl_gc_state_pb2.Command, + team: proto.ssl_gc_common_pb2.Team, + final_ball_placement_point=None, ): """Send a command to the gamecontroller @@ -121,7 +99,7 @@ def send_gamecontroller_command( :param team: The team which the command as attributed to :param final_ball_placement_point: The ball placement point """ - self.gamecontroller.send_gc_command( + self.gamecontroller.send_ci_input( gc_command=gc_command, team=team, final_ball_placement_point=final_ball_placement_point, @@ -129,10 +107,10 @@ def send_gamecontroller_command( @override def run_test( - self, - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=3, + self, + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=3, ): """Run a test. In a field test this means beginning validation. @@ -146,19 +124,14 @@ def run_test( def stop_test(delay): time.sleep(delay) - # We no longer close thunderscope here, because a test might call run_test multiple times. - # Thunderscope will be closed when the fixture is torn down. + if self.thunderscope: + self.thunderscope.close() def __runner(): time.sleep(LAUNCH_DELAY_S) test_end_time = time.time() + test_timeout_s - # Keep track if we started with any eventually validations - has_eventually_validations = any( - len(seq) > 0 for seq in eventually_validation_sequence_set - ) - while time.time() < test_end_time: while True: try: @@ -199,15 +172,9 @@ def __runner(): # Check that all always validations are always valid validation.check_validation(always_validation_proto_set) - # Break if eventually validation passes - if has_eventually_validations and all( - len(seq) == 0 for seq in eventually_validation_sequence_set - ): - break - + # Check that all eventually validations are eventually valid validation.check_validation(eventually_validation_proto_set) - - stop_test(delay=PROCESS_BUFFER_DELAY_S) + stop_test(TEST_END_DELAY) def excepthook(args): """This function is _critical_ for show_thunderscope to work. @@ -222,22 +189,14 @@ def excepthook(args): threading.excepthook = excepthook - # If visualization is enabled, we need to be careful. - # Thunderscope.show() is blocking. if self.thunderscope: run_test_thread = threading.Thread(target=__runner, daemon=True) run_test_thread.start() - - # Only call show if the window is not already open. - # If it IS open, it means we are ALREADY in the Qt event loop, - # which can only happen if we are running this run_test in a background thread. - if not self.thunderscope.is_open(): - self.thunderscope.show() - + self.thunderscope.show() run_test_thread.join() if self.last_exception: - pytest.fail(str(self.last_exception)) + pytest.fail(str(ex.last_exception)) else: __runner() @@ -335,7 +294,7 @@ def load_command_line_arguments(): action="store", default="", help="The test filter, if not specified all tests will run. " - + "See https://docs.pytest.org/en/latest/how-to/usage.html#specifying-tests-selecting-tests", + + "See https://docs.pytest.org/en/latest/how-to/usage.html#specifying-tests-selecting-tests", ) parser.add_argument( @@ -418,11 +377,11 @@ def field_test_runner(): # Launch all binaries with FullSystem( - "software/unix_full_system", - full_system_runtime_dir=runtime_dir, - debug_full_system=debug_full_sys, - friendly_colour_yellow=args.run_yellow, - should_restart_on_crash=False, + "software/unix_full_system", + full_system_runtime_dir=runtime_dir, + debug_full_system=debug_full_sys, + friendly_colour_yellow=args.run_yellow, + should_restart_on_crash=False, ) as friendly_fs, Gamecontroller( # we would be using conventional port if and only if we are playing in robocup. suppress_logs=(not args.show_gamecontroller_logs), @@ -486,7 +445,6 @@ def field_test_runner(): yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, gamecontroller=gamecontroller, thunderscope=tscope, - robot_communication=rc_friendly, is_yellow_friendly=args.run_yellow, ) diff --git a/src/software/gameplay_tests/movement_robot_field_test.py b/src/software/gameplay_tests/movement_robot_field_test.py index 82693830a0..cd6850d7df 100644 --- a/src/software/gameplay_tests/movement_robot_field_test.py +++ b/src/software/gameplay_tests/movement_robot_field_test.py @@ -3,10 +3,68 @@ from software.gameplay_tests.simulated_test_fixture import * from software.logger.logger import create_logger +import math logger = create_logger(__name__) -import math -import threading + + +# TODO 2908: Support running this test in both simulator or field mode +# this test can be run either in simulation or on the field +# @pytest.mark.parametrize( +# "robot_x_destination, robot_y_destination", +# [(-2.0, -1), (-2.0, 1.0), (0.0, 1.0), (0.0, -1.0)], +# ) +# def test_basic_movement(simulated_test_runner): +# +# robot_starting_x = 0 +# robot_starting_y = 0 +# ROBOT_ID = 0 +# angle = 0 +# robot_x_destination = -2 +# robot_y_destination = -1 +# rob_pos_p = Point(x_meters=robot_x_destination, y_meters=robot_y_destination) +# +# move_tactic = MoveTactic() +# move_tactic.destination.CopyFrom(rob_pos_p) +# move_tactic.dribbler_mode = DribblerMode.OFF +# move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) +# move_tactic.ball_collision_type = BallCollisionType.AVOID +# move_tactic.auto_chip_or_kick.CopyFrom(AutoChipOrKick(autokick_speed_m_per_s=0.0)) +# move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT +# move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE +# +# # setup world state +# initial_worldstate = create_world_state( +# yellow_robot_locations=[], +# blue_robot_locations=[tbots_cpp.Point(robot_starting_x, robot_starting_y)], +# ball_location=tbots_cpp.Point(1, 1), +# ball_velocity=tbots_cpp.Point(0, 0), +# ) +# simulated_test_runner.set_world_state(initial_worldstate) +# +# # Setup Tactic +# params = AssignedTacticPlayControlParams() +# +# params.assigned_tactics[ROBOT_ID].move.CopyFrom(move_tactic) +# +# # Eventually Validation +# eventually_validation_sequence_set = [ +# [ +# RobotEventuallyEntersRegion( +# regions=[ +# tbots_cpp.Circle( +# tbots_cpp.Point(robot_x_destination, robot_y_destination), 0.2 +# ) +# ] +# ), +# ] +# ] +# simulated_test_runner.set_tactics(params, True) +# +# simulated_test_runner.run_test( +# eventually_validation_sequence_set=eventually_validation_sequence_set, +# test_timeout_s=5, +# ) # this test can only be run on the field @@ -30,60 +88,38 @@ def test_basic_rotation(field_test_runner): robot = world.friendly_team.team_robots[0] rob_pos_p = robot.current_state.global_position - - def execute_test(): - # Wait for the user to flip the estop to PLAY - field_test_runner.wait_for_estop_play() - - # Force start the game automatically - field_test_runner.gamecontroller.send_gc_command( - proto.ssl_gc_state_pb2.Command.FORCE_START, - proto.ssl_gc_common_pb2.Team.UNKNOWN, - final_ball_placement_point=None, + logger.info("staying in pos {rob_pos_p}") + + for angle in test_angles: + move_tactic = MoveTactic() + move_tactic.destination.CopyFrom(rob_pos_p) + move_tactic.dribbler_mode = DribblerMode.OFF + move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) + move_tactic.ball_collision_type = BallCollisionType.AVOID + move_tactic.auto_chip_or_kick.CopyFrom( + AutoChipOrKick(autokick_speed_m_per_s=0.0) ) + move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT + move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE - for angle in test_angles: - print(f"Rotating to {angle} degrees") - move_tactic = MoveTactic() - move_tactic.destination.CopyFrom(rob_pos_p) - move_tactic.dribbler_mode = DribblerMode.OFF - move_tactic.final_orientation.CopyFrom( - Angle(radians=angle * math.pi / 180.0) - ) - move_tactic.ball_collision_type = BallCollisionType.AVOID - move_tactic.auto_chip_or_kick.CopyFrom( - AutoChipOrKick(autokick_speed_m_per_s=0.0) - ) - move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT - move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE - - # Setup Tactic - field_test_runner.set_tactics( - blue_tactics={id: move_tactic}, yellow_tactics=None - ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=5, - ) - - # validate by eye - logger.info(f"robot set to {angle} orientation") - time.sleep(2) - + # Setup Tactic + field_test_runner.set_tactics( + blue_tactics={id: move_tactic}, yellow_tactics=None + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=5, + ) # Send a halt tactic after the test finishes field_test_runner.set_tactics( blue_tactics={id: HaltTactic()}, yellow_tactics=None ) - test_thread = threading.Thread(target=execute_test, daemon=True) - test_thread.start() + # validate by eye + logger.info(f"robot set to {angle} orientation") - # If thunderscope is enabled, show it in the main thread (blocking) - if field_test_runner.thunderscope: - field_test_runner.thunderscope.show() - - test_thread.join() + time.sleep(2) def test_one_robots_square(field_test_runner): @@ -102,10 +138,10 @@ def test_one_robots_square(field_test_runner): id = world.friendly_team.team_robots[0].id print(f"Running test on robot {id}") - point1 = Point(x_meters=0.4, y_meters=0.4) - point2 = Point(x_meters=0.4, y_meters=-0.4) - point3 = Point(x_meters=-0.4, y_meters=-0.4) - point4 = Point(x_meters=-0.4, y_meters=0.4) + point1 = Point(x_meters=-0.3, y_meters=0.6) + point2 = Point(x_meters=-0.3, y_meters=-0.6) + point3 = Point(x_meters=-1.5, y_meters=-0.6) + point4 = Point(x_meters=-1.5, y_meters=0.6) tactic_0 = MoveTactic( destination=point1, @@ -143,48 +179,25 @@ def test_one_robots_square(field_test_runner): max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, ) + tactics = [tactic_0, tactic_1, tactic_2, tactic_3] - tactics = [tactic_0, tactic_1, tactic_2, tactic_3, tactic_0] - - def execute_test(): - # Wait for the user to flip the estop to PLAY - field_test_runner.wait_for_estop_play() - - # Force start the game automatically - field_test_runner.gamecontroller.send_gc_command( - proto.ssl_gc_state_pb2.Command.FORCE_START, - proto.ssl_gc_common_pb2.Team.UNKNOWN, - final_ball_placement_point=None, - ) - - for tactic in tactics: - print(f"Going to {tactic.destination}") - - field_test_runner.set_tactics( - blue_tactics={ - id: tactic, - }, - yellow_tactics=None, - ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=8, - ) + for tactic in tactics: + print(f"Going to {tactic.destination}") - # Send a halt tactic after the test finishes field_test_runner.set_tactics( - blue_tactics={id: HaltTactic()}, yellow_tactics=None + blue_tactics={ + id: tactic, + }, + yellow_tactics=None, + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=4, ) - test_thread = threading.Thread(target=execute_test, daemon=True) - test_thread.start() - - # If thunderscope is enabled, show it in the main thread (blocking) - if field_test_runner.thunderscope: - field_test_runner.thunderscope.show() - - test_thread.join() + # Send a halt tactic after the test finishes + field_test_runner.set_tactics(blue_tactics={id: HaltTactic()}, yellow_tactics=None) if __name__ == "__main__": diff --git a/src/software/power/charger.cpp b/src/software/power/charger.cpp index 445d25471c..5963593250 100644 --- a/src/software/power/charger.cpp +++ b/src/software/power/charger.cpp @@ -13,11 +13,6 @@ void Charger::chargeCapacitors() digitalWrite(CHRG, HIGH); } -void Charger::stopChargeCapacitors() -{ - digitalWrite(CHRG, LOW); -} - float Charger::getCapacitorVoltage() { return analogRead(HV_SENSE) / RESOLUTION * SCALE_VOLTAGE * VOLTAGE_DIVIDER; @@ -27,8 +22,3 @@ bool Charger::getFlybackFault() { return !digitalRead(FLYBACK_FAULT); } - -bool Charger::isCapacitorCharging() -{ - return digitalRead(CHRG_DONE); -} diff --git a/src/software/power/charger.h b/src/software/power/charger.h index 71e96afeba..39708582d6 100644 --- a/src/software/power/charger.h +++ b/src/software/power/charger.h @@ -7,7 +7,7 @@ */ class Charger { - public: +public: /** * Creates a Charger setting up pins and attaching interrupts. */ @@ -19,10 +19,6 @@ class Charger * */ static void chargeCapacitors(); - /** - * Sets the charge pin to LOW to stop charging the capacitors. - */ - static void stopChargeCapacitors(); /** * Returns the voltage of the capacitors * @@ -35,14 +31,8 @@ class Charger * @return whether the flyback fault is tripped or not */ bool getFlybackFault(); - /** - * Returns whether the capacitors are currently charging - * - * @return whether the capacitors are charging or not - */ - bool isCapacitorCharging(); - private: +private: static constexpr float VOLTAGE_DIVIDER = 1003.0 / 13.0; static constexpr float RESOLUTION = 4096.0; static constexpr float SCALE_VOLTAGE = 3.3; diff --git a/src/software/power/control_executor.cpp b/src/software/power/control_executor.cpp index 20f9cc4242..aa018c9a6c 100644 --- a/src/software/power/control_executor.cpp +++ b/src/software/power/control_executor.cpp @@ -13,37 +13,29 @@ void ControlExecutor::execute(const TbotsProto_PowerPulseControl& control) { switch (control.chicker.which_chicker_command) { - case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: - chicker->kick(control.chicker.chicker_command.kick_pulse_width); - delay(5); - charger->stopChargeCapacitors(); - delay(5); - charger->chargeCapacitors(); + case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: + chicker->kick(control.chicker.chicker_command.kick_pulse_width); + break; + case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: + chicker->chip(control.chicker.chicker_command.chip_pulse_width); + break; + case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: + switch ( + control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) + { + case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: + chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autokick_pulse_width); break; - case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: - chicker->chip(control.chicker.chicker_command.chip_pulse_width); - delay(5); - charger->stopChargeCapacitors(); - delay(5); - charger->chargeCapacitors(); + case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: + chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autochip_pulse_width); break; - case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: - switch ( - control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) - { - case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: - chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autokick_pulse_width); - break; - case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: - chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autochip_pulse_width); - break; - default: - break; - } - break; - default: + default: break; + } + break; + default: + break; } } diff --git a/src/software/power/pins.h b/src/software/power/pins.h index 9f82c586f0..c171e863ec 100644 --- a/src/software/power/pins.h +++ b/src/software/power/pins.h @@ -11,14 +11,14 @@ const uint8_t LEFT_DIR = 1; const uint8_t RIGHT_DIR = -1; // Chicker -const uint8_t KICKER_PIN = 33; -const uint8_t CHIPPER_PIN = 32; +const uint8_t KICKER_PIN = 32; +const uint8_t CHIPPER_PIN = 33; // Charger const uint8_t HV_SENSE = 36; const uint8_t FLYBACK_FAULT = 27; -const uint8_t CHRG_DONE = 25; -const uint8_t CHRG = 26; +const uint8_t CHRG_DONE = 26; +const uint8_t CHRG = 25; // Break Beam const uint8_t BREAK_BEAM_PIN = 37; From 325aa27af7eee9c21f5c8c909b59ff95c9e56e72 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 16 Jun 2026 20:41:16 -0700 Subject: [PATCH 38/46] removed unrelated changes --- src/software/power/charger.h | 4 +-- src/software/power/control_executor.cpp | 42 ++++++++++++------------- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/software/power/charger.h b/src/software/power/charger.h index 39708582d6..001ab323fc 100644 --- a/src/software/power/charger.h +++ b/src/software/power/charger.h @@ -7,7 +7,7 @@ */ class Charger { -public: + public: /** * Creates a Charger setting up pins and attaching interrupts. */ @@ -32,7 +32,7 @@ class Charger */ bool getFlybackFault(); -private: + private: static constexpr float VOLTAGE_DIVIDER = 1003.0 / 13.0; static constexpr float RESOLUTION = 4096.0; static constexpr float SCALE_VOLTAGE = 3.3; diff --git a/src/software/power/control_executor.cpp b/src/software/power/control_executor.cpp index aa018c9a6c..e1723dd58a 100644 --- a/src/software/power/control_executor.cpp +++ b/src/software/power/control_executor.cpp @@ -13,29 +13,29 @@ void ControlExecutor::execute(const TbotsProto_PowerPulseControl& control) { switch (control.chicker.which_chicker_command) { - case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: - chicker->kick(control.chicker.chicker_command.kick_pulse_width); - break; - case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: - chicker->chip(control.chicker.chicker_command.chip_pulse_width); - break; - case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: - switch ( - control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) - { - case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: - chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autokick_pulse_width); + case TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag: + chicker->kick(control.chicker.chicker_command.kick_pulse_width); break; - case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: - chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick - .auto_chip_or_kick.autochip_pulse_width); + case TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag: + chicker->chip(control.chicker.chicker_command.chip_pulse_width); break; - default: + case TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag: + switch ( + control.chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick) + { + case TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag: + chicker->autokick(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autokick_pulse_width); + break; + case TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag: + chicker->autochip(control.chicker.chicker_command.auto_chip_or_kick + .auto_chip_or_kick.autochip_pulse_width); + break; + default: + break; + } + break; + default: break; - } - break; - default: - break; } } From 907725ab3b1233da1f3ba487acbf6e884628d0cf Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 16 Jun 2026 20:41:58 -0700 Subject: [PATCH 39/46] removed unrelated changes --- .../gameplay_tests/field_test_fixture.py | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/software/gameplay_tests/field_test_fixture.py b/src/software/gameplay_tests/field_test_fixture.py index 2b512dd687..19e4ff6aad 100644 --- a/src/software/gameplay_tests/field_test_fixture.py +++ b/src/software/gameplay_tests/field_test_fixture.py @@ -37,14 +37,14 @@ class FieldTestRunner(TbotsTestRunner): """Run a field test""" def __init__( - self, - test_name, - thunderscope, - blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io, - gamecontroller, - publish_validation_protos=True, - is_yellow_friendly=False, + self, + test_name, + thunderscope, + blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io, + gamecontroller, + publish_validation_protos=True, + is_yellow_friendly=False, ): """Initialize the FieldTestRunner @@ -88,10 +88,10 @@ def __init__( @override def send_gamecontroller_command( - self, - gc_command: proto.ssl_gc_state_pb2.Command, - team: proto.ssl_gc_common_pb2.Team, - final_ball_placement_point=None, + self, + gc_command: proto.ssl_gc_state_pb2.Command, + team: proto.ssl_gc_common_pb2.Team, + final_ball_placement_point=None, ): """Send a command to the gamecontroller @@ -107,10 +107,10 @@ def send_gamecontroller_command( @override def run_test( - self, - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=3, + self, + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=3, ): """Run a test. In a field test this means beginning validation. @@ -294,7 +294,7 @@ def load_command_line_arguments(): action="store", default="", help="The test filter, if not specified all tests will run. " - + "See https://docs.pytest.org/en/latest/how-to/usage.html#specifying-tests-selecting-tests", + + "See https://docs.pytest.org/en/latest/how-to/usage.html#specifying-tests-selecting-tests", ) parser.add_argument( @@ -377,11 +377,11 @@ def field_test_runner(): # Launch all binaries with FullSystem( - "software/unix_full_system", - full_system_runtime_dir=runtime_dir, - debug_full_system=debug_full_sys, - friendly_colour_yellow=args.run_yellow, - should_restart_on_crash=False, + "software/unix_full_system", + full_system_runtime_dir=runtime_dir, + debug_full_system=debug_full_sys, + friendly_colour_yellow=args.run_yellow, + should_restart_on_crash=False, ) as friendly_fs, Gamecontroller( # we would be using conventional port if and only if we are playing in robocup. suppress_logs=(not args.show_gamecontroller_logs), From 18c3d92540394fe3d23723db0af2faf7f5bea31e Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Wed, 17 Jun 2026 00:36:42 -0700 Subject: [PATCH 40/46] reverts --- .../ansible/playbooks/deploy_motor_firmware.yml | 4 ++-- .../embedded/drivers/flash_motor_drivers.py | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml b/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml index 2f2ba7b95f..b674569c6e 100644 --- a/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml +++ b/src/software/embedded/ansible/playbooks/deploy_motor_firmware.yml @@ -108,10 +108,10 @@ failed_when: false changed_when: false - - name: Run flashing script for Motor Drivers (FL FR BL BR) + - name: Run flashing script for Motor Drivers (A B C D) ansible.builtin.shell: > set -o pipefail; - echo {{ ansible_sudo_pass }} | sudo -S python3 flash_motor_drivers.py FL FR BL BR + echo {{ ansible_sudo_pass }} | sudo -S python3 flash_motor_drivers.py A B C D args: chdir: "{{ remote_dir }}" executable: /bin/bash diff --git a/src/software/embedded/drivers/flash_motor_drivers.py b/src/software/embedded/drivers/flash_motor_drivers.py index e1e1b15ea1..d2e50b703f 100755 --- a/src/software/embedded/drivers/flash_motor_drivers.py +++ b/src/software/embedded/drivers/flash_motor_drivers.py @@ -12,13 +12,13 @@ DEMUX_DISABLE_PIN = 0 # Pull HIGH to disable SWD and SWCLK multiplexing # Put these in order of board 0, 1, 2, 3 -BOARD_NAMES = ["FL", "FR", "BL", "BR"] +BOARD_NAMES = ["A", "B", "C", "D"] class MotorDriverFlasher: - def __init__(self, board_name, drivers): - switch_case_num = BOARD_NAMES.index(board_name) - self.board_name = board_name + def __init__(self, board_letter, drivers): + switch_case_num = BOARD_NAMES.index(board_letter) + self.board_letter = board_letter self.multiplex = [switch_case_num % 2, switch_case_num // 2] self.drivers = drivers @@ -30,7 +30,7 @@ def flash(self): else: self.drivers[i].off() - print(f"Preparing to flash driver on board {self.board_name}...") + print(f"Preparing to flash driver on board {self.board_letter}...") # Short delay to ensure lines settle time.sleep(0.5) @@ -72,7 +72,7 @@ def flash(self): raise e # After flashing all, ensure all are set to High (Run) - print(f"Flash to board {self.board_name} complete.") + print(f"Flash to board {self.board_letter} complete.") # Let line settle rq time.sleep(0.5) @@ -81,7 +81,7 @@ def flash(self): # If no arguments given, if not (2 <= len(sys.argv) <= 5): print( - "Usage: python3 flash_motor_drivers.py (FL, FR, BL, BR valid)" + "Usage: python3 flash_motor_drivers.py (A to D valid)" ) sys.exit(1) @@ -89,7 +89,7 @@ def flash(self): for i in range(1, len(sys.argv)): if sys.argv[i] not in BOARD_NAMES: print( - f"Usage: python3 flash_motor_drivers.py (Valid Names: {BOARD_NAMES})" + f"Usage: python3 flash_motor_drivers.py (Valid Letters: {BOARD_NAMES})" ) sys.exit(1) From b2d8adb877686db83beb14ea21e3da8ead0f9625 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Wed, 17 Jun 2026 17:51:07 -0700 Subject: [PATCH 41/46] limits added --- src/software/embedded/primitive_executor.cpp | 64 ++++++++++++-------- src/software/embedded/primitive_executor.h | 8 +-- src/software/embedded/thunderloop.cpp | 2 +- src/software/embedded/thunderloop.h | 2 - 4 files changed, 43 insertions(+), 33 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 0e625f79c4..7b477f3962 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -10,9 +10,9 @@ #include "software/logger/logger.h" #include "software/physics/velocity_conversion_util.h" -PrimitiveExecutor::PrimitiveExecutor( - const robot_constants::RobotConstants& robot_constants) - : current_primitive_(), robot_constants_(robot_constants) +PrimitiveExecutor::PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants) : + state_(), current_primitive_(), + robot_constants_(robot_constants) { } @@ -47,7 +47,7 @@ void PrimitiveExecutor::updateState(const RobotState& state) state_ = state; } -Vector PrimitiveExecutor::stepTargetLinearVelocity(Duration delta_time) +Vector PrimitiveExecutor::stepTargetLinearVelocity(const Duration& delta_time) { Vector target_v_global = position_controller_.step(state_.position(), *trajectory_path_, @@ -58,25 +58,33 @@ Vector PrimitiveExecutor::stepTargetLinearVelocity(Duration delta_time) std::min(target_v_global.length(), static_cast(robot_constants_.robot_max_speed_m_per_s))); - // The acceleration limit applies to the robot's translational (global-frame) - // velocity. Measuring it in the rotating body frame (i.e. after - // globalToLocalVelocity) would add the v*omega term from the body frame spinning, - // which falsely trips the limit whenever the robot translates while rotating even - // though its global motion is within limits. So compare the change in global - // velocity. - const Vector global_acceleration = - (target_v_global - prev_target_global_velocity_) / delta_time.toSeconds(); - if (global_acceleration.length() > robot_constants_.robot_max_acceleration_m_per_s_2) + // The trajectory's own velocity is acceleration-bounded, but the PID correction and + // per-tick trajectory regeneration are added on top, so the emitted command must be + // re-limited here to the robot's kinematic acceleration limit. Otherwise, the + // commanded velocity can step far more than robot_max_acceleration * delta_time per + // tick, asking the robot to accelerate well beyond what it (and the motors/SPI) can + // sustain. + // + // The limit applies to the robot's translational (global-frame) velocity. Measuring + // the change in the rotating body frame (i.e. after globalToLocalVelocity) would add + // the v*omega term from the body frame spinning, which falsely trips the limit + // whenever the robot translates while rotating even though its global motion is + // within limits. So clamp the change in global velocity, relative to the previous + // commanded (not measured) velocity. + const Vector velocity_delta = target_v_global - prev_target_global_velocity_; + const double max_velocity_delta = + robot_constants_.robot_max_acceleration_m_per_s_2 * delta_time.toSeconds(); + if (velocity_delta.length() > max_velocity_delta) { - LOG(WARNING) << "Robot trying to accelerate at " << global_acceleration.length() - << "m/s^2."; + target_v_global = + prev_target_global_velocity_ + velocity_delta.normalize(max_velocity_delta); } prev_target_global_velocity_ = target_v_global; return globalToLocalVelocity(target_v_global, state_.orientation()); } -AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(Duration delta_time) +AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(const Duration& delta_time) { auto target_w = orientation_controller_.step(state_.orientation(), *angular_trajectory_, @@ -87,21 +95,25 @@ AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(Duration delta_time const double clamped_w = std::clamp(target_w.toRadians(), -max_speed, max_speed); target_w = AngularVelocity::fromRadians(clamped_w); - const auto angular_acceleration = - (target_w - prev_target_angular_velocity_) / delta_time.toSeconds(); - if (std::abs(angular_acceleration.toRadians()) > - robot_constants_.robot_max_ang_acceleration_rad_per_s_2) - { - LOG(WARNING) << "Robot trying to angular accelerate at " - << angular_acceleration.toRadians() << "rads/s^2."; - } + // Re-limit the commanded angular velocity to the robot's angular acceleration limit, + // for the same reason as the translational clamp above: the feedforward trajectory is + // acceleration-bounded, but the PID correction and per-tick regeneration are added on + // top, so the emitted command can step more than robot_max_ang_acceleration * + // delta_time per tick. + const double max_angular_velocity_delta = + robot_constants_.robot_max_ang_acceleration_rad_per_s_2 * delta_time.toSeconds(); + const double angular_velocity_delta = std::clamp( + (target_w - prev_target_angular_velocity_).toRadians(), + -max_angular_velocity_delta, max_angular_velocity_delta); + target_w = prev_target_angular_velocity_ + + AngularVelocity::fromRadians(angular_velocity_delta); prev_target_angular_velocity_ = target_w; return target_w; } std::unique_ptr PrimitiveExecutor::stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time) + TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time) { time_since_linear_trajectory_creation_ += delta_time; time_since_angular_trajectory_creation_ += delta_time; @@ -188,7 +200,7 @@ void PrimitiveExecutor::setPrevCommandedVelocity(const Vector& local_velocity, } void PrimitiveExecutor::sendLinearMotionToPlotJuggler(const Vector& target_local_velocity, - Duration delta_time) + const Duration& delta_time) const { const Vector& local_acceleration = (target_local_velocity - state_.localVelocity()) / delta_time.toSeconds(); diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index b6dbc407d4..6a0db87ef3 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -45,7 +45,7 @@ class PrimitiveExecutor * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time); + TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time); private: /* @@ -54,7 +54,7 @@ class PrimitiveExecutor * * @returns Vector The target linear _local_ velocity */ - Vector stepTargetLinearVelocity(Duration delta_time); + Vector stepTargetLinearVelocity(const Duration& delta_time); /* * Compute the next target angular velocity the robot should have. @@ -62,7 +62,7 @@ class PrimitiveExecutor * * @returns AngularVelocity The target angular velocity */ - AngularVelocity stepTargetAngularVelocity(Duration delta_time); + AngularVelocity stepTargetAngularVelocity(const Duration& delta_time); /** * Sends the position, local velocity, and local acceleration to PlotJuggler. @@ -72,7 +72,7 @@ class PrimitiveExecutor * @param delta_time Used to calculate acceleration. */ void sendLinearMotionToPlotJuggler(const Vector& target_local_velocity, - Duration delta_time); + const Duration& delta_time) const; /** * Records the velocities commanded this step so the next step can measure the diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index d29bd376bc..7a18d53426 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -81,7 +81,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, std::stoi(toml_config_client_->get(ROBOT_MULTICAST_CHANNEL_CONFIG_KEY))), network_interface_(toml_config_client_->get(ROBOT_NETWORK_INTERFACE_CONFIG_KEY)), loop_hz_(loop_hz), - primitive_executor_(robot_constants), + primitive_executor_(robot_constants, robot_id_), robot_localizer_(RobotLocalizer::RobotLocalizerConfig{ robot_constants.kalman_process_noise_variance_rad_per_s_4, robot_constants.kalman_vision_noise_variance_rad_2, diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 842b4b2ed7..7ef0cac3ea 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -16,7 +16,6 @@ #include "software/embedded/services/network/network.h" #include "software/embedded/services/power.h" #include "software/embedded/toml_config/toml_config_client.h" -#include "software/logger/logger.h" class Thunderloop { @@ -177,7 +176,6 @@ class Thunderloop // Current State robot_constants::RobotConstants robot_constants_; - Angle current_orientation_; int robot_id_; int channel_id_; std::string network_interface_; From 214ae7ee95ed9d5a398afeee5b8f29b941a2f0e4 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Wed, 17 Jun 2026 20:44:41 -0700 Subject: [PATCH 42/46] cleanup --- src/software/embedded/thunderloop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 7a18d53426..d29bd376bc 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -81,7 +81,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, std::stoi(toml_config_client_->get(ROBOT_MULTICAST_CHANNEL_CONFIG_KEY))), network_interface_(toml_config_client_->get(ROBOT_NETWORK_INTERFACE_CONFIG_KEY)), loop_hz_(loop_hz), - primitive_executor_(robot_constants, robot_id_), + primitive_executor_(robot_constants), robot_localizer_(RobotLocalizer::RobotLocalizerConfig{ robot_constants.kalman_process_noise_variance_rad_per_s_4, robot_constants.kalman_vision_noise_variance_rad_2, From fac4b7195f619feb773d002af216f53c92ba5573 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Thu, 18 Jun 2026 03:50:19 +0000 Subject: [PATCH 43/46] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/primitive_executor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 7b477f3962..08db5abca7 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -10,9 +10,9 @@ #include "software/logger/logger.h" #include "software/physics/velocity_conversion_util.h" -PrimitiveExecutor::PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants) : - state_(), current_primitive_(), - robot_constants_(robot_constants) +PrimitiveExecutor::PrimitiveExecutor( + const robot_constants::RobotConstants& robot_constants) + : state_(), current_primitive_(), robot_constants_(robot_constants) { } @@ -102,9 +102,9 @@ AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(const Duration& del // delta_time per tick. const double max_angular_velocity_delta = robot_constants_.robot_max_ang_acceleration_rad_per_s_2 * delta_time.toSeconds(); - const double angular_velocity_delta = std::clamp( - (target_w - prev_target_angular_velocity_).toRadians(), - -max_angular_velocity_delta, max_angular_velocity_delta); + const double angular_velocity_delta = + std::clamp((target_w - prev_target_angular_velocity_).toRadians(), + -max_angular_velocity_delta, max_angular_velocity_delta); target_w = prev_target_angular_velocity_ + AngularVelocity::fromRadians(angular_velocity_delta); prev_target_angular_velocity_ = target_w; From c0f17b76adf7ec41e396243af3340952e89737fb Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Thu, 18 Jun 2026 18:16:15 -0700 Subject: [PATCH 44/46] enable i2c --- src/software/embedded/linux_configs/pi/config.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/linux_configs/pi/config.txt b/src/software/embedded/linux_configs/pi/config.txt index 22488d67e0..7ea6724ced 100755 --- a/src/software/embedded/linux_configs/pi/config.txt +++ b/src/software/embedded/linux_configs/pi/config.txt @@ -3,8 +3,8 @@ # Some settings may impact device functionality. See link above for details # Uncomment some or all of these to enable the optional hardware interfaces -#dtparam=i2c_arm=on -#dtparam=i2s=on +dtparam=i2c_arm=on +dtparam=i2s=on dtparam=spi=on # Enable audio (loads snd_bcm2835) From 44c311d45998887b936cec48b328f40f10cc716e Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 20 Jun 2026 00:19:43 -0700 Subject: [PATCH 45/46] Implement new SPI protocol with message delimiting and acknowledgements --- .../stspin_motor_controller.cpp | 246 ++++++++++-------- .../stspin_motor_controller.h | 47 ++-- .../stspin_motor_controller_test.cpp | 26 +- .../embedded/motor_controller/stspin_types.h | 18 +- 4 files changed, 183 insertions(+), 154 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 1c29915b4d..66b08fb239 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -45,8 +45,9 @@ void StSpinMotorController::setup() for (const MotorIndex motor : driveMotors()) { - sendAndReceiveFrame(motor, SetPidSpeedKpKiFrame{.kp = SPEED_PID_PROPORTIONAL_GAIN, - .ki = SPEED_PID_INTEGRAL_GAIN}); + sendAndReceiveMessage(motor, + SetPidSpeedKpKiMessage{.kp = SPEED_PID_PROPORTIONAL_GAIN, + .ki = SPEED_PID_INTEGRAL_GAIN}); } } @@ -181,12 +182,12 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, return 0; } - const auto outgoing_frame = SetTargetSpeedFrame{ + const auto outgoing_message = SetTargetSpeedMessage{ .motor_enabled = motor_status_.at(motor).enabled, .motor_target_speed_rpm = static_cast(target_velocity), }; - sendAndReceiveFrame(motor, outgoing_frame); + sendAndReceiveMessage(motor, outgoing_message); return motor_status_.at(motor).speed; } @@ -199,18 +200,18 @@ void StSpinMotorController::updateEuclideanVelocity( if (local_velocity.length() <= MINIMUM_SPEED_FOR_FEED_FORWARD) { - sendAndReceiveFrame( + sendAndReceiveMessage( MotorIndex::FRONT_LEFT, - SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); - sendAndReceiveFrame( + SetSpeedFeedForwardKsMessage{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveMessage( MotorIndex::FRONT_RIGHT, - SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); - sendAndReceiveFrame( + SetSpeedFeedForwardKsMessage{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveMessage( MotorIndex::BACK_RIGHT, - SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); - sendAndReceiveFrame( + SetSpeedFeedForwardKsMessage{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + sendAndReceiveMessage( MotorIndex::BACK_LEFT, - SetSpeedFeedForwardKsFrame{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); + SetSpeedFeedForwardKsMessage{.ks = MIN_SPEED_FEED_FORWARD_STATIC_GAIN}); return; } @@ -234,14 +235,14 @@ void StSpinMotorController::updateEuclideanVelocity( MAX_SPEED_FEED_FORWARD_STATIC_GAIN * std::abs((direction - Angle::quarter() - back_wheel_angle).sin())); - sendAndReceiveFrame(MotorIndex::FRONT_LEFT, - SetSpeedFeedForwardKsFrame{.ks = front_left_ks}); - sendAndReceiveFrame(MotorIndex::FRONT_RIGHT, - SetSpeedFeedForwardKsFrame{.ks = front_right_ks}); - sendAndReceiveFrame(MotorIndex::BACK_RIGHT, - SetSpeedFeedForwardKsFrame{.ks = back_right_ks}); - sendAndReceiveFrame(MotorIndex::BACK_LEFT, - SetSpeedFeedForwardKsFrame{.ks = back_left_ks}); + sendAndReceiveMessage(MotorIndex::FRONT_LEFT, + SetSpeedFeedForwardKsMessage{.ks = front_left_ks}); + sendAndReceiveMessage(MotorIndex::FRONT_RIGHT, + SetSpeedFeedForwardKsMessage{.ks = front_right_ks}); + sendAndReceiveMessage(MotorIndex::BACK_RIGHT, + SetSpeedFeedForwardKsMessage{.ks = back_right_ks}); + sendAndReceiveMessage(MotorIndex::BACK_LEFT, + SetSpeedFeedForwardKsMessage{.ks = back_left_ks}); } void StSpinMotorController::immediatelyDisable() @@ -272,175 +273,200 @@ void StSpinMotorController::openSpiFileDescriptor(const MotorIndex motor) << "error: " << strerror(errno); } -void StSpinMotorController::sendAndReceiveFrame(const MotorIndex motor, - const OutgoingFrame& outgoing_frame) +void StSpinMotorController::sendAndReceiveMessage(const MotorIndex motor, + const OutgoingMessage& outgoing_message) { - std::array tx{}; - std::array rx{}; + std::array tx{}; + std::array rx{}; - populateTx(outgoing_frame, tx); + motor_status_[motor].seq_num++; - spiTransfer(spi_fds_[motor], tx.data(), rx.data(), FRAME_LEN, SPI_SPEED_HZ); + populateTx(motor, outgoing_message, tx); - motor_status_[motor].frame_count++; - - // Frame integrity check - const uint8_t rx_crc = Crc8Autosar::calc(rx.data(), FRAME_LEN - 1); - if (rx[5] != rx_crc) + std::vector received_data; + while (true) { - // Log RX buffer and expected vs. actual CRC - std::ostringstream oss; - oss << std::hex << std::uppercase << std::setfill('0') << "Expected CRC 0x" - << std::setw(2) << static_cast(rx[5]) << " but got 0x" << std::setw(2) - << static_cast(rx_crc) << ". RX: "; - for (const uint8_t byte : rx) + spiTransfer(spi_fds_[motor], tx.data(), rx.data(), MESSAGE_SIZE, SPI_SPEED_HZ); + received_data.insert(received_data.end(), rx.begin(), rx.end()); + + auto delimiter_pos = + std::find(received_data.begin(), received_data.end(), MESSAGE_DELIMITER); + + if (delimiter_pos == received_data.end()) { - oss << "0x" << std::setw(2) << static_cast(byte) << " "; + // No delimiter byte found, discard everything + received_data.clear(); + continue; } - LOG(WARNING) << "Frame #" << motor_status_[motor].frame_count - << " received from motor " << motor << " failed integrity check. " - << oss.str(); - return; - } + if (std::distance(delimiter_pos, received_data.end()) < MESSAGE_SIZE) + { + // Not enough bytes after delimiter byte for a full message, + // wait for more bytes + continue; + } - processRx(motor, rx); + // Message integrity check + const uint8_t expected_crc = *std::next(delimiter_pos, MESSAGE_SIZE - 1); + const uint8_t actual_crc = Crc8Autosar::calc(&(*delimiter_pos), MESSAGE_SIZE - 1); + if (expected_crc == actual_crc) + { + const uint8_t ack_seq_num = *std::next(delimiter_pos); + if (ack_seq_num == motor_status_.at(motor).seq_num) + { + // The message we just sent was acknowledged, we can stop resending + // and waiting for a response + std::array message{}; + std::copy(delimiter_pos, std::next(delimiter_pos, MESSAGE_SIZE), + message.begin()); + processRx(motor, message); + return; + } + } + + received_data.erase(received_data.begin(), std::next(delimiter_pos)); + } } -void StSpinMotorController::populateTx(const OutgoingFrame& outgoing_frame, - std::array& tx) +void StSpinMotorController::populateTx(const MotorIndex motor, + const OutgoingMessage& outgoing_message, + std::array& tx) { + tx[0] = MESSAGE_DELIMITER; + tx[1] = motor_status_.at(motor).seq_num; + std::visit( - [&](TFrame&& frame) + [&](TMessage&& message) { - using T = std::decay_t; - if constexpr (std::is_same_v) + using T = std::decay_t; + if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::NO_OP); + tx[2] = static_cast(StSpinOpcode::NO_OP); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_TARGET_SPEED); - tx[1] = static_cast(frame.motor_enabled); - tx[2] = static_cast(0xFF & (frame.motor_target_speed_rpm >> 8)); - tx[3] = static_cast(0xFF & frame.motor_target_speed_rpm); + tx[2] = static_cast(StSpinOpcode::SET_TARGET_SPEED); + tx[3] = static_cast(message.motor_enabled); + tx[4] = + static_cast(0xFF & (message.motor_target_speed_rpm >> 8)); + tx[5] = static_cast(0xFF & message.motor_target_speed_rpm); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_TARGET_TORQUE); - tx[1] = static_cast(frame.motor_enabled); - tx[2] = static_cast(0xFF & (frame.motor_target_torque >> 8)); - tx[3] = static_cast(0xFF & frame.motor_target_torque); + tx[2] = static_cast(StSpinOpcode::SET_TARGET_TORQUE); + tx[3] = static_cast(message.motor_enabled); + tx[4] = static_cast(0xFF & (message.motor_target_torque >> 8)); + tx[5] = static_cast(0xFF & message.motor_target_torque); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_RESPONSE_TYPE); - tx[1] = static_cast(frame.response_type); + tx[2] = static_cast(StSpinOpcode::SET_RESPONSE_TYPE); + tx[3] = static_cast(message.response_type); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_PID_TORQUE_KP_KI); - tx[1] = static_cast(0xFF & (frame.kp >> 8)); - tx[2] = static_cast(0xFF & frame.kp); - tx[3] = static_cast(0xFF & (frame.ki >> 8)); - tx[4] = static_cast(0xFF & frame.ki); + tx[2] = static_cast(StSpinOpcode::SET_PID_TORQUE_KP_KI); + tx[3] = static_cast(0xFF & (message.kp >> 8)); + tx[4] = static_cast(0xFF & message.kp); + tx[5] = static_cast(0xFF & (message.ki >> 8)); + tx[6] = static_cast(0xFF & message.ki); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_PID_FLUX_KP_KI); - tx[1] = static_cast(0xFF & (frame.kp >> 8)); - tx[2] = static_cast(0xFF & frame.kp); - tx[3] = static_cast(0xFF & (frame.ki >> 8)); - tx[4] = static_cast(0xFF & frame.ki); + tx[2] = static_cast(StSpinOpcode::SET_PID_FLUX_KP_KI); + tx[3] = static_cast(0xFF & (message.kp >> 8)); + tx[4] = static_cast(0xFF & message.kp); + tx[5] = static_cast(0xFF & (message.ki >> 8)); + tx[6] = static_cast(0xFF & message.ki); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_PID_SPEED_KP_KI); - tx[1] = static_cast(0xFF & (frame.kp >> 8)); - tx[2] = static_cast(0xFF & frame.kp); - tx[3] = static_cast(0xFF & (frame.ki >> 8)); - tx[4] = static_cast(0xFF & frame.ki); + tx[2] = static_cast(StSpinOpcode::SET_PID_SPEED_KP_KI); + tx[3] = static_cast(0xFF & (message.kp >> 8)); + tx[4] = static_cast(0xFF & message.kp); + tx[5] = static_cast(0xFF & (message.ki >> 8)); + tx[6] = static_cast(0xFF & message.ki); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KA_KV); - tx[1] = static_cast(0xFF & (frame.ka >> 8)); - tx[2] = static_cast(0xFF & frame.ka); - tx[3] = static_cast(0xFF & (frame.kv >> 8)); - tx[4] = static_cast(0xFF & frame.kv); + tx[2] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KA_KV); + tx[3] = static_cast(0xFF & (message.ka >> 8)); + tx[4] = static_cast(0xFF & message.ka); + tx[5] = static_cast(0xFF & (message.kv >> 8)); + tx[6] = static_cast(0xFF & message.kv); } - else if constexpr (std::is_same_v) + else if constexpr (std::is_same_v) { - tx[0] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KS); - tx[1] = static_cast(0xFF & (frame.ks >> 8)); - tx[2] = static_cast(0xFF & frame.ks); + tx[2] = static_cast(StSpinOpcode::SET_SPEED_FEED_FORWARD_KS); + tx[3] = static_cast(0xFF & (message.ks >> 8)); + tx[4] = static_cast(0xFF & message.ks); } }, - outgoing_frame); + outgoing_message); - tx[5] = Crc8Autosar::calc(tx.data(), FRAME_LEN - 1); + tx[7] = Crc8Autosar::calc(tx.data(), MESSAGE_SIZE - 1); } void StSpinMotorController::processRx(const MotorIndex motor, - const std::array& rx) + const std::array& rx) { - switch (static_cast(rx[0])) + switch (static_cast(rx[2])) { case StSpinResponseType::SPEED_AND_FAULTS: { motor_status_[motor].speed = - static_cast((static_cast(rx[1]) << 8) | rx[2]); + static_cast((static_cast(rx[3]) << 8) | rx[4]); const uint16_t fault_flags = - static_cast((static_cast(rx[3]) << 8) | rx[4]); + static_cast((static_cast(rx[5]) << 8) | rx[6]); updateFaults(motor, fault_flags); break; } case StSpinResponseType::IQ_AND_ID: { motor_status_[motor].iq = - static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_status_[motor].id = static_cast((static_cast(rx[3]) << 8) | rx[4]); + motor_status_[motor].id = + static_cast((static_cast(rx[5]) << 8) | rx[6]); break; } case StSpinResponseType::VQ_AND_VD: { motor_status_[motor].vq = - static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_status_[motor].vd = static_cast((static_cast(rx[3]) << 8) | rx[4]); + motor_status_[motor].vd = + static_cast((static_cast(rx[5]) << 8) | rx[6]); break; } case StSpinResponseType::PHASE_CURRENT_AND_VOLTAGE: { motor_status_[motor].phase_current = - static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_status_[motor].phase_voltage = static_cast((static_cast(rx[3]) << 8) | rx[4]); + motor_status_[motor].phase_voltage = + static_cast((static_cast(rx[5]) << 8) | rx[6]); break; } case StSpinResponseType::IQ_AND_IQ_REF: { motor_status_[motor].iq = - static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_status_[motor].iq_ref = static_cast((static_cast(rx[3]) << 8) | rx[4]); + motor_status_[motor].iq_ref = + static_cast((static_cast(rx[5]) << 8) | rx[6]); break; } case StSpinResponseType::ID_AND_ID_REF: { motor_status_[motor].id = - static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_status_[motor].id_ref = static_cast((static_cast(rx[3]) << 8) | rx[4]); + motor_status_[motor].id_ref = + static_cast((static_cast(rx[5]) << 8) | rx[6]); break; } case StSpinResponseType::SPEED_AND_SPEED_REF: { motor_status_[motor].speed = - static_cast((static_cast(rx[1]) << 8) | rx[2]); - motor_status_[motor].speed_ref = static_cast((static_cast(rx[3]) << 8) | rx[4]); + motor_status_[motor].speed_ref = + static_cast((static_cast(rx[5]) << 8) | rx[6]); break; } } @@ -452,7 +478,7 @@ void StSpinMotorController::sendMotorStatusToPlotJuggler(const MotorIndex motor) {StSpinResponseType::SPEED_AND_SPEED_REF, StSpinResponseType::IQ_AND_IQ_REF, StSpinResponseType::ID_AND_ID_REF, StSpinResponseType::SPEED_AND_FAULTS}) { - sendAndReceiveFrame(motor, SetResponseTypeFrame{response_type}); + sendAndReceiveMessage(motor, SetResponseTypeMessage{response_type}); } const MotorStatus& motor_status = motor_status_.at(motor); diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.h b/src/software/embedded/motor_controller/stspin_motor_controller.h index 88a6c778ae..9043847833 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.h +++ b/src/software/embedded/motor_controller/stspin_motor_controller.h @@ -33,14 +33,16 @@ class StSpinMotorController : public MotorController private: // TODO: #3750 Use a template function instead of std::variant. - using OutgoingFrame = - std::variant; + using OutgoingMessage = + std::variant; - // Length of frame (in number of bytes) - static constexpr unsigned int FRAME_LEN = 6; + // Length of message (in number of bytes) + static constexpr unsigned int MESSAGE_SIZE = 8; + + static constexpr uint8_t MESSAGE_DELIMITER = 0xAA; // clang-format off static const inline std::unordered_map SPI_PATHS = { @@ -74,7 +76,7 @@ class StSpinMotorController : public MotorController struct MotorStatus { - unsigned int frame_count; + uint16_t seq_num; bool enabled; MotorFaultIndicator faults; uint16_t fault_flags; @@ -100,30 +102,31 @@ class StSpinMotorController : public MotorController void openSpiFileDescriptor(MotorIndex motor); /** - * Transmits a frame to the given motor and receives a frame back over SPI. + * Transmits a message to the given motor and receives a message back over SPI. * - * @param motor the motor to send the frame to - * @param outgoing_frame the outgoing frame to send to the motor + * @param motor the motor to send the message to + * @param outgoing_message the outgoing message to send to the motor */ - void sendAndReceiveFrame(MotorIndex motor, const OutgoingFrame& outgoing_frame); + void sendAndReceiveMessage(MotorIndex motor, const OutgoingMessage& outgoing_message); /** - * Populates the transmit buffer with the data from an outgoing frame. + * Populates the transmit buffer with the data from an outgoing message. * - * @param outgoing_frame the outgoing frame to populate the transmit buffer with - * @param tx the transmit buffer to populate with the outgoing frame's data + * @param motor the motor to send the message to + * @param outgoing_message the outgoing message to populate the transmit buffer with + * @param tx the transmit buffer to populate with the outgoing message's data */ - void populateTx(const OutgoingFrame& outgoing_frame, - std::array& tx); + void populateTx(MotorIndex motor, const OutgoingMessage& outgoing_message, + std::array& tx); /** - * Processes a frame received from the given motor, caching any motor status - * the frame provides. + * Processes a message received from the given motor, caching any motor status + * the message provides. * - * @param motor the motor that the received frame corresponds to - * @param rx the receive buffer with the received frame to process + * @param motor the motor that the received message corresponds to + * @param message the received message to process */ - void processRx(MotorIndex motor, const std::array& rx); + void processRx(MotorIndex motor, const std::array& message); /** * Records that the given faults were raised for a given motor. diff --git a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp index 1f54b929f5..c2be3f6bda 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller_test.cpp @@ -56,23 +56,23 @@ class StSpinMotorControllerTest { if (args_.torque_pid.kp && args_.torque_pid.ki) { - motor_controller->sendAndReceiveFrame( - motor, SetPidTorqueKpKiFrame{.kp = args_.torque_pid.kp.value(), - .ki = args_.torque_pid.ki.value()}); + motor_controller->sendAndReceiveMessage( + motor, SetPidTorqueKpKiMessage{.kp = args_.torque_pid.kp.value(), + .ki = args_.torque_pid.ki.value()}); } if (args_.flux_pid.kp && args_.flux_pid.ki) { - motor_controller->sendAndReceiveFrame( - motor, SetPidFluxKpKiFrame{.kp = args_.flux_pid.kp.value(), - .ki = args_.flux_pid.ki.value()}); + motor_controller->sendAndReceiveMessage( + motor, SetPidFluxKpKiMessage{.kp = args_.flux_pid.kp.value(), + .ki = args_.flux_pid.ki.value()}); } if (args_.speed_pid.kp && args_.speed_pid.ki) { - motor_controller->sendAndReceiveFrame( - motor, SetPidSpeedKpKiFrame{.kp = args_.speed_pid.kp.value(), - .ki = args_.speed_pid.ki.value()}); + motor_controller->sendAndReceiveMessage( + motor, SetPidSpeedKpKiMessage{.kp = args_.speed_pid.kp.value(), + .ki = args_.speed_pid.ki.value()}); } } @@ -87,17 +87,17 @@ class StSpinMotorControllerTest break; } - StSpinMotorController::OutgoingFrame command_frame; + StSpinMotorController::OutgoingMessage command_message; if (args_.mode == "speed") { - command_frame = SetTargetSpeedFrame{ + command_message = SetTargetSpeedMessage{ .motor_enabled = true, .motor_target_speed_rpm = setpoint, }; } else if (args_.mode == "torque") { - command_frame = SetTargetTorqueFrame{ + command_message = SetTargetTorqueMessage{ .motor_enabled = true, .motor_target_torque = setpoint, }; @@ -105,7 +105,7 @@ class StSpinMotorControllerTest for (const MotorIndex motor : args_.enabled_motors) { - motor_controller->sendAndReceiveFrame(motor, command_frame); + motor_controller->sendAndReceiveMessage(motor, command_message); } auto start_time = std::chrono::steady_clock::now(); diff --git a/src/software/embedded/motor_controller/stspin_types.h b/src/software/embedded/motor_controller/stspin_types.h index 7b1ba2e3c8..50e9ed988b 100644 --- a/src/software/embedded/motor_controller/stspin_types.h +++ b/src/software/embedded/motor_controller/stspin_types.h @@ -42,52 +42,52 @@ enum class StSpinFaultCode DP_FAULT = 0x0400 }; -struct NoOpFrame +struct NoOpMessage { }; -struct SetTargetSpeedFrame +struct SetTargetSpeedMessage { bool motor_enabled; int16_t motor_target_speed_rpm; }; -struct SetTargetTorqueFrame +struct SetTargetTorqueMessage { bool motor_enabled; int16_t motor_target_torque; }; -struct SetResponseTypeFrame +struct SetResponseTypeMessage { StSpinResponseType response_type; }; -struct SetPidTorqueKpKiFrame +struct SetPidTorqueKpKiMessage { int16_t kp; int16_t ki; }; -struct SetPidFluxKpKiFrame +struct SetPidFluxKpKiMessage { int16_t kp; int16_t ki; }; -struct SetPidSpeedKpKiFrame +struct SetPidSpeedKpKiMessage { int16_t kp; int16_t ki; }; -struct SetSpeedFeedForwardKaKvFrame +struct SetSpeedFeedForwardKaKvMessage { int16_t ka; int16_t kv; }; -struct SetSpeedFeedForwardKsFrame +struct SetSpeedFeedForwardKsMessage { int16_t ks; }; From d15fcad41a713ff5f9a9883cf6e896a125244031 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sat, 20 Jun 2026 19:31:34 +0000 Subject: [PATCH 46/46] [pre-commit.ci lite] apply automatic fixes --- .../motor_controller/stspin_motor_controller.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index f88ff978ff..f49c572036 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -46,15 +46,11 @@ void StSpinMotorController::setup() for (const MotorIndex motor : driveMotors()) { sendAndReceiveMessage(motor, - SetPidSpeedKpKiMessage{ - .kp = SPEED_PID_PROPORTIONAL_GAIN, - .ki = SPEED_PID_INTEGRAL_GAIN - }); + SetPidSpeedKpKiMessage{.kp = SPEED_PID_PROPORTIONAL_GAIN, + .ki = SPEED_PID_INTEGRAL_GAIN}); sendAndReceiveMessage(motor, - SetPidTorqueKpKiMessage{ - .kp = TORQUE_PID_PROPORTIONAL_GAIN, - .ki = TORQUE_PID_INTEGRAL_GAIN - }); + SetPidTorqueKpKiMessage{.kp = TORQUE_PID_PROPORTIONAL_GAIN, + .ki = TORQUE_PID_INTEGRAL_GAIN}); } }