diff --git a/AccelerationConstraint.hpp b/AccelerationConstraint.hpp new file mode 100644 index 0000000..dbf9373 --- /dev/null +++ b/AccelerationConstraint.hpp @@ -0,0 +1,484 @@ +#pragma once + +#include +#include +#include + +/** + * Two-dimensional acceleration allocation, following the document's polygon, + * ellipse-support and edge-intersection construction. Motor speeds and wheel PID + * outputs are frozen in Input. Powers are signed; braking power is not clipped. + * Decision coordinates are normalized acceleration fractions in [0, 1]. All + * geometry uses single precision so Cortex-M4F runs it on the hardware FPU. + */ +namespace AccelerationConstraint +{ +struct Vec2 +{ + float x = 0.0f; + float y = 0.0f; +}; + +struct MotorModel +{ + float u = 0.0f; ///< Current = u*x + v*y + c, in motor command units. + float v = 0.0f; + float c = 0.0f; + float rpm = 0.0f; + float kt = 0.0f; + float k1 = 0.0f; + float k2 = 0.0f; +}; + +struct HalfPlane +{ + float x = 0.0f; + float y = 0.0f; + float limit = 0.0f; ///< x*decision.x + y*decision.y <= limit. +}; + +struct Input +{ + MotorModel motors[4]{}; + HalfPlane halfplanes[12]{}; + int halfplane_count = 0; + Vec2 weights{1.0f, 1.0f}; + float power_budget = 0.0f; ///< Already excludes static chassis loss and margin. +}; + +enum class Status +{ + OPTIMAL, + INFEASIBLE, + INVALID_INPUT +}; + +struct Result +{ + bool feasible = false; + bool minimum_available = false; + Vec2 point{}; + float power = 0.0f; + Status status = Status::INVALID_INPUT; +}; + +namespace Detail +{ +struct Point +{ + float x; + float y; +}; + +struct Polygon +{ + Point points[20]{}; + int count = 0; +}; + +struct Quadratic +{ + float a = 0.0f; + float b = 0.0f; // f = a*x*x + 2*b*x*y + c*y*y + d*x + e*y + f. + float c = 0.0f; + float d = 0.0f; + float e = 0.0f; + float f = 0.0f; + + float Value(Point p) const + { + return (a * p.x + 2.0f * b * p.y + d) * p.x + (c * p.y + e) * p.y + f; + } +}; + +inline bool AddPoint(Polygon& polygon, Point point) +{ + if (polygon.count > 0) + { + const Point LAST = polygon.points[polygon.count - 1]; + if (std::abs(point.x - LAST.x) + std::abs(point.y - LAST.y) < 1e-12f) + { + return true; + } + } + if (polygon.count == 20) + { + return false; + } + polygon.points[polygon.count++] = point; + return true; +} + +inline bool Clip(Polygon& polygon, HalfPlane plane) +{ + Polygon output; + if (polygon.count == 0) + { + return true; + } + Point previous = polygon.points[polygon.count - 1]; + float previous_distance = plane.x * previous.x + plane.y * previous.y - plane.limit; + for (int i = 0; i < polygon.count; ++i) + { + const Point CURRENT = polygon.points[i]; + const float DISTANCE = plane.x * CURRENT.x + plane.y * CURRENT.y - plane.limit; + if ((DISTANCE <= 0.0f) != (previous_distance <= 0.0f)) + { + const float T = previous_distance / (previous_distance - DISTANCE); + if (!AddPoint(output, {previous.x + T * (CURRENT.x - previous.x), + previous.y + T * (CURRENT.y - previous.y)})) + { + return false; + } + } + if (DISTANCE <= 0.0f && !AddPoint(output, CURRENT)) + { + return false; + } + previous = CURRENT; + previous_distance = DISTANCE; + } + if (output.count > 1) + { + const Point FIRST = output.points[0]; + const Point LAST = output.points[output.count - 1]; + if (std::abs(FIRST.x - LAST.x) + std::abs(FIRST.y - LAST.y) < 1e-12f) + { + --output.count; + } + } + polygon = output; + return true; +} + +inline bool Inside(const Input& input, Point point) +{ + constexpr float TOLERANCE = 2e-7f; + if (!std::isfinite(point.x) || !std::isfinite(point.y) || point.x < -TOLERANCE || + point.x > 1.0f + TOLERANCE || point.y < -TOLERANCE || point.y > 1.0f + TOLERANCE) + { + return false; + } + for (int i = 0; i < input.halfplane_count; ++i) + { + const auto& PLANE = input.halfplanes[i]; + const float SCALE = + 1.0f + std::abs(PLANE.x) + std::abs(PLANE.y) + std::abs(PLANE.limit); + if (PLANE.x * point.x + PLANE.y * point.y - PLANE.limit > TOLERANCE * SCALE) + { + return false; + } + } + return true; +} + +inline float MotorPower(const Input& input, Point point) +{ + float total = 0.0f; + for (const auto& MOTOR : input.motors) + { + const float CURRENT = MOTOR.u * point.x + MOTOR.v * point.y + MOTOR.c; + total += MOTOR.k1 * CURRENT * CURRENT + MOTOR.kt * MOTOR.rpm * CURRENT + + MOTOR.k2 * MOTOR.rpm * MOTOR.rpm; + } + return total; +} + +inline Quadratic MakeQuadratic(const Input& input) +{ + Quadratic result; + for (const auto& MOTOR : input.motors) + { + const float U = MOTOR.u; + const float V = MOTOR.v; + const float C = MOTOR.c; + const float LINEAR = MOTOR.kt * MOTOR.rpm; + result.a += MOTOR.k1 * U * U; + result.b += MOTOR.k1 * U * V; + result.c += MOTOR.k1 * V * V; + result.d += (2.0f * MOTOR.k1 * C + LINEAR) * U; + result.e += (2.0f * MOTOR.k1 * C + LINEAR) * V; + result.f += MOTOR.k1 * C * C + LINEAR * C + MOTOR.k2 * MOTOR.rpm * MOTOR.rpm; + } + return result; +} + +struct Search +{ + const Input& input; + const Quadratic& quadratic; + Point minimum{}; + float minimum_power = std::numeric_limits::infinity(); + Point best{}; + float best_value = -std::numeric_limits::infinity(); + bool found = false; + + void Minimum(Point point) + { + if (!Inside(input, point)) + { + return; + } + const float POWER = MotorPower(input, point); + if (POWER < minimum_power) + { + minimum_power = POWER; + minimum = point; + } + } + + void Candidate(Point point) + { + // Validate the actual float coordinates which the firmware will consume. + point = {std::clamp(point.x, 0.0f, 1.0f), std::clamp(point.y, 0.0f, 1.0f)}; + if (!Inside(input, point)) + { + return; + } + float power = MotorPower(input, point); + const float POWER_TOLERANCE = 2e-5f + 2e-7f * std::abs(input.power_budget); + // Rounding an analytic boundary root can put it a few ulps outside. Move + // toward a known lower-power point, preserving every convex half-plane. + // Convexity bounds power on this segment without a root-search iteration. + if (std::isfinite(power) && power > input.power_budget + POWER_TOLERANCE && + minimum_power < input.power_budget) + { + const float FRACTION = std::min( + 1.0f, (power - input.power_budget + POWER_TOLERANCE) / (power - minimum_power)); + point = {point.x + FRACTION * (minimum.x - point.x), + point.y + FRACTION * (minimum.y - point.y)}; + power = MotorPower(input, point); + if (!Inside(input, point)) + { + return; + } + } + if (!std::isfinite(power) || power > input.power_budget + POWER_TOLERANCE) + { + return; + } + const float VALUE = input.weights.x * point.x + input.weights.y * point.y; + if (!found || VALUE > best_value + 2e-7f || + (std::abs(VALUE - best_value) <= 2e-7f && point.x + point.y > best.x + best.y)) + { + best = point; + best_value = VALUE; + found = true; + } + } + + void Edge(Point start, Point end) + { + const Point STEP{end.x - start.x, end.y - start.y}; + const float A = quadratic.a * STEP.x * STEP.x + 2.0f * quadratic.b * STEP.x * STEP.y + + quadratic.c * STEP.y * STEP.y; + const float B = 2.0f * quadratic.a * start.x * STEP.x + + 2.0f * quadratic.b * (start.x * STEP.y + start.y * STEP.x) + + 2.0f * quadratic.c * start.y * STEP.y + quadratic.d * STEP.x + + quadratic.e * STEP.y; + const float C = quadratic.Value(start) - input.power_budget; + if (A > 0.0f) + { + const float T = std::clamp(-B / (2.0f * A), 0.0f, 1.0f); + Minimum({start.x + T * STEP.x, start.y + T * STEP.y}); + } + if (std::abs(A) <= 2e-7f * (1.0f + std::abs(B) + std::abs(C))) + { + if (B != 0.0f) + { + const float T = -C / B; + if (T >= -2e-7f && T <= 1.0f + 2e-7f) + { + Candidate({start.x + std::clamp(T, 0.0f, 1.0f) * STEP.x, + start.y + std::clamp(T, 0.0f, 1.0f) * STEP.y}); + } + } + return; + } + float discriminant = B * B - 4.0f * A * C; + const float DISCRIMINANT_TOLERANCE = + 2e-7f * (B * B + std::abs(4.0f * A * C) + 1e-30f); + if (discriminant < -DISCRIMINANT_TOLERANCE) + { + return; + } + discriminant = std::max(0.0f, discriminant); + const float ROOT = std::sqrt(discriminant); + // Stable quadratic roots, avoiding cancellation when B and sqrt(delta) are close. + const float Q = -0.5f * (B + std::copysign(ROOT, B)); + const float ROOTS[2] = {Q / A, Q != 0.0f ? C / Q : -B / (2.0f * A)}; + for (float t : ROOTS) + { + if (t >= -2e-7f && t <= 1.0f + 2e-7f) + { + t = std::clamp(t, 0.0f, 1.0f); + Candidate({start.x + t * STEP.x, start.y + t * STEP.y}); + } + } + } + + void Interior() + { + const float A = quadratic.a; + const float B = quadratic.b; + const float C = quadratic.c; + const float EIGENVALUE = A + C; + if (EIGENVALUE <= 0.0f) + { + return; // Affine or constant power; polygon edges cover the optimum. + } + // Rotate before forming the Gram matrix. Directly subtracting a*c-b*b + // loses a thin ellipse's small curvature in single precision. Projecting + // the original motor-current vectors preserves it as a sum of squares. + const float RATIO = A >= C ? B / A : B / C; + const float INVERSE_LENGTH = 1.0f / std::sqrt(1.0f + RATIO * RATIO); + const Point normal = A >= C ? Point{INVERSE_LENGTH, RATIO * INVERSE_LENGTH} + : Point{RATIO * INVERSE_LENGTH, INVERSE_LENGTH}; + const Point TANGENT{-normal.y, normal.x}; + float qn = 0.0f; + float qt = 0.0f; + float qnt = 0.0f; + float dn = 0.0f; + float dt = 0.0f; + for (const auto& MOTOR : input.motors) + { + const float UN = MOTOR.u * normal.x + MOTOR.v * normal.y; + const float UT = MOTOR.u * TANGENT.x + MOTOR.v * TANGENT.y; + const float LINEAR = 2.0f * MOTOR.k1 * MOTOR.c + MOTOR.kt * MOTOR.rpm; + qn += MOTOR.k1 * UN * UN; + qt += MOTOR.k1 * UT * UT; + qnt += MOTOR.k1 * UN * UT; + dn += LINEAR * UN; + dt += LINEAR * UT; + } + const float DN = dn; + const float DT = dt; + const float WN = input.weights.x * normal.x + input.weights.y * normal.y; + const float WT = input.weights.x * TANGENT.x + input.weights.y * TANGENT.y; + const float DETERMINANT = qn * qt - qnt * qnt; + if (DETERMINANT > 1e-12f * qn * qn) + { + const float CN = (qnt * DT - qt * DN) / (2.0f * DETERMINANT); + const float CT = (qnt * DN - qn * DT) / (2.0f * DETERMINANT); + const Point CENTER{normal.x * CN + TANGENT.x * CT, normal.y * CN + TANGENT.y * CT}; + Minimum(CENTER); + Candidate(CENTER); + const float RADIUS_SQUARED = input.power_budget - MotorPower(input, CENTER); + const float DIRECTION_N = (qt * WN - qnt * WT) / DETERMINANT; + const float DIRECTION_T = (qn * WT - qnt * WN) / DETERMINANT; + const float NORM = WN * DIRECTION_N + WT * DIRECTION_T; + if (RADIUS_SQUARED >= 0.0f && NORM > 0.0f) + { + const float STEP = std::sqrt(RADIUS_SQUARED / NORM); + Candidate({CENTER.x + STEP * (normal.x * DIRECTION_N + TANGENT.x * DIRECTION_T), + CENTER.y + STEP * (normal.y * DIRECTION_N + TANGENT.y * DIRECTION_T)}); + } + return; + } + // Rank-one PSD: a strip/line, or a parabola when the linear null-space + // term is nonzero. Edges cover a strip's optimum. + if (std::abs(DT) > 2e-7f * (1.0f + std::abs(DN) + std::abs(DT)) && WT / DT > 0.0f) + { + const float MULTIPLIER = WT / DT; + const float N = (WN / MULTIPLIER - DN) / (2.0f * EIGENVALUE); + const float T = + (input.power_budget - quadratic.f - EIGENVALUE * N * N - DN * N) / DT; + Candidate({normal.x * N + TANGENT.x * T, normal.y * N + TANGENT.y * T}); + } + } +}; +} // namespace Detail + +inline Result Solve(const Input& input) +{ + Result result; + if (input.halfplane_count < 0 || input.halfplane_count > 12 || + !std::isfinite(input.power_budget) || !std::isfinite(input.weights.x) || + !std::isfinite(input.weights.y) || input.weights.x < 0.0f || input.weights.y < 0.0f) + { + return result; + } + for (const auto& MOTOR : input.motors) + { + if (!std::isfinite(MOTOR.u) || !std::isfinite(MOTOR.v) || !std::isfinite(MOTOR.c) || + !std::isfinite(MOTOR.rpm) || !std::isfinite(MOTOR.kt) || + !std::isfinite(MOTOR.k1) || !std::isfinite(MOTOR.k2) || MOTOR.kt < 0.0f || + MOTOR.k1 < 0.0f || MOTOR.k2 < 0.0f) + { + return result; + } + } + for (int i = 0; i < input.halfplane_count; ++i) + { + const auto& PLANE = input.halfplanes[i]; + if (!std::isfinite(PLANE.x) || !std::isfinite(PLANE.y) || !std::isfinite(PLANE.limit)) + { + return result; + } + } + // Do not build a polygon or quadratic for the unconstrained common case. + if (Detail::Inside(input, {1.0f, 1.0f})) + { + const float POWER = Detail::MotorPower(input, {1.0f, 1.0f}); + if (std::isfinite(POWER) && POWER <= input.power_budget) + { + return {true, true, {1.0f, 1.0f}, POWER, Status::OPTIMAL}; + } + } + + Detail::Polygon polygon{{{0.0f, 0.0f}, {1.0f, 0.0f}, {1.0f, 1.0f}, {0.0f, 1.0f}}, 4}; + for (int i = 0; i < input.halfplane_count; ++i) + { + if (!Detail::Clip(polygon, input.halfplanes[i])) + { + return result; + } + } + result.status = Status::INFEASIBLE; + if (polygon.count == 0) + { + return result; + } + + // With inactive power constraints a linear objective attains its optimum at + // a polygon vertex. This also makes hard traction saturation a cheap path. + Detail::Point vertex = polygon.points[0]; + float vertex_value = input.weights.x * vertex.x + input.weights.y * vertex.y; + for (int i = 1; i < polygon.count; ++i) + { + const auto POINT = polygon.points[i]; + const float VALUE = input.weights.x * POINT.x + input.weights.y * POINT.y; + if (VALUE > vertex_value || + (VALUE == vertex_value && POINT.x + POINT.y > vertex.x + vertex.y)) + { + vertex = POINT; + vertex_value = VALUE; + } + } + vertex = {std::clamp(vertex.x, 0.0f, 1.0f), std::clamp(vertex.y, 0.0f, 1.0f)}; + const float VERTEX_POWER = Detail::MotorPower(input, vertex); + if (Detail::Inside(input, vertex) && std::isfinite(VERTEX_POWER) && + VERTEX_POWER <= input.power_budget) + { + return {true, true, {vertex.x, vertex.y}, VERTEX_POWER, Status::OPTIMAL}; + } + + const auto QUADRATIC = Detail::MakeQuadratic(input); + Detail::Search search{input, QUADRATIC}; + for (int i = 0; i < polygon.count; ++i) + { + search.Minimum(polygon.points[i]); + search.Candidate(polygon.points[i]); + search.Edge(polygon.points[i], polygon.points[(i + 1) % polygon.count]); + } + search.Interior(); + search.Candidate(search.minimum); + result.minimum_available = std::isfinite(search.minimum_power); + result.feasible = search.found; + const auto POINT = search.found ? search.best : search.minimum; + result.point = {POINT.x, POINT.y}; + result.power = Detail::MotorPower(input, {result.point.x, result.point.y}); + result.status = search.found ? Status::OPTIMAL : Status::INFEASIBLE; + return result; +} +} // namespace AccelerationConstraint diff --git a/Chassis.hpp b/Chassis.hpp index c11f9b5..0185262 100644 --- a/Chassis.hpp +++ b/Chassis.hpp @@ -127,6 +127,12 @@ class Chassis ///< High buffer energy threshold (J) float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 ///< First-order low-pass coefficient of the dynamic scale + float moment_of_inertia = + 0.0f; ///< Helm yaw inertia (kg*m^2); zero disables yaw feedforward. + float friction_coefficient = 0.35f; ///< Helm equal-load traction model. + float translation_acceleration_weight = 1.0f; + float angular_acceleration_weight = 1.0f; + float rotor_angular_acceleration_weight = 4.0f; }; /** @@ -239,34 +245,19 @@ class Chassis .chassis_cmd_topic_name = "chassis_cmd", .chassis_ref_topic_name = "chassis_ref", .gimbal_euler_topic_name = "gimbal_euler"}) - : chassis_(&motor_wheel_0, &motor_wheel_1, &motor_wheel_2, &motor_wheel_3, - motor_steer_0, motor_steer_1, motor_steer_2, motor_steer_3, &cmd, - &power_control, &referee, task_stack_depth, - typename ChassisType::ChassisParam{ - .wheel_radius = param.chassis_param.wheel_radius, - .wheel_to_center = param.chassis_param.wheel_to_center, - .gravity_height = param.chassis_param.gravity_height, - .reduction_ratio = param.chassis_param.reduction_ratio, - .wheel_resistance = param.chassis_param.wheel_resistance, - .error_compensation = param.chassis_param.error_compensation, - .gravity = param.chassis_param.gravity, - .length = param.chassis_param.length, - .width = param.chassis_param.width, - .rotor_speed_scale = param.chassis_param.rotor_speed_scale, - .rotor_omega_min_scale = param.chassis_param.rotor_omega_min_scale, - .rotor_buffer_low_j = param.chassis_param.rotor_buffer_low_j, - .rotor_buffer_high_j = param.chassis_param.rotor_buffer_high_j, - .rotor_scale_lpf_alpha = param.chassis_param.rotor_scale_lpf_alpha}, - param.pid_follow, param.pid_velocity_x, param.pid_velocity_y, - param.pid_omega, param.pid_wheel_speed_0, param.pid_wheel_speed_1, - param.pid_wheel_speed_2, param.pid_wheel_speed_3, - param.pid_steer_angle_0, param.pid_steer_angle_1, - param.pid_steer_angle_2, param.pid_steer_angle_3, - param.pid_steer_speed_0, param.pid_steer_speed_1, - param.pid_steer_speed_2, param.pid_steer_speed_3, param.thread_priority, - ChassisTopicNames{.chassis_cmd = param.chassis_cmd_topic_name, - .chassis_ref = param.chassis_ref_topic_name, - .gimbal_euler = param.gimbal_euler_topic_name}), + : chassis_( + &motor_wheel_0, &motor_wheel_1, &motor_wheel_2, &motor_wheel_3, motor_steer_0, + motor_steer_1, motor_steer_2, motor_steer_3, &cmd, &power_control, &referee, + task_stack_depth, MakeChassisParam(param.chassis_param), param.pid_follow, + param.pid_velocity_x, param.pid_velocity_y, param.pid_omega, + param.pid_wheel_speed_0, param.pid_wheel_speed_1, param.pid_wheel_speed_2, + param.pid_wheel_speed_3, param.pid_steer_angle_0, param.pid_steer_angle_1, + param.pid_steer_angle_2, param.pid_steer_angle_3, param.pid_steer_speed_0, + param.pid_steer_speed_1, param.pid_steer_speed_2, param.pid_steer_speed_3, + param.thread_priority, + ChassisTopicNames{.chassis_cmd = param.chassis_cmd_topic_name, + .chassis_ref = param.chassis_ref_topic_name, + .gimbal_euler = param.gimbal_euler_topic_name}), referee_(&referee) { auto callback = LibXR::Callback::Create( @@ -313,6 +304,34 @@ class Chassis void EventHandler(uint32_t event_id) { chassis_.SetMode(event_id); } private: + static typename ChassisType::ChassisParam MakeChassisParam(const ChassisParam& param) + { + typename ChassisType::ChassisParam result{ + .wheel_radius = param.wheel_radius, + .wheel_to_center = param.wheel_to_center, + .gravity_height = param.gravity_height, + .reduction_ratio = param.reduction_ratio, + .wheel_resistance = param.wheel_resistance, + .error_compensation = param.error_compensation, + .gravity = param.gravity, + .length = param.length, + .width = param.width, + .rotor_speed_scale = param.rotor_speed_scale, + .rotor_omega_min_scale = param.rotor_omega_min_scale, + .rotor_buffer_low_j = param.rotor_buffer_low_j, + .rotor_buffer_high_j = param.rotor_buffer_high_j, + .rotor_scale_lpf_alpha = param.rotor_scale_lpf_alpha}; + if constexpr (std::is_same_v) + { + result.moment_of_inertia = param.moment_of_inertia; + result.friction_coefficient = param.friction_coefficient; + result.translation_acceleration_weight = param.translation_acceleration_weight; + result.angular_acceleration_weight = param.angular_acceleration_weight; + result.rotor_angular_acceleration_weight = param.rotor_angular_acceleration_weight; + } + return result; + } + ChassisType chassis_; LibXR::Event chassis_event_; Referee* referee_; diff --git a/Helm.hpp b/Helm.hpp index 93492e0..1d5d62f 100644 --- a/Helm.hpp +++ b/Helm.hpp @@ -12,6 +12,7 @@ depends: [] #include #include +#include "AccelerationConstraint.hpp" #include "CMD.hpp" #include "Chassis.hpp" #include "Motor.hpp" @@ -32,6 +33,14 @@ depends: [] #define HELM_CHASSIS_MAX_POWER 100 +struct ChassisVelocity +{ + float x = 0.0f; // m/s, +x right + float y = 0.0f; // m/s, +y forward + float omega = 0.0f; // rad/s, counterclockwise positive + bool valid = false; +}; + template class Chassis; @@ -76,6 +85,11 @@ class Helm ///< High buffer energy threshold (J) float rotor_scale_lpf_alpha = 0.2f; ///< 动态缩放的一阶低通系数 ///< First-order low-pass coefficient of the dynamic scale + float moment_of_inertia = 0.0f; ///< Yaw inertia (kg*m^2), zero disables feedforward + float friction_coefficient = 0.35f; + float translation_acceleration_weight = 1.0f; + float angular_acceleration_weight = 1.0f; + float rotor_angular_acceleration_weight = 4.0f; }; /** * @brief 底盘模式。 @@ -207,13 +221,11 @@ class Helm power_control_(power_control) { topic_names_ = topic_names; - UNUSED(referee); - - /* 舵轮底盘控制线程无条件访问四个舵向电机 */ ASSERT(motor_steer_0 != nullptr); ASSERT(motor_steer_1 != nullptr); ASSERT(motor_steer_2 != nullptr); ASSERT(motor_steer_3 != nullptr); + UNUSED(referee); for (int i = 0; i < 4; i++) { @@ -238,33 +250,41 @@ class Helm thread_.Create(this, ThreadFunction, "HelmChassisThread", task_stack_depth, thread_priority); + auto start_ctrl_callback = LibXR::Callback::Create( + [](bool in_isr, Helm* helm, uint32_t event_id) + { + UNUSED(in_isr); + UNUSED(event_id); + helm->mutex_.Lock(); + helm->chassis_event_ = ChassisMode::RELAX; + helm->mutex_.Unlock(); + }, + this); auto lost_ctrl_callback = LibXR::Callback::Create( [](bool in_isr, Helm* helm, uint32_t event_id) { UNUSED(in_isr); UNUSED(event_id); - helm->LostCtrl(); + helm->mutex_.Lock(); + helm->chassis_event_ = ChassisMode::RELAX; + helm->mutex_.Unlock(); }, this); + cmd_->GetEvent().Register(CMD::CMD_EVENT_START_CTRL, start_ctrl_callback); cmd_->GetEvent().Register(CMD::CMD_EVENT_LOST_CTRL, lost_ctrl_callback); } /** - * @brief 舵轮底盘控制线程函数。 - * Control thread function of the helm chassis. - * - * @details 线程主循环:接收控制命令,执行运动学解算并输出控制量。 - * Main loop: receives the control command, solves the kinematics and sends the - * outputs. - * - * @param helm Helm 对象指针。 - * Pointer to the Helm object. + * @brief 舵轮底盘控制线程函数 + * @param helm Helm对象指针 + * @details 控制线程主循环,负责接收控制指令、执行运动学解算和动力学控制输出 */ static void ThreadFunction(Helm* helm) { helm->mutex_.Lock(); - LibXR::Topic::ASyncSubscriber cmd_suber(helm->topic_names_.chassis_cmd); + LibXR::Topic::ASyncSubscriber cmd_suber( + helm->topic_names_.chassis_cmd); LibXR::Topic::ASyncSubscriber referee_suber( helm->topic_names_.chassis_ref); LibXR::Topic::ASyncSubscriber yawmotor_angle_suber("yawmotor_angle"); @@ -279,6 +299,7 @@ class Helm helm->mutex_.Unlock(); while (true) { + const auto LOOP_START = LibXR::Timebase::GetMicroseconds(); if (cmd_suber.Available()) { helm->cmd_data_ = cmd_suber.GetData(); @@ -303,39 +324,85 @@ class Helm helm->Update(); helm->UpdateCMD(); helm->topic_delta_yaw_.Publish(helm->delta_yaw_); - helm->Helmcontrol(); + helm->SelfResolution(); + helm->InverseKinematicsSolution(); + helm->OptimizeWheelAngle(); + helm->CompensateWheelSpeed(); + helm->DynamicInverseSolution(); + helm->CalculateMotorCurrent(); + const auto POWER_START = LibXR::Timebase::GetMicroseconds(); helm->PowerControlUpdate(); + helm->control_diagnostics_.power_control_time_us = ElapsedMicroseconds(POWER_START); + helm->control_diagnostics_.max_power_control_time_us = + std::max(helm->control_diagnostics_.max_power_control_time_us, + helm->control_diagnostics_.power_control_time_us); helm->mutex_.Unlock(); + const auto OUTPUT_START = LibXR::Timebase::GetMicroseconds(); helm->Output(); - helm->thread_.SleepUntil(last_time, 2); + auto& diagnostics = helm->control_diagnostics_; + diagnostics.output_time_us = ElapsedMicroseconds(OUTPUT_START); + diagnostics.max_output_time_us = + std::max(diagnostics.max_output_time_us, diagnostics.output_time_us); + ++diagnostics.loop_count; +#if defined(LIBXR_SYSTEM_freertos) && INCLUDE_uxTaskGetStackHighWaterMark == 1 + if ((diagnostics.loop_count & 255U) == 0U) + { + diagnostics.stack_min_free_bytes = + uxTaskGetStackHighWaterMark(nullptr) * sizeof(StackType_t); + } +#endif + diagnostics.loop_time_us = ElapsedMicroseconds(LOOP_START); + diagnostics.max_loop_time_us = + std::max(diagnostics.max_loop_time_us, diagnostics.loop_time_us); + if (diagnostics.loop_time_us >= CONTROL_PERIOD_US) + { + ++diagnostics.loop_overrun_count; + } + const auto NOW_MS = LibXR::Timebase::GetMilliseconds(); + if ((NOW_MS - last_time).ToMillisecond() >= CONTROL_PERIOD_MS) + { + // Skip missed releases instead of running back-to-back catch-up loops. + last_time = NOW_MS; + ++diagnostics.schedule_reset_count; + } + helm->thread_.SleepUntil(last_time, CONTROL_PERIOD_MS); } } /** - * @brief 更新电机状态。 - * Update the motor states. - * - * @details 记录采样间隔,更新四个驱动轮电机与四个舵向电机并读取反馈。 - * Records the sampling interval, updates the four wheel motors and the four - * steering motors and reads their feedback. + * @brief 更新电机状态 + * @details 获取当前时间戳并更新所有驱动轮电机的状态 */ void Update() { auto now = LibXR::Timebase::GetMicroseconds(); dt_ = (now - last_online_time_).ToSecondf(); + control_diagnostics_.period_us = + static_cast((now - last_online_time_).ToMicrosecond()); + control_diagnostics_.max_period_us = + std::max(control_diagnostics_.max_period_us, control_diagnostics_.period_us); + if (control_diagnostics_.period_us >= 3000U) + { + ++control_diagnostics_.period_over_3ms_count; + } last_online_time_ = now; + feedback_valid_ = true; for (int i = 0; i < 4; i++) { - motor_wheel_[i]->Update(); - motor_steer_[i]->Update(); + const auto WHEEL_STATUS = motor_wheel_[i]->Update(); + const auto STEER_STATUS = motor_steer_[i]->Update(); motor_wheel_feedback_[i] = motor_wheel_[i]->GetFeedback(); motor_steer_feedback_[i] = motor_steer_[i]->GetFeedback(); + feedback_valid_ = feedback_valid_ && WHEEL_STATUS == LibXR::ErrorCode::OK && + STEER_STATUS == LibXR::ErrorCode::OK && + motor_wheel_feedback_[i].state != 0 && + motor_steer_feedback_[i].state != 0; } } /** - * @brief 失去控制时使全部电机放松。 - * Relax all motors when control is lost. + * @brief 失去控制处理 + * */ void LostCtrl() { @@ -347,12 +414,8 @@ class Helm } /** - * @brief 设置底盘模式并复位速度、轮速与舵向 PID,由 Chassis 外壳调用。 - * Set the chassis mode and reset the velocity, wheel and steering PIDs; called - * by the Chassis shell. - * - * @param mode 新模式,取 ChassisMode 的值。 - * New mode, a value of ChassisMode. + * @brief 设置底盘模式 (由 Chassis 外壳调用) + * @param mode 要设置的新模式 */ void SetMode(uint32_t mode) { @@ -371,193 +434,454 @@ class Helm } /** - * @brief 从 CMD 命令更新目标速度。 - * Update the target velocities from the CMD command. + * @brief 更新底盘控制指令状态 + * @details 将归一化 CMD 输入转换为底盘目标速度,单位为 m/s 和 rad/s */ void UpdateCMD() { - target_vx_ = cmd_data_.x; - target_vy_ = cmd_data_.y; - target_omega_ = cmd_data_.z; + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + const float MAX_CHASSIS_OMEGA = GetMaxChassisOmega(); + if (chassis_event_ != ChassisMode::ROTOR) + { + rotor_ramped_vx_ = 0.0f; + rotor_ramped_vy_ = 0.0f; + } + + /* 先生成目标角速度。 */ + switch (chassis_event_) + { + case ChassisMode::RELAX: + target_omega_ = 0.0f; + break; + case ChassisMode::INDEPENDENT: + case ChassisMode::ROTOR: + target_omega_ = 0.7 * MAX_CHASSIS_OMEGA; + break; + case ChassisMode::FOLLOW: + target_omega_ = + -pid_follow_.Calculate(0.0f, current_yaw_, dt_) * MAX_CHASSIS_OMEGA; + /* 增加FOLLOW死区,避免不期望抖动 */ + { + const float BETA = LibXR::CycleValue(current_yaw_) - 0.0f; + constexpr float FOLLOW_DEADZONE = static_cast(M_PI) / 90.0f; + if (std::fabs(BETA) < FOLLOW_DEADZONE) + { + target_omega_ = 0.0f; + pid_follow_.Reset(); + } + else + { + target_omega_ = + -pid_follow_.Calculate(0.0f, current_yaw_, dt_) * MAX_CHASSIS_OMEGA; + } + } + break; + default: + target_omega_ = 0.0f; + break; + } + + /* 再生成目标平移速度,保持原有云台坐标变换。 */ + switch (chassis_event_) + { + case ChassisMode::RELAX: + target_vx_ = 0.0f; + target_vy_ = 0.0f; + break; + case ChassisMode::INDEPENDENT: + target_vx_ = cmd_data_.x * MAX_WHEEL_LINEAR_SPEED; + target_vy_ = cmd_data_.y * MAX_WHEEL_LINEAR_SPEED; + break; + case ChassisMode::FOLLOW: + case ChassisMode::ROTOR: + { + const float BETA = 2 * current_yaw_; + const float COS_BETA = cosf(BETA); + const float SIN_BETA = sinf(BETA); + target_vx_ = + (COS_BETA * cmd_data_.x - SIN_BETA * cmd_data_.y) * MAX_WHEEL_LINEAR_SPEED; + target_vy_ = + (SIN_BETA * cmd_data_.x + COS_BETA * cmd_data_.y) * MAX_WHEEL_LINEAR_SPEED; + /*限制平移速度来保持自旋*/ + if (chassis_event_ == ChassisMode::ROTOR) + { + constexpr float MAX_TRANSLATION_SPEED = 1.5f; + // constexpr float MAX_TRANSLATION_ACCELERATION = 1.0f; + const float REQUESTED_SPEED = hypotf(target_vx_, target_vy_); + if (REQUESTED_SPEED > MAX_TRANSLATION_SPEED) + { + const float SPEED_SCALE = MAX_TRANSLATION_SPEED / REQUESTED_SPEED; + target_vx_ *= SPEED_SCALE; + target_vy_ *= SPEED_SCALE; + } + + // const float DELTA_X = target_vx_ - rotor_ramped_vx_; + // const float DELTA_Y = target_vy_ - rotor_ramped_vy_; + // const float DELTA_SPEED = hypotf(DELTA_X, DELTA_Y); + // const float MAX_DELTA_SPEED = MAX_TRANSLATION_ACCELERATION * dt_; + // if (DELTA_SPEED > MAX_DELTA_SPEED && DELTA_SPEED > 0.0f) + // { + // const float RAMP_SCALE = MAX_DELTA_SPEED / DELTA_SPEED; + // target_vx_ = rotor_ramped_vx_ + DELTA_X * RAMP_SCALE; + // target_vy_ = rotor_ramped_vy_ + DELTA_Y * RAMP_SCALE; + // } + // rotor_ramped_vx_ = target_vx_; + // rotor_ramped_vy_ = target_vy_; + } + } + break; + default: + target_vx_ = 0.0f; + target_vy_ = 0.0f; + break; + } } /** - * @brief 更新功率控制:提交反馈与期望输出,计算功率上限并读取限幅结果。 - * Update power control: submit the feedback and the requested outputs, compute - * the power limit and read the limited result. + * @brief 功率控制更新 + * */ void PowerControlUpdate() { + constraint_output_valid_ = false; + constraint_infeasible_ = false; + constraint_pid_scale_ = 0.0f; + acceleration_constraint_result_ = {}; + control_diagnostics_.solver_attempts = 0; + control_diagnostics_.solve_time_us = 0; + control_diagnostics_.total_solve_time_us = 0; + control_diagnostics_.model_update_time_us = 0; + control_diagnostics_.requested_ax = target_ax_; + control_diagnostics_.requested_ay = target_ay_; + control_diagnostics_.requested_alpha = target_alpha_; + if (chassis_event_ == ChassisMode::RELAX || !feedback_valid_ || + !measured_chassis_velocity_.valid || !std::isfinite(dt_) || dt_ <= 0.0f || + !std::isfinite(target_vx_) || !std::isfinite(target_vy_) || + !std::isfinite(target_omega_)) + { + return; + } + + const auto MODEL_START = LibXR::Timebase::GetMicroseconds(); + // Feedback and command currents use the same configured motor direction. for (int i = 0; i < 4; i++) { motor_data_.rotorspeed_rpm_3508[i] = motor_wheel_feedback_[i].velocity; motor_data_.output_current_3508[i] = motor_wheel_feedback_[i].torque * M3508_NM_TO_LSB_RATIO; - - motor_data_.rotorspeed_rpm_6020[i] = motor_steer_feedback_[i].velocity; - motor_data_.output_current_6020[i] = - motor_steer_feedback_[i].torque * GM6020_NM_TO_LSB_RATIO; } - - /* 第一次设置: 反馈数据用于 RLS 参数估计 */ power_control_->SetMotorData3508(motor_data_.output_current_3508, motor_data_.rotorspeed_rpm_3508); - - power_control_->SetMotorData6020(motor_data_.output_current_6020, - motor_data_.rotorspeed_rpm_6020); - power_control_->CalculatePowerControlParam(); - - /* 计算 3508 速度跟踪误差 */ - float speed_error_3508[4]; - for (int i = 0; i < 4; i++) + const auto MODEL = power_control_->GetDrivePowerModel(); + control_diagnostics_.model_update_time_us = ElapsedMicroseconds(MODEL_START); + control_diagnostics_.max_model_update_time_us = + std::max(control_diagnostics_.max_model_update_time_us, + control_diagnostics_.model_update_time_us); + control_diagnostics_.power_model = MODEL; + if (!MODEL.valid || !std::isfinite(PARAM.friction_coefficient) || + PARAM.friction_coefficient <= 0.0f || !std::isfinite(PARAM.gravity) || + PARAM.gravity <= 0.0f || !std::isfinite(PARAM.wheel_radius) || + PARAM.wheel_radius <= 0.0f || !std::isfinite(PARAM.wheel_to_center) || + PARAM.wheel_to_center <= 0.0f || !std::isfinite(PARAM.moment_of_inertia) || + PARAM.moment_of_inertia < 0.0f || !std::isfinite(PARAM.reduction_ratio) || + PARAM.reduction_ratio <= 0.0f) { - float actual_speed = motor_reverse_[i] ? -motor_wheel_feedback_[i].velocity - : motor_wheel_feedback_[i].velocity; - speed_error_3508[i] = target_speed_[i] - actual_speed; + return; + } - motor_data_.output_current_3508[i] = - std::clamp(wheel_out_[i] * M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio, - -16384.0f, 16384.0f); - motor_data_.output_current_6020[i] = - std::clamp(steer_out_[i] * GM6020_NM_TO_LSB_RATIO, -16384.0f, 16384.0f); + const float TRANSLATION_REQUEST = hypotf(target_ax_, target_ay_); + constexpr float GRAVITY_ACCELERATION = 9.80665f; + const float CURRENT_PER_TORQUE = M3508_NM_TO_LSB_RATIO / PARAM.reduction_ratio; + // In addition to the document's diamond, include actual PID+FF drive traction. + const float TORQUE_LIMIT = std::min( + {4.4f, 16384.0f / CURRENT_PER_TORQUE, + PARAM.friction_coefficient * PARAM.gravity * PARAM.wheel_radius / 4.0f}); + const float CURRENT_LIMIT = TORQUE_LIMIT * CURRENT_PER_TORQUE; + AccelerationConstraint::Input input{}; + input.power_budget = GetChassisPowerBudget() - MODEL.static_loss - MODEL.margin; + control_diagnostics_.drive_power_budget = input.power_budget; + input.weights = { + TRANSLATION_REQUEST > 1.0e-6f ? PARAM.translation_acceleration_weight : 0.0f, + fabsf(target_alpha_) > 1.0e-6f ? (chassis_event_ == ChassisMode::ROTOR + ? PARAM.rotor_angular_acceleration_weight + : PARAM.angular_acceleration_weight) + : 0.0f}; + input.halfplanes[0] = { + TRANSLATION_REQUEST / (PARAM.friction_coefficient * GRAVITY_ACCELERATION), + fabsf(target_alpha_) * PARAM.moment_of_inertia / + (PARAM.friction_coefficient * PARAM.gravity * PARAM.wheel_to_center), + 1.0f}; + + // PID state is advanced only once by CalculateMotorCurrent. If its fixed + // offset makes the request box infeasible, retry with less PID contribution. + constexpr float PID_SCALES[] = {1.0f, 0.5f, 0.0f}; + for (float pid_scale : PID_SCALES) + { + input.halfplane_count = 1; + for (int i = 0; i < 4; i++) + { + auto& motor = input.motors[i]; + motor = {wheel_translation_ff_[i] * CURRENT_PER_TORQUE, + wheel_rotation_ff_[i] * CURRENT_PER_TORQUE, + pid_scale * wheel_pid_out_[i] * CURRENT_PER_TORQUE, + motor_data_.rotorspeed_rpm_3508[i], + MODEL.kt, + MODEL.k1, + MODEL.k2}; + input.halfplanes[input.halfplane_count++] = {motor.u, motor.v, + CURRENT_LIMIT - motor.c}; + input.halfplanes[input.halfplane_count++] = {-motor.u, -motor.v, + CURRENT_LIMIT + motor.c}; + } + const auto SOLVE_START = LibXR::Timebase::GetMicroseconds(); + acceleration_constraint_result_ = AccelerationConstraint::Solve(input); + control_diagnostics_.solve_time_us = ElapsedMicroseconds(SOLVE_START); + control_diagnostics_.max_solve_time_us = std::max( + control_diagnostics_.max_solve_time_us, control_diagnostics_.solve_time_us); + control_diagnostics_.total_solve_time_us += control_diagnostics_.solve_time_us; + ++control_diagnostics_.solver_attempts; + if (acceleration_constraint_result_.feasible) + { + constraint_pid_scale_ = pid_scale; + constraint_output_valid_ = true; + break; + } + } + if (!constraint_output_valid_) + { + constraint_infeasible_ = acceleration_constraint_result_.status == + AccelerationConstraint::Status::INFEASIBLE; + if (constraint_infeasible_) + { + ++control_diagnostics_.infeasible_count; + } + else + { + ++control_diagnostics_.invalid_solver_input_count; + } + // Zero drive current is the fallback, not a feasible optimum. Speed loss + // alone can exceed the budget. Keep valid steering control independent. + return; } - /* 计算 6020 舵向速度跟踪误差 */ - float speed_error_6020[4]; + const auto SCALE = acceleration_constraint_result_.point; + target_ax_ *= SCALE.x; + target_ay_ *= SCALE.x; + target_alpha_ *= SCALE.y; for (int i = 0; i < 4; i++) { - speed_error_6020[i] = steer_angle_[i] - motor_steer_feedback_[i].velocity * - static_cast(LibXR::TWO_PI) / - 60.0f; + target_motor_torque_ff_[i] = + SCALE.x * wheel_translation_ff_[i] + SCALE.y * wheel_rotation_ff_[i]; + wheel_out_[i] = + constraint_pid_scale_ * wheel_pid_out_[i] + target_motor_torque_ff_[i]; } + } - power_control_->SetMotorData3508(motor_data_.output_current_3508, - motor_data_.rotorspeed_rpm_3508, speed_error_3508); - power_control_->SetMotorData6020(motor_data_.output_current_6020, - motor_data_.rotorspeed_rpm_6020, speed_error_6020); - - auto now_ms = LibXR::Timebase::GetMilliseconds(); - bool referee_online = (now_ms - referee_last_rx_time_).ToSecondf() <= 1.0f; - bool power_control_online = power_control_->IsOnline(); - bool boost_mode = (cmd_data_.self_define == CMD::ChasStat::BOOST); - + float GetChassisPowerBudget() + { + const auto NOW_MS = LibXR::Timebase::GetMilliseconds(); + const bool REFEREE_ONLINE = (NOW_MS - referee_last_rx_time_).ToSecondf() <= 1.0f; float max_power = static_cast(referee_chassis_pack_.rs.chassis_power_limit); - if (!referee_online || max_power <= 1.0f) + if (!REFEREE_ONLINE || max_power <= 1.0f) { max_power = HELM_CHASSIS_MAX_POWER; } - - if (power_control_online && boost_mode) + if (power_control_->IsOnline() && cmd_data_.self_define == CMD::ChasStat::BOOST) { - float cap_energy = power_control_->GetCapEnergy(); - if (cap_energy > 0.8f) + // Preserve the existing board feedback interpretation and boost tiers. + const float CAP_LEVEL = power_control_->GetCapEnergy(); + if (CAP_LEVEL > 0.8f) { max_power += 100.0f; } - else if (cap_energy > 0.5f) + else if (CAP_LEVEL > 0.5f) { max_power += 70.0f; } - else if (cap_energy > 0.25f) + else if (CAP_LEVEL > 0.25f) { max_power += 40.0f; } } - - power_control_->OutputLimit(max_power); - power_control_data_ = power_control_->GetPowerControlData(); + return max_power; } /** - * @brief 按模式计算目标速度、舵角,以及轮与舵向电机的输出。 - * Compute the target velocity and steering angles for the current mode, and the - * outputs of the wheel and steering motors. + * @brief 舵轮底盘逆动力学解算 + * @details 车体速度 PID 输出加速度,分配轮端前馈力矩,单位 N*m */ - void Helmcontrol() + void DynamicInverseSolution() { - /* 计算目标平移速度 */ - switch (chassis_event_) + target_ax_ = 0.0f; + target_ay_ = 0.0f; + target_alpha_ = 0.0f; + for (int i = 0; i < 4; i++) { - case (ChassisMode::RELAX): - target_vx_ = 0.0f; - target_vy_ = 0.0f; - break; - case (ChassisMode::INDEPENDENT): + target_motor_torque_ff_[i] = 0.0f; + wheel_translation_ff_[i] = 0.0f; + wheel_rotation_ff_[i] = 0.0f; + } + + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + const float MAX_CHASSIS_OMEGA = GetMaxChassisOmega(); + if (chassis_event_ == ChassisMode::RELAX || + + !measured_chassis_velocity_.valid || !std::isfinite(dt_) || + !std::isfinite(target_vx_) || !std::isfinite(target_vy_) || + !std::isfinite(target_omega_) || dt_ <= 0.0f || + !std::isfinite(MAX_WHEEL_LINEAR_SPEED) || MAX_WHEEL_LINEAR_SPEED <= 0.0f || + !std::isfinite(MAX_CHASSIS_OMEGA) || MAX_CHASSIS_OMEGA <= 0.0f || + !std::isfinite(PARAM.gravity) || PARAM.gravity <= 0.0f || + !std::isfinite(PARAM.moment_of_inertia) || PARAM.moment_of_inertia < 0.0f) + { + pid_velocity_x_.Reset(); + pid_velocity_y_.Reset(); + pid_omega_.Reset(); + return; + } + + /* 零速目标时仍闭环底盘速度,以模型力矩制动残余运动。 */ + target_ax_ = pid_velocity_x_.Calculate(target_vx_, measured_chassis_velocity_.x, dt_); + target_ay_ = pid_velocity_y_.Calculate(target_vy_, measured_chassis_velocity_.y, dt_); + if (PARAM.moment_of_inertia > 0.0f) + { + target_alpha_ = + pid_omega_.Calculate(target_omega_, measured_chassis_velocity_.omega, dt_); + } + else + { + pid_omega_.Reset(); + } + if (!std::isfinite(target_ax_) || !std::isfinite(target_ay_) || + !std::isfinite(target_alpha_)) + { + target_ax_ = 0.0f; + target_ay_ = 0.0f; + target_alpha_ = 0.0f; + pid_velocity_x_.Reset(); + pid_velocity_y_.Reset(); + pid_omega_.Reset(); + return; + } + + constexpr float GRAVITY_ACCELERATION = 9.80665f; + const float MASS = PARAM.gravity / GRAVITY_ACCELERATION; + float sum_radius_squared = 0.0f; + for (int i = 0; i < 4; i++) + { + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + sum_radius_squared += + WHEEL_POSITION.x * WHEEL_POSITION.x + WHEEL_POSITION.y * WHEEL_POSITION.y; + } + if (!std::isfinite(sum_radius_squared) || sum_radius_squared <= 1.0e-12f) + { + return; + } + + /* 对称四轮的最小范数分配: + * Fx_i = m*ax/4 - J*alpha*y_i/S + * Fy_i = m*ay/4 + J*alpha*x_i/S,S = sum(x_i^2+y_i^2)。 */ + const float TRANSLATION_FORCE_X = MASS * target_ax_ / 4.0f; + const float TRANSLATION_FORCE_Y = MASS * target_ay_ / 4.0f; + const float ROTATION_FORCE_SCALE = + PARAM.moment_of_inertia * target_alpha_ / sum_radius_squared; + const bool ZERO_CHASSIS_COMMAND = fabsf(target_vx_) <= 1.0e-6f && + fabsf(target_vy_) <= 1.0e-6f && + fabsf(target_omega_) <= 1.0e-6f; + for (int i = 0; i < 4; i++) + { + /* 停车时舵角虽锁存,仍需在该实际方向施加制动力矩。 */ + if (steer_angle_held_[i] && !ZERO_CHASSIS_COMMAND) { - target_vx_ = cmd_data_.x; - target_vy_ = cmd_data_.y; + continue; } - break; - case (ChassisMode::FOLLOW): - case (ChassisMode::ROTOR): + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + const float STEER_ANGLE = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + /* 实际舵向的驱动正方向为 (sin(a), cos(a))。 + * 投影已包含方向,不再按 motor_reverse 取反或重复乘 cos^3。 */ + wheel_translation_ff_[i] = + PARAM.wheel_radius * (TRANSLATION_FORCE_X * sinf(STEER_ANGLE) + + TRANSLATION_FORCE_Y * cosf(STEER_ANGLE)); + wheel_rotation_ff_[i] = + PARAM.wheel_radius * ROTATION_FORCE_SCALE * + (-WHEEL_POSITION.y * sinf(STEER_ANGLE) + WHEEL_POSITION.x * cosf(STEER_ANGLE)); + const float TORQUE_FF = wheel_translation_ff_[i] + wheel_rotation_ff_[i]; + if (std::isfinite(TORQUE_FF)) { - float beta = current_yaw_; - float cos_beta = cosf(beta); - float sin_beta = sinf(beta); - target_vx_ = cos_beta * cmd_data_.x - sin_beta * cmd_data_.y; - target_vy_ = sin_beta * cmd_data_.x + cos_beta * cmd_data_.y; + target_motor_torque_ff_[i] = TORQUE_FF; } - break; - - default: - target_vx_ = 0.0f; - target_vy_ = 0.0f; - break; } + } - const float SQRT2 = 1.41421356237f; - /* 计算 wz */ - switch (chassis_event_) + /** + * @brief 舵轮底盘逆运动学解算 + * @details 根据底盘目标速度计算各轮目标舵角与电机转子转速 + */ + void InverseKinematicsSolution() + { + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + if (MAX_WHEEL_LINEAR_SPEED <= 1.0e-6f) { - case (ChassisMode::RELAX): - target_omega_ = 0.0f; - break; - case (ChassisMode::INDEPENDENT): - /* 独立模式每个轮子的方向相同,wz当作轮子转向角速度 */ - target_omega_ = -cmd_data_.z; - break; - case (ChassisMode::FOLLOW): - target_omega_ = pid_follow_.Calculate(0.0f, current_yaw_, dt_); - break; - case (ChassisMode::ROTOR): - /* 陀螺模式底盘以一定速度旋转 */ - target_omega_ = -cmd_data_.z; - break; - default: - target_omega_ = 0.0f; - break; + for (int i = 0; i < 4; i++) + { + target_speed_[i] = 0.0f; + target_angle_[i] = 0.0f; + steer_angle_held_[i] = false; + } + return; } + const float MOTOR_RPM_PER_MPS = motor_max_speed_ / MAX_WHEEL_LINEAR_SPEED; switch (chassis_event_) { case (ChassisMode::RELAX): for (int i = 0; i < 4; i++) { target_speed_[i] = 0.0f; - target_angle_[i] = 0.0; + target_angle_[i] = 0.0f; + steer_angle_held_[i] = false; } break; case (ChassisMode::INDEPENDENT): case (ChassisMode::FOLLOW): case (ChassisMode::ROTOR): { - float x = 0, y = 0, wheel_pos = 0; + float max_target_speed = 0.0f; for (int i = 0; i < 4; i++) { - wheel_pos = -static_cast(i) * static_cast(M_PI_2) + - static_cast(M_PI) / 4.0f * 3.0f; - x = -sinf(wheel_pos) * target_omega_ * SQRT2 + target_vx_; - y = -cosf(wheel_pos) * target_omega_ * SQRT2 + target_vy_; - - if (fabsf(x) < 0.05f && fabsf(y) < 0.05f) + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + const float WHEEL_VX = target_vx_ - target_omega_ * WHEEL_POSITION.y; + const float WHEEL_VY = target_vy_ + target_omega_ * WHEEL_POSITION.x; + const float WHEEL_VECTOR_SPEED = + sqrtf(WHEEL_VX * WHEEL_VX + WHEEL_VY * WHEEL_VY); + if (WHEEL_VECTOR_SPEED <= 1.0e-6f) { - target_angle_[i] = wheel_pos; + /* 仅轮速度近零时锁存舵角,避免零向量的目标舵角无定义。 */ + if (!steer_angle_held_[i]) + { + target_angle_[i] = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + steer_angle_held_[i] = true; + } target_speed_[i] = 0.0f; } else { - target_angle_[i] = M_PI_2 - atan2f(y, x); - target_speed_[i] = motor_max_speed_ * (fmaxf(fabsf(x), fabsf(y))); + steer_angle_held_[i] = false; + /* 沿用原有舵角零位:+y 为 0,正角朝 +x。 */ + target_angle_[i] = static_cast(M_PI_2) - atan2f(WHEEL_VY, WHEEL_VX); + target_speed_[i] = WHEEL_VECTOR_SPEED * MOTOR_RPM_PER_MPS; + max_target_speed = fmaxf(max_target_speed, target_speed_[i]); + } + } + /* 按最大轮速同比例缩放,保留四轮速度比例并限制至 RPM 上限。 */ + if (max_target_speed > motor_max_speed_) + { + const float SPEED_SCALE = motor_max_speed_ / max_target_speed; + for (int i = 0; i < 4; i++) + { + target_speed_[i] *= SPEED_SCALE; } } } @@ -566,14 +890,27 @@ class Helm for (int i = 0; i < 4; i++) { target_speed_[i] = 0.0f; - target_angle_[i] = 0.0; + target_angle_[i] = 0.0f; + steer_angle_held_[i] = false; } break; } + } - /* 舵向差超过 90 度时反转轮电机, 减少舵向转动 */ + /** + * @brief 最优舵角选择 + * @details 舵向差超过 90 度时反转驱动轮,减少舵向转动 + */ + void OptimizeWheelAngle() + { for (int i = 0; i < 4; i++) { + if (steer_angle_held_[i]) + { + /* 静止时保持锁存的物理舵角,不再做 180 度等价翻转。 */ + motor_reverse_[i] = false; + continue; + } if (fabs(LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]) - target_angle_[i]) > M_PI_2) { @@ -584,15 +921,64 @@ class Helm motor_reverse_[i] = false; } } + } + + /** + * @brief 舵向误差对应的驱动轮速度补偿 + */ + void CompensateWheelSpeed() + { + /* 最优角确定后按舵向误差抑制轮速,减少转舵时的侧向拖拽。 */ + for (int i = 0; i < 4; i++) + { + const LibXR::CycleValue CURRENT_ANGLE(motor_steer_feedback_[i].abs_angle - + zero_[i]); + const LibXR::CycleValue OPTIMAL_ANGLE( + target_angle_[i] + (motor_reverse_[i] ? static_cast(M_PI) : 0.0f)); + const float STEER_ERROR = OPTIMAL_ANGLE - CURRENT_ANGLE; + const float ALIGNMENT = fmaxf(0.0f, cosf(STEER_ERROR)); + target_speed_[i] *= ALIGNMENT * ALIGNMENT * ALIGNMENT; + } + } - /* 根据反转状态计算轮电机和舵向电机输出 */ + /** + * @brief 计算舵向和驱动轮 PID 输出 + */ + void CalculateMotorCurrent() + { + /* 轮速 PID 跟踪观测底盘速度在当前实际舵向上的投影。 */ + const float MAX_WHEEL_LINEAR_SPEED = GetMaxWheelLinearSpeed(); + const float MOTOR_RPM_PER_MPS = measured_chassis_velocity_.valid && + std::isfinite(MAX_WHEEL_LINEAR_SPEED) && + MAX_WHEEL_LINEAR_SPEED > 1.0e-6f + ? motor_max_speed_ / MAX_WHEEL_LINEAR_SPEED + : 0.0f; + /* 轮速目标的符号由实际舵向决定,反转状态只影响舵向控制。 */ for (int i = 0; i < 4; i++) { + float observed_motor_rpm = 0.0f; + if (MOTOR_RPM_PER_MPS > 0.0f) + { + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + const float STEER_ANGLE = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + const float WHEEL_VX = measured_chassis_velocity_.x - + measured_chassis_velocity_.omega * WHEEL_POSITION.y; + const float WHEEL_VY = measured_chassis_velocity_.y + + measured_chassis_velocity_.omega * WHEEL_POSITION.x; + observed_motor_rpm = + (WHEEL_VX * sinf(STEER_ANGLE) + WHEEL_VY * cosf(STEER_ANGLE)) * + MOTOR_RPM_PER_MPS; + if (!std::isfinite(observed_motor_rpm)) + { + observed_motor_rpm = 0.0f; + } + } + wheel_out_[i] = pid_wheel_speed_[i].Calculate( + observed_motor_rpm, motor_wheel_feedback_[i].velocity, dt_); + wheel_pid_out_[i] = wheel_out_[i]; if (motor_reverse_[i]) { - wheel_out_[i] = pid_wheel_speed_[i].Calculate( - -target_speed_[i], motor_wheel_feedback_[i].velocity, dt_); - steer_angle_[i] = pid_steer_angle_[i].Calculate( LibXR::CycleValue(target_angle_[i] + static_cast(M_PI) + zero_[i]), @@ -604,8 +990,6 @@ class Helm } else { - wheel_out_[i] = pid_wheel_speed_[i].Calculate( - target_speed_[i], motor_wheel_feedback_[i].velocity, dt_); steer_angle_[i] = pid_steer_angle_[i].Calculate( target_angle_[i] + zero_[i], motor_steer_feedback_[i].abs_angle, dt_); steer_out_[i] = pid_steer_speed_[i].Calculate( @@ -613,29 +997,25 @@ class Helm motor_steer_feedback_[i].velocity * static_cast(LibXR::TWO_PI) / 60.0f, dt_); } + /* 与轮速 PID 同为减速器输出轴扭矩 N*m,交给现有功率控制限幅。 */ + wheel_out_[i] += target_motor_torque_ff_[i]; } } /** - * @brief 向电机下发控制命令;RELAX 模式下使全部电机放松。 - * Send the control commands to the motors; all motors are relaxed in RELAX mode. + * @brief 输出控制命令到电机 + * */ void Output() { - if (power_control_data_.is_power_limited) + bool finite_output = true; + for (int i = 0; i < 4; i++) { - for (int i = 0; i < 4; i++) - { - wheel_out_[i] = std::clamp(power_control_data_.new_output_current_3508[i] / - M3508_NM_TO_LSB_RATIO * PARAM.reduction_ratio, - -4.4f, 4.4f); - steer_out_[i] = static_cast(std ::clamp( - power_control_data_.new_output_current_6020[i] / GM6020_NM_TO_LSB_RATIO * 1.0, - -2.5, 2.5)); - } + finite_output = finite_output && std::isfinite(steer_out_[i]) && + (!constraint_output_valid_ || std::isfinite(wheel_out_[i])); } - - if (chassis_event_ == (ChassisMode::RELAX)) + if (chassis_event_ == ChassisMode::RELAX || !feedback_valid_ || !finite_output || + (!constraint_output_valid_ && !constraint_infeasible_)) { LostCtrl(); } @@ -643,7 +1023,8 @@ class Helm { for (int i = 0; i < 4; i++) { - motor_wheel_cmd_[i].torque = std::clamp(wheel_out_[i], -4.4f, 4.4f); + motor_wheel_cmd_[i].torque = + constraint_output_valid_ ? std::clamp(wheel_out_[i], -4.4f, 4.4f) : 0.0f; motor_steer_cmd_[i].torque = std::clamp(steer_out_[i], -2.5f, 2.5f); } for (int i = 0; i < 4; i++) @@ -657,28 +1038,180 @@ class Helm private: const ChassisParam PARAM; + static constexpr uint32_t CONTROL_PERIOD_MS = 2; + static constexpr uint32_t CONTROL_PERIOD_US = CONTROL_PERIOD_MS * 1000; + + static uint32_t ElapsedMicroseconds(LibXR::MicrosecondTimestamp start) + { + return static_cast( + (LibXR::Timebase::GetMicroseconds() - start).ToMicrosecond()); + } + + // Instance diagnostics only: no logging or global sampling symbols in the loop. + // Work times include preemption and lock waits, but exclude scheduled sleep. + // period_us measures Update-to-Update, including sleep, just like dt_. + struct ControlDiagnostics + { + uint32_t loop_count = 0; + uint32_t loop_time_us = 0; + uint32_t max_loop_time_us = 0; + uint32_t period_us = 0; + uint32_t max_period_us = 0; + uint32_t period_over_3ms_count = 0; + uint32_t loop_overrun_count = 0; + uint32_t schedule_reset_count = 0; + uint32_t power_control_time_us = 0; + uint32_t max_power_control_time_us = 0; + uint32_t output_time_us = 0; + uint32_t max_output_time_us = 0; + uint32_t model_update_time_us = 0; + uint32_t max_model_update_time_us = 0; + uint32_t solve_time_us = 0; + uint32_t max_solve_time_us = 0; + uint32_t total_solve_time_us = 0; + uint32_t solver_attempts = 0; + uint32_t infeasible_count = 0; + uint32_t invalid_solver_input_count = 0; + uint32_t stack_min_free_bytes = 0; + float drive_power_budget = 0.0f; + float requested_ax = 0.0f; + float requested_ay = 0.0f; + float requested_alpha = 0.0f; + PowerControl::DrivePowerModel power_model{}; + }; + + ControlDiagnostics control_diagnostics_{}; + + struct WheelPosition + { + float x; + float y; + }; + + WheelPosition GetWheelPosition(uint8_t wheel_index) const + { + const float COMPONENT = PARAM.wheel_to_center * 0.70710678118f; + switch (wheel_index) + { + case 0: + return {-COMPONENT, -COMPONENT}; // Left rear, quadrant III + case 1: + return {COMPONENT, -COMPONENT}; // Right rear, quadrant IV + case 2: + return {COMPONENT, COMPONENT}; // Right front, quadrant I + case 3: + return {-COMPONENT, COMPONENT}; // Left front, quadrant II + default: + return {0.0f, 0.0f}; + } + } + + float GetMaxWheelLinearSpeed() const + { + if (PARAM.wheel_radius <= 1.0e-6f || PARAM.reduction_ratio <= 1.0e-6f || + motor_max_speed_ <= 1.0e-6f) + { + return 0.0f; + } + return motor_max_speed_ * static_cast(LibXR::TWO_PI) * PARAM.wheel_radius / + (60.0f * PARAM.reduction_ratio); + } + + float GetMaxChassisOmega() const + { + /* 纯旋转时轮心线速度为 |omega|*R,R 已是轮心到车体中心的距离。 */ + return PARAM.wheel_to_center > 1.0e-6f + ? GetMaxWheelLinearSpeed() / PARAM.wheel_to_center + : 0.0f; + } + + /** + * @brief 舵轮底盘正运动学解算 + * @details 根据四轮转速和舵角解算当前底盘速度 + */ + void SelfResolution() + { + measured_chassis_velocity_ = {}; + if (!feedback_valid_ || PARAM.wheel_to_center <= 0.0f || PARAM.wheel_radius <= 0.0f || + PARAM.reduction_ratio <= 0.0f) + { + return; + } + + const float RPM_TO_LINEAR_SPEED = static_cast(LibXR::TWO_PI) * + PARAM.wheel_radius / + (60.0f * PARAM.reduction_ratio); + float sum_vx = 0.0f; + float sum_vy = 0.0f; + float sum_rotation = 0.0f; + float sum_radius_squared = 0.0f; + for (int i = 0; i < 4; i++) + { + const WheelPosition WHEEL_POSITION = GetWheelPosition(static_cast(i)); + /* 舵向零位沿 +y,正角朝 +x;电机 reverse 已在反馈解码中处理。 */ + const float STEER_ANGLE = + LibXR::CycleValue(motor_steer_feedback_[i].abs_angle - zero_[i]); + const float WHEEL_SPEED = motor_wheel_feedback_[i].velocity * RPM_TO_LINEAR_SPEED; + if (!std::isfinite(STEER_ANGLE) || !std::isfinite(WHEEL_SPEED)) + { + return; + } + const float WHEEL_VX = WHEEL_SPEED * sinf(STEER_ANGLE); + const float WHEEL_VY = WHEEL_SPEED * cosf(STEER_ANGLE); + sum_vx += WHEEL_VX; + sum_vy += WHEEL_VY; + sum_rotation += WHEEL_POSITION.x * WHEEL_VY - WHEEL_POSITION.y * WHEEL_VX; + sum_radius_squared += + WHEEL_POSITION.x * WHEEL_POSITION.x + WHEEL_POSITION.y * WHEEL_POSITION.y; + } + if (sum_radius_squared <= 1.0e-12f) + { + return; + } + measured_chassis_velocity_.x = sum_vx / 4.0f; + measured_chassis_velocity_.y = sum_vy / 4.0f; + measured_chassis_velocity_.omega = sum_rotation / sum_radius_squared; + measured_chassis_velocity_.valid = std::isfinite(measured_chassis_velocity_.x) && + std::isfinite(measured_chassis_velocity_.y) && + std::isfinite(measured_chassis_velocity_.omega); + if (!measured_chassis_velocity_.valid) + { + measured_chassis_velocity_ = {}; + } + } + + ChassisVelocity measured_chassis_velocity_{}; + bool feedback_valid_ = false; + float rotor_ramped_vx_ = 0.0f; + float rotor_ramped_vy_ = 0.0f; float target_vx_ = 0.0f; float target_vy_ = 0.0f; float target_omega_ = 0.0f; + float target_ax_ = 0.0f; /* m/s^2 */ + float target_ay_ = 0.0f; /* m/s^2 */ + float target_alpha_ = 0.0f; /* rad/s^2 */ + float target_motor_torque_ff_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + float wheel_translation_ff_[4]{}; + float wheel_rotation_ff_[4]{}; + float wheel_pid_out_[4]{}; bool motor_reverse_[4]{false, false, false, false}; LibXR::CycleValue zero_[4] = {1.10063124, 2.05323339, 3.2244277, 0.979446769}; float current_yaw_ = 0.0f; float delta_yaw_ = 0.0f; - /* 目标轮速与目标舵角 */ + /* 驱动电机转子目标转速 RPM,舵角以 +y 为零位、单位 rad。 */ float target_speed_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; LibXR::CycleValue target_angle_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + bool steer_angle_held_[4] = {false, false, false, false}; - /* 轮与舵向电机的输出 */ + /* 输出的电流值 */ float wheel_out_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; float steer_out_[4] = {0.0f, 0.0f, 0.0f, 0.0f}; float steer_angle_[4] = {0.0, 0.0, 0.0, 0.0}; float motor_max_speed_ = 9000.0; - LibXR::CycleValue main_direct_ = 0.0f; - float dt_ = 0; LibXR::MicrosecondTimestamp last_online_time_ = 0; @@ -691,6 +1224,8 @@ class Helm Motor* motor_steer_2_; Motor* motor_steer_3_; + // Wheel order: 0=LB (QIII), 1=RB (QIV), 2=RF (QI), 3=LF (QII). + // Coordinates use +x right and +y forward; indices increase counterclockwise. Motor* motor_wheel_[4] = {motor_wheel_0_, motor_wheel_1_, motor_wheel_2_, motor_wheel_3_}; Motor* motor_steer_[4] = {motor_steer_0_, motor_steer_1_, motor_steer_2_, @@ -717,6 +1252,7 @@ class Helm LibXR::PID(LibXR::PID::Param()), LibXR::PID(LibXR::PID::Param())}; + ChassisTopicNames topic_names_{}; CMD* cmd_; CMD::ChassisCMD cmd_data_; @@ -725,11 +1261,13 @@ class Helm MotorData motor_data_ = {}; PowerControl* power_control_; - PowerControlData power_control_data_; + bool constraint_output_valid_ = false; + bool constraint_infeasible_ = false; + float constraint_pid_scale_ = 0.0f; + AccelerationConstraint::Result acceleration_constraint_result_{}; Referee::ChassisPack referee_chassis_pack_{}; LibXR::MillisecondTimestamp referee_last_rx_time_ = 0; - ChassisTopicNames topic_names_{}; LibXR::Thread thread_; LibXR::Mutex mutex_; diff --git a/HelmMigrationRecord.md b/HelmMigrationRecord.md new file mode 100644 index 0000000..bd6b394 --- /dev/null +++ b/HelmMigrationRecord.md @@ -0,0 +1,148 @@ +# Helm migration from luyan2 + +The source is the working tree of `D:/Documents/luyan2`, including its local module +changes. `User/xrobot.yaml` is converted to the current XRobot constructor/YAML +contracts. The older `User/RobotConfig/helm_infantry.yaml` in the source contains +only BlinkLED and is not the product configuration used for this migration. + +## Control and configuration + +- The control thread runs every 2 ms. Commands are normalized inputs; the Helm + controller converts them to m/s and rad/s using wheel radius, reduction ratio, + the 9000 RPM motor limit, and wheel-to-center distance. +- Coordinates are +x right, +y forward, and positive angular velocity + counterclockwise. Wheel indices are 0 left rear, 1 right rear, 2 right front, + 3 left front. Geometry assumes four symmetric 45-degree wheel positions. +- Steering zero is along +y, with positive steering angle toward +x. The existing + four zero offsets are retained. The wheel radius is 0.06 m, wheel-to-center + distance 0.233 m, reduction ratio 14, chassis weight 182.692 N, and yaw inertia + 0.49436791247 kg*m^2, matching the source product. +- The source forward/inverse kinematics, zero-speed steering hold, optimal-angle + reversal, cos-cubed speed compensation, chassis-velocity feedback, and wheel + torque feedforward are retained. Wheel-speed PID uses observed chassis velocity + projected along each actual steering direction, matching the source. +- INDEPENDENT and ROTOR use the joystick z command for target angular velocity. + FOLLOW uses a 2-degree deadband on the signed shortest yaw error: inside the + deadband, target angular velocity is zero and the follow PID is reset. Outside + it, normal following resumes. Kinematics and inverse dynamics share that target; + the deadband has no hysteresis. +- The current default configuration selects RELAX / FOLLOW / ROTOR from top to + bottom on the left switch. The right switch selects gimbal RELAX / COMMON / COMMON. +- Existing hardware registrations, CAN receive/transport implementation, other + module constructor interfaces, and configurable Topic names are retained. + +## Acceleration, traction and signed power constraints + +`AccelerationConstraint.hpp` follows sections 1.1.5 and 1.1.6 of the supplied +swerve-controller report: Sutherland-Hodgman half-plane clipping, feasible polygon +vertices, quadratic/segment intersections, and the power ellipse's objective +support point. Rank-zero/rank-one quadratics, point/line polygons, negative budgets, +and infeasible requests are handled without changing the physical quadratic. +The solver uses fixed arrays and no dynamic allocation. Its arithmetic is single +precision for the M4F hardware FPU. It first checks the full request, then the +polygon's objective-maximizing vertex, before enumerating power intersections and +support points. Thin ellipses use a rotated Gram matrix constructed from motor +current coefficients. Rounded boundary candidates are checked against the actual +float current/power expressions and, if needed, moved toward a known lower-power +feasible point without relaxing the power limit. + +Decision coordinates are retained fractions x,y in [0,1] of requested translation +and angular acceleration. Translation direction and angular acceleration sign are +preserved. The objective is translation_weight*x + angular_weight*y. Defaults are +1:1 in normal modes and 1:4 in ROTOR, a weighted preference rather than strict +lexicographic priority. YAML fields `translation_acceleration_weight`, +`angular_acceleration_weight` and `rotor_angular_acceleration_weight` are forwarded +only to Helm. Zero requested axes have zero objective weight. + +The document's equal-load traction diamond is +|a|/(mu*g) + |alpha|*J/(mu*weight*R) <= 1. The user measured mu approximately 0.4; +the requested conservative configured `friction_coefficient` is 0.35. Extra affine +half-planes bound each actual PID+FF wheel torque by the minimum of 4.4 N*m, +the 16384-LSB hardware limit, and mu*weight*wheel_radius/4. Thus the PID offset +cannot bypass the longitudinal drive traction bound. Normal load is assumed equal; +load transfer, lateral tire force and transient tire dynamics are not modeled. +The constraints are model-based and do not guarantee absence of real wheel slip. + +Motor speeds and wheel PID outputs are frozen once per cycle. Per-wheel current +is affine in x,y: I_i=u_i*x+v_i*y+c_i. Power is the SIGNED sum of +kt*I_i*rpm_i + k1*I_i^2 + k2*rpm_i^2. Regenerative negative power is not clipped or +counted twice. The constant PID loss term is k1*c_i^2. RMMotor already aligns +feedback torque with feedback RPM, so no extra polarity interface is used. + +PowerControl retains its existing RLS/legacy limiters, adding only a locked +GetDrivePowerModel() coefficient snapshot. Helm no longer invokes the legacy +positive-power limiter or replaces outputs with its result. Only four drive motors +are constrained; motor_count_6020 remains zero and steering keeps its PID output +and +/-2.5 N*m clamp. Static chassis loss and the existing 4 W control margin are +deducted from the available chassis budget before solving. + +Referee data older than one second, or a missing/invalid limit, selects +HELM_CHASSIS_MAX_POWER. Existing supercap policy is preserved: only online BOOST +adds 100/70/40 W when feedback exceeds 0.8/0.5/0.25 respectively. GetCapEnergy() +currently decodes an output-capability ratio, not a separately measured energy +in joules; this board-protocol interpretation still needs hardware confirmation. + +If full frozen PID makes the constrained request infeasible, solve again at PID +scales 0.5 and 0 without advancing PID state. If the solver still reports +INFEASIBLE, drive torque is zero while valid steering control continues, avoiding +relax/re-enable cycles of the steering motors. Invalid feedback, commands, timing, +model or solver input still relax all motors, as does RELAX. Zero drive current +is an explicit fallback, not a claim that impossible modeled power can be met: +the speed-loss term alone can exceed the budget. `acceleration_constraint_result_`, +`constraint_pid_scale_`, `constraint_output_valid_` and `constraint_infeasible_` +retain the result/status inside Helm for diagnosis. + +The control thread uses a 2 ms scheduled period. A missed release is discarded +and the schedule is restarted, avoiding back-to-back catch-up iterations. This +does not make an overlong computation meet its deadline. `Sleep(1)` would instead +add a tick delay to variable computation time and does not enforce a 2 ms period. + +`control_diagnostics_` is an instance member, with no fixed RAM globals or printing +in the control loop. Work durations use the existing microsecond timebase and +include interrupts, preemption and mutex waits. Inspect these on the MCU: + +| Member | Meaning | +| --- | --- | +| `period_us`, `max_period_us` | Update-to-Update interval, including sleep, matching `dt_` | +| `loop_time_us`, `max_loop_time_us` | Complete iteration work, excluding scheduled sleep | +| `loop_overrun_count` | Iterations with work lasting at least 2000 us | +| `period_over_3ms_count` | Update intervals lasting at least 3000 us | +| `power_control_time_us`, `max_power_control_time_us` | Entire power/traction update, including all retries | +| `model_update_time_us`, `max_model_update_time_us` | Motor-data submission, online identification and model snapshot | +| `solve_time_us`, `max_solve_time_us` | Last individual solve and maximum individual solve | +| `total_solve_time_us`, `solver_attempts` | Sum of solve times and attempt count in the latest iteration | +| `output_time_us`, `max_output_time_us` | Motor command submission, including any driver logging | +| `infeasible_count`, `invalid_solver_input_count` | Failed solver cycles, excluding early feedback/model rejection | +| `stack_min_free_bytes` | FreeRTOS stack high-water mark in bytes, sampled every 256 iterations when enabled | +| `drive_power_budget`, `power_model` | Budget after static loss/margin, and live coefficients | +| `requested_ax`, `requested_ay`, `requested_alpha` | Acceleration requests before constraint scaling | + +Watch maxima over repeated constraint activation and all three retry paths; +average solve duration cannot establish a 2 ms worst-case bound. If solver time +is short while iteration time is long, investigate other tasks, driver output +and logging. This driver can log on every command when a motor exceeds 75 C. + +Other product configurations retain their constructor contract after xrobot sync; +only Helm receives the new fields. No vendor code is changed. Module changes remain +local to their own repositories until explicitly committed, published and relocked. + +## Hardware validation still required + +1. With no projectiles and the wheels unloaded, check CAN IDs, wheel ordering, + reverse flags, steering zeros, reduction ratio and feedback units. Confirm + RELAX and loss of remote control remove motor torque. +2. Apply small +y, +x and pure rotation commands. Check wheel angle, drive sign, + and measured chassis velocity; verify FOLLOW decreases the yaw error. +3. Check steering hold at zero input and reduced wheel targets during steering + alignment. Check actual metered drive power and limited drive currents against + the drive budget. Steering power is outside that budget; verify the measurement + branch before using online identification. +4. Only after those checks, test on the floor at low speed. Observe braking, + mode transitions, feedback validity, current and power before raising limits. + +Before enabling higher limits, measure DWT cycle count and task stack high-water +mark on the MCU, including the worst-case three solver attempts, against the +2 ms control period. Compilation and host solver tests do not establish timing. + +Compilation does not validate physical direction, +power-meter coverage, PID tuning, or closed-loop stability on the robot. diff --git a/tests/acceleration_constraint_test.cpp b/tests/acceleration_constraint_test.cpp new file mode 100644 index 0000000..738f07c --- /dev/null +++ b/tests/acceleration_constraint_test.cpp @@ -0,0 +1,259 @@ +#include +#include +#include +#include +#include +#include + +#include "AccelerationConstraint.hpp" + +using AccelerationConstraint::Input; +using AccelerationConstraint::Result; +using AccelerationConstraint::Solve; +using AccelerationConstraint::Status; + +namespace +{ +void near(float actual, float expected, float tolerance = 1e-4f) +{ + assert(std::isfinite(actual)); + if (std::abs(actual - expected) > tolerance) + { + std::fprintf(stderr, "Expected %.9g, got %.9g\n", expected, actual); + assert(false); + } +} + +double power(const Input& input, double x, double y) +{ + double result = 0.0; + for (const auto& MOTOR : input.motors) + { + const double CURRENT = MOTOR.u * x + MOTOR.v * y + MOTOR.c; + result += MOTOR.k1 * CURRENT * CURRENT + + static_cast(MOTOR.kt) * MOTOR.rpm * CURRENT + + static_cast(MOTOR.k2) * MOTOR.rpm * MOTOR.rpm; + } + return result; +} + +void check_feasible(const Input& input, const Result& result) +{ + assert(result.feasible && result.status == Status::OPTIMAL); + assert(result.point.x >= 0.0f && result.point.x <= 1.0f); + assert(result.point.y >= 0.0f && result.point.y <= 1.0f); + assert(power(input, result.point.x, result.point.y) <= input.power_budget + 1e-4); + for (int i = 0; i < input.halfplane_count; ++i) + { + const auto& PLANE = input.halfplanes[i]; + assert(PLANE.x * result.point.x + PLANE.y * result.point.y <= PLANE.limit + 1e-5f); + } +} + +void deterministic() +{ + Input input; + input.motors[0].u = 1.0f; + input.motors[0].k1 = 1.0f; + input.motors[1].v = 1.0f; + input.motors[1].k1 = 1.0f; + input.power_budget = 0.5f; + Result result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.5f); + near(result.point.y, 0.5f); + + // A shifted ellipse has an interior support point rather than a polygon-edge optimum. + input.motors[0].c = -0.4f; + input.motors[1].c = -0.6f; + input.motors[1].v = 2.0f; + input.power_budget = 0.1f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.4f + std::sqrt(0.1f / 1.25f)); + near(result.point.y, 0.3f + 0.25f * std::sqrt(0.1f / 1.25f)); + + // Zero budget may leave exactly one interior feasible point. + input.power_budget = 0.0f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.4f); + near(result.point.y, 0.3f); + + // Rank-one strip, followed by the zero-width line degeneracy. + input.motors[1] = {}; + input.power_budget = 0.04f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.6f); + near(result.point.y, 1.0f); + input.power_budget = 0.0f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.4f); + near(result.point.y, 1.0f); + + // Rank-one parabola x^2 + y <= 0.5: its support is not on a polygon edge. + input = {}; + input.motors[0].u = 1.0f; + input.motors[0].k1 = 1.0f; + input.motors[1].v = 1.0f; + input.motors[1].rpm = 1.0f; + input.motors[1].kt = 1.0f; + input.power_budget = 0.5f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.5f); + near(result.point.y, 0.25f); + + // Signed braking power increases usable positive motor power, without abs/max0. + input = {}; + input.motors[0].u = 140.0f; + input.motors[0].rpm = 1.0f; + input.motors[0].kt = 1.0f; + input.motors[1].v = -40.0f; + input.motors[1].rpm = 1.0f; + input.motors[1].kt = 1.0f; + input.power_budget = 70.0f; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 110.0f / 140.0f); + near(result.point.y, 1.0f); + + // Linear clipping, angular preference and a disabled translation axis. + input = {}; + input.power_budget = 100.0f; + input.halfplanes[0] = {1.0f, 1.0f, 0.6f}; + input.halfplane_count = 1; + input.weights = {1.0f, 10.0f}; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.0f); + near(result.point.y, 0.6f); + input.halfplanes[1] = {1.0f, 0.0f, 0.0f}; + input.halfplane_count = 2; + result = Solve(input); + check_feasible(input, result); + near(result.point.x, 0.0f); + + // Infeasible power still returns the actual minimum-power point on the polygon. + input = {}; + input.motors[0].u = 1.0f; + input.motors[0].c = -0.3f; + input.motors[0].k1 = 1.0f; + input.motors[1].v = 1.0f; + input.motors[1].rpm = 1.0f; + input.motors[1].kt = 1.0f; + input.motors[2].rpm = 1.0f; + input.motors[2].k2 = 10.0f; + input.power_budget = 9.0f; + result = Solve(input); + assert(!result.feasible && result.minimum_available && + result.status == Status::INFEASIBLE); + near(result.point.x, 0.3f); + near(result.point.y, 0.0f); + near(result.power, 10.0f); + + // Empty geometric polygon must not pretend that the origin is a valid fallback. + input.halfplanes[0] = {0.0f, 0.0f, -1.0f}; + input.halfplane_count = 1; + result = Solve(input); + assert(!result.feasible && !result.minimum_available && + result.status == Status::INFEASIBLE); + + // Near-collinear motor vectors produce a thin interior ellipse. Forming + // a*c-b*b in float would erase its small but physically significant axis. + input = {}; + input.motors[0] = {10000.0f, 10000.0f, -10000.0f, 0.0f, 0.0f, 1.0f, 0.0f}; + input.motors[1] = {1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f}; + input.power_budget = 0.1f; + input.weights = {1.0f, 4.0f}; + result = Solve(input); + check_feasible(input, result); + const double SUPPORT = std::sqrt(0.1 / (6.25e-8 + 2.25)); + near(result.point.x, + static_cast((1.0 + 2.5e-8 * SUPPORT - 1.5 * SUPPORT) / 2.0)); + near(result.point.y, + static_cast((1.0 + 2.5e-8 * SUPPORT + 1.5 * SUPPORT) / 2.0)); + + input = {}; + input.power_budget = std::numeric_limits::quiet_NaN(); + assert(Solve(input).status == Status::INVALID_INPUT); + input.power_budget = 1.0f; + input.motors[0].k1 = -1.0f; + assert(Solve(input).status == Status::INVALID_INPUT); +} + +void randomized() +{ + std::mt19937 random(0xacc35U); + std::uniform_real_distribution signed_unit(-1.0f, 1.0f); + std::uniform_real_distribution unit(0.0f, 1.0f); + for (int trial = 0; trial < 1500; ++trial) + { + Input input; + for (auto& motor : input.motors) + { + motor.u = 5000.0f * signed_unit(random); + motor.v = 5000.0f * signed_unit(random); + motor.c = 1000.0f * signed_unit(random); + motor.rpm = 4000.0f * signed_unit(random); + motor.kt = 1.99688994e-6f; + motor.k1 = 2e-7f; + motor.k2 = 3e-7f; + } + input.power_budget = 60.0f * unit(random); + input.weights = {0.1f + unit(random), 0.1f + 5.0f * unit(random)}; + input.halfplanes[0] = {1.0f, 0.7f, 0.2f + 1.5f * unit(random)}; + input.halfplane_count = 1; + const Result RESULT = Solve(input); + if (RESULT.feasible) + { + check_feasible(input, RESULT); + } + double reference = -std::numeric_limits::infinity(); + constexpr int GRID = 80; + for (int ix = 0; ix <= GRID; ++ix) + { + for (int iy = 0; iy <= GRID; ++iy) + { + const double X = static_cast(ix) / GRID; + const double Y = static_cast(iy) / GRID; + if (X + 0.7 * Y <= input.halfplanes[0].limit && + power(input, X, Y) <= input.power_budget) + { + reference = std::max(reference, input.weights.x * X + input.weights.y * Y); + } + } + } + if (std::isfinite(reference)) + { + if (!RESULT.feasible) + { + std::fprintf(stderr, "False infeasible at trial %d\n", trial); + assert(false); + } + const double OBJECTIVE = + input.weights.x * RESULT.point.x + input.weights.y * RESULT.point.y; + if (OBJECTIVE + 2e-4 < reference) + { + std::fprintf(stderr, "Suboptimal trial %d: %.9g vs grid %.9g\n", trial, OBJECTIVE, + reference); + assert(false); + } + } + } +} +} // namespace + +int main() +{ + deterministic(); + const auto START = std::chrono::steady_clock::now(); + randomized(); + const auto STOP = std::chrono::steady_clock::now(); + std::printf( + "PASS: polygon/ellipse geometry, signed braking, degenerate quadratics, " + "infeasible fallback and 1500 dense-reference randomized cases (%.3f s).\n", + std::chrono::duration(STOP - START).count()); +} diff --git a/tests/helm_constraint_model_test.cpp b/tests/helm_constraint_model_test.cpp new file mode 100644 index 0000000..665fb57 --- /dev/null +++ b/tests/helm_constraint_model_test.cpp @@ -0,0 +1,411 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "AccelerationConstraint.hpp" + +namespace +{ +constexpr float GRAVITY = 9.80665f; +constexpr float WEIGHT = 182.692f; +constexpr float MASS = WEIGHT / GRAVITY; +constexpr float RADIUS = 0.06f; +constexpr float CENTER_RADIUS = 0.233f; +constexpr float INERTIA = 0.49436791247f; +constexpr float MU = 0.35f; +constexpr float REDUCTION = 14.0f; +constexpr float CURRENT_PER_TORQUE = 52437.5f / REDUCTION; +constexpr float KT = 1.99688994e-6f; +constexpr float K1 = 2.0e-7f; +constexpr float K2 = 3.0e-7f; +constexpr float PI = 3.14159265358979323846f; + +struct Scenario +{ + float ax = 0.0f; + float ay = 0.0f; + float alpha = 0.0f; + float inertia = INERTIA; + float power_budget = 100.0f - 5.5f - 4.0f; + float angular_weight = 1.0f; + std::array steer{}; + std::array pid{}; + std::array rpm{}; +}; + +struct Model +{ + AccelerationConstraint::Input input{}; + std::array translation{}; + std::array rotation{}; + float pid_scale = 1.0f; +}; + +void near(double actual, double expected, double tolerance = 1e-5) +{ + assert(std::isfinite(actual)); + if (std::abs(actual - expected) > tolerance) + { + std::fprintf(stderr, "Expected %.12g, got %.12g\n", expected, actual); + assert(false); + } +} + +float wheel_x(int index) +{ + return CENTER_RADIUS * (index == 0 || index == 3 ? -1.0f : 1.0f) / std::sqrt(2.0f); +} + +float wheel_y(int index) +{ + return CENTER_RADIUS * (index < 2 ? -1.0f : 1.0f) / std::sqrt(2.0f); +} + +float effective_alpha(const Scenario& scenario) +{ + return scenario.inertia > 0.0f ? scenario.alpha : 0.0f; +} + +float torque_limit() +{ + return std::min({4.4f, 16384.0f / CURRENT_PER_TORQUE, MU * WEIGHT * RADIUS / 4.0f}); +} + +// Physical scenario adapter independent of LibXR, motor drivers and RTOS objects. +Model make_model(const Scenario& scenario, float pid_scale) +{ + Model model; + model.pid_scale = pid_scale; + auto& input = model.input; + const float ALPHA = effective_alpha(scenario); + const float ACCELERATION = std::hypot(scenario.ax, scenario.ay); + input.power_budget = scenario.power_budget; + input.weights = {ACCELERATION > 1e-6f ? 1.0f : 0.0f, + std::abs(ALPHA) > 1e-6f ? scenario.angular_weight : 0.0f}; + input.halfplanes[input.halfplane_count++] = { + ACCELERATION / (MU * GRAVITY), + std::abs(ALPHA) * scenario.inertia / (MU * WEIGHT * CENTER_RADIUS), 1.0f}; + for (int i = 0; i < 4; ++i) + { + const float SIN = std::sin(scenario.steer[i]); + const float COS = std::cos(scenario.steer[i]); + model.translation[i] = RADIUS * MASS * (scenario.ax * SIN + scenario.ay * COS) / 4.0f; + model.rotation[i] = RADIUS * scenario.inertia * ALPHA * + (-wheel_y(i) * SIN + wheel_x(i) * COS) / + (4.0f * CENTER_RADIUS * CENTER_RADIUS); + auto& motor = input.motors[i]; + motor = {model.translation[i] * CURRENT_PER_TORQUE, + model.rotation[i] * CURRENT_PER_TORQUE, + pid_scale * scenario.pid[i] * CURRENT_PER_TORQUE, + scenario.rpm[i], + KT, + K1, + K2}; + const float CURRENT_LIMIT = torque_limit() * CURRENT_PER_TORQUE; + input.halfplanes[input.halfplane_count++] = {motor.u, motor.v, + CURRENT_LIMIT - motor.c}; + input.halfplanes[input.halfplane_count++] = {-motor.u, -motor.v, + CURRENT_LIMIT + motor.c}; + } + assert(input.halfplane_count == 9); + return model; +} + +// Reconstruct exactly the float torque/current path sent to Motor::Control. +float output_torque(const Scenario& scenario, const Model& model, int index, + float translation_fraction, float angular_fraction) +{ + const float FF = translation_fraction * model.translation[index] + + angular_fraction * model.rotation[index]; + return model.pid_scale * scenario.pid[index] + FF; +} + +double output_power(const Scenario& scenario, const Model& model, float x, float y, + bool clip_negative = false) +{ + double sum = 0.0; + for (int i = 0; i < 4; ++i) + { + const float CURRENT = output_torque(scenario, model, i, x, y) * CURRENT_PER_TORQUE; + const double POWER = static_cast(KT) * CURRENT * scenario.rpm[i] + + static_cast(K1) * CURRENT * CURRENT + + static_cast(K2) * scenario.rpm[i] * scenario.rpm[i]; + sum += clip_negative ? std::max(0.0, POWER) : POWER; + } + return sum; +} + +bool physically_feasible(const Scenario& scenario, const Model& model, float x, float y) +{ + const double TRANSLATION = std::hypot(scenario.ax, scenario.ay) * x; + const double ROTATION = std::abs(effective_alpha(scenario)) * y; + if (TRANSLATION / (MU * GRAVITY) + + ROTATION * scenario.inertia / (MU * WEIGHT * CENTER_RADIUS) > + 1.0) + { + return false; + } + for (int i = 0; i < 4; ++i) + { + if (std::abs(output_torque(scenario, model, i, x, y)) > torque_limit()) + { + return false; + } + } + return output_power(scenario, model, x, y) <= scenario.power_budget; +} + +void check_result(const Scenario& scenario, const Model& model, + const AccelerationConstraint::Result& result) +{ + assert(result.feasible); + const float X = result.point.x; + const float Y = result.point.y; + assert(X >= 0.0f && X <= 1.0f && Y >= 0.0f && Y <= 1.0f); + const double DIAMOND = std::hypot(scenario.ax, scenario.ay) * X / (MU * GRAVITY) + + std::abs(effective_alpha(scenario)) * Y * scenario.inertia / + (MU * WEIGHT * CENTER_RADIUS); + assert(DIAMOND <= 1.0 + 3e-6); + for (int i = 0; i < 4; ++i) + { + const float TORQUE = output_torque(scenario, model, i, X, Y); + assert(std::abs(TORQUE) <= torque_limit() + 5e-6f); + assert(std::abs(TORQUE * CURRENT_PER_TORQUE) <= 16384.0f + 0.02f); + const auto& MOTOR = model.input.motors[i]; + const float AFFINE_CURRENT = MOTOR.u * X + MOTOR.v * Y + MOTOR.c; + near(TORQUE * CURRENT_PER_TORQUE, AFFINE_CURRENT, 0.005); + } + // Separate reconstruction may differ slightly from the solver's affine + // current evaluation. + assert(output_power(scenario, model, X, Y) <= scenario.power_budget + 3e-4); +} + +void feedforward_equivalence() +{ + std::mt19937 random(0xff3508U); + std::uniform_real_distribution unit(-1.0f, 1.0f); + for (int trial = 0; trial < 5000; ++trial) + { + Scenario scenario; + scenario.ax = 8.0f * unit(random); + scenario.ay = 8.0f * unit(random); + scenario.alpha = 80.0f * unit(random); + for (float& angle : scenario.steer) + { + angle = PI * unit(random); + } + const Model MODEL = make_model(scenario, 1.0f); + for (int i = 0; i < 4; ++i) + { + // Original force allocation, projected only after forming its 2D force vector. + const double SCALE = + scenario.inertia * scenario.alpha / (4.0 * CENTER_RADIUS * CENTER_RADIUS); + const double FX = MASS * scenario.ax / 4.0 - SCALE * wheel_y(i); + const double FY = MASS * scenario.ay / 4.0 + SCALE * wheel_x(i); + const double OLD_TORQUE = + RADIUS * (FX * std::sin(scenario.steer[i]) + FY * std::cos(scenario.steer[i])); + near(MODEL.translation[i] + MODEL.rotation[i], OLD_TORQUE, 3e-6); + } + } +} + +void deterministic() +{ + Scenario scenario; + scenario.ay = 1.5f; + scenario.rpm = {5000.0f, -3000.0f, -3000.0f, -3000.0f}; + scenario.power_budget = 10.0f; + Model model = make_model(scenario, 1.0f); + auto result = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, result); + near(result.point.x, 1.0); + assert(output_power(scenario, model, result.point.x, result.point.y, true) > + scenario.power_budget); + + // A shared physical acceleration budget must favor rotation more under ROTOR weight. + scenario = {}; + scenario.ay = 3.0f; + scenario.alpha = 30.0f; + scenario.power_budget = 500.0f; + model = make_model(scenario, 1.0f); + const auto BALANCED = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, BALANCED); + scenario.angular_weight = 4.0f; + model = make_model(scenario, 1.0f); + const auto ROTOR = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, ROTOR); + assert(ROTOR.point.y > BALANCED.point.y + 0.5f); + assert(ROTOR.point.x < BALANCED.point.x); + + // Opposite angular requests must give opposite torques with symmetric budgets. + scenario = {}; + scenario.alpha = 40.0f; + model = make_model(scenario, 1.0f); + const auto POSITIVE = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, POSITIVE); + scenario.alpha = -40.0f; + const Model NEGATIVE_MODEL = make_model(scenario, 1.0f); + const auto NEGATIVE = AccelerationConstraint::Solve(NEGATIVE_MODEL.input); + check_result(scenario, NEGATIVE_MODEL, NEGATIVE); + near(POSITIVE.point.y, NEGATIVE.point.y); + for (int i = 0; i < 4; ++i) + { + near(output_torque(scenario, model, i, POSITIVE.point.x, POSITIVE.point.y), + -output_torque(scenario, NEGATIVE_MODEL, i, NEGATIVE.point.x, NEGATIVE.point.y)); + } + + // Zero axes and disabled yaw inertia leave no spurious rotational constraint/output. + scenario = {}; + model = make_model(scenario, 1.0f); + result = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, result); + assert(model.input.weights.x == 0.0f && model.input.weights.y == 0.0f); + scenario.ay = 2.0f; + scenario.alpha = 100.0f; + scenario.inertia = 0.0f; + model = make_model(scenario, 1.0f); + result = AccelerationConstraint::Solve(model.input); + check_result(scenario, model, result); + assert(model.input.weights.y == 0.0f && model.input.halfplanes[0].y == 0.0f); + for (float rotation : model.rotation) + { + near(rotation, 0.0); + } +} + +void pid_fallbacks() +{ + for (const float REQUESTED_PID : {0.2f, 1.2f, 2.5f}) + { + Scenario scenario; + scenario.pid.fill(REQUESTED_PID); + const auto FROZEN_PID = scenario.pid; + float accepted = -1.0f; + for (const float SCALE : {1.0f, 0.5f, 0.0f}) + { + const Model MODEL = make_model(scenario, SCALE); + const auto RESULT = AccelerationConstraint::Solve(MODEL.input); + assert(scenario.pid == FROZEN_PID); + if (RESULT.feasible) + { + check_result(scenario, MODEL, RESULT); + accepted = SCALE; + break; + } + } + near(accepted, REQUESTED_PID == 0.2f ? 1.0 : REQUESTED_PID == 1.2f ? 0.5 : 0.0); + } + Scenario impossible; + impossible.rpm.fill(9000.0f); + impossible.power_budget = 20.0f; + for (const float SCALE : {1.0f, 0.5f, 0.0f}) + { + const auto RESULT = + AccelerationConstraint::Solve(make_model(impossible, SCALE).input); + assert(!RESULT.feasible && + RESULT.status == AccelerationConstraint::Status::INFEASIBLE); + } +} + +void high_demand_saturation() +{ + // Large stick steps exercise the traction-limited vertex path and large + // affine coefficients, rather than only the small-command early return. + for (const float ACCELERATION : {20.0f, 80.0f, 200.0f}) + { + for (const float RPM : {0.0f, 3000.0f, -3000.0f}) + { + Scenario scenario; + scenario.ay = ACCELERATION; + scenario.rpm.fill(RPM); + scenario.power_budget = 90.5f; + const Model MODEL = make_model(scenario, 0.0f); + const auto RESULT = AccelerationConstraint::Solve(MODEL.input); + check_result(scenario, MODEL, RESULT); + assert(RESULT.point.x < 1.0f); + assert(RESULT.point.x > 0.0f); + // At rest traction alone sets the optimum; the retained acceleration + // must remain independent of the raw acceleration request's magnitude. + if (RPM == 0.0f) + { + near(ACCELERATION * RESULT.point.x, MU * GRAVITY, 3e-5); + } + } + } +} + +void random_physical_scenarios() +{ + std::mt19937 random(0xc0ffeeU); + std::uniform_real_distribution unit(-1.0f, 1.0f); + std::uniform_real_distribution positive(0.0f, 1.0f); + int feasible_count = 0; + for (int trial = 0; trial < 600; ++trial) + { + Scenario scenario; + scenario.ax = 8.0f * unit(random); + scenario.ay = 8.0f * unit(random); + scenario.alpha = 80.0f * unit(random); + scenario.angular_weight = trial % 2 ? 4.0f : 1.0f; + scenario.power_budget = 5.0f + 90.0f * positive(random); + for (int i = 0; i < 4; ++i) + { + scenario.steer[i] = PI * unit(random); + scenario.rpm[i] = 5000.0f * unit(random); + scenario.pid[i] = 2.0f * unit(random); + } + for (const float SCALE : {1.0f, 0.5f, 0.0f}) + { + const Model MODEL = make_model(scenario, SCALE); + const auto RESULT = AccelerationConstraint::Solve(MODEL.input); + double reference = -std::numeric_limits::infinity(); + constexpr int GRID = 40; + for (int ix = 0; ix <= GRID; ++ix) + { + for (int iy = 0; iy <= GRID; ++iy) + { + const float X = static_cast(ix) / GRID; + const float Y = static_cast(iy) / GRID; + if (physically_feasible(scenario, MODEL, X, Y)) + { + reference = + std::max(reference, static_cast(MODEL.input.weights.x) * X + + static_cast(MODEL.input.weights.y) * Y); + } + } + } + if (std::isfinite(reference)) + { + assert(RESULT.feasible); + const double OBJECTIVE = MODEL.input.weights.x * RESULT.point.x + + MODEL.input.weights.y * RESULT.point.y; + assert(OBJECTIVE >= reference - 3e-4); + } + if (RESULT.feasible) + { + check_result(scenario, MODEL, RESULT); + ++feasible_count; + break; + } + } + } + assert(feasible_count > 400); +} +} // namespace + +int main() +{ + feedforward_equivalence(); + deterministic(); + pid_fallbacks(); + high_demand_saturation(); + random_physical_scenarios(); + std::puts( + "PASS: 5000 FF equivalence cases, physical torque/current/grip/power bounds, " + "signed braking, +/- yaw, zero axes, J=0, ROTOR weighting, frozen PID " + "fallbacks and 600 dense-reference physical scenarios."); +}