refactor: enforce layered crates and Rust audit gates
This commit is contained in:
Horldsence
@@ -0,0 +1,19 @@
|
||||
[package]
|
||||
name = "pid"
|
||||
description = "PID controller"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "control/algorithm"
|
||||
@@ -0,0 +1,272 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! 通用 PID 控制器。
|
||||
|
||||
/// 并联式 PID 控制器（f32，显式时间步）。
|
||||
///
|
||||
/// 特性：
|
||||
/// - 输出限幅 `[out_min, out_max]`；
|
||||
/// - 积分抗饱和（clamping：输出已饱和且误差继续同向时暂停积分）；
|
||||
/// - 微分作用于**测量值**（避免设定值阶跃的微分冲击），可选一阶低通滤波。
|
||||
///
|
||||
/// 时间步 `dt` 由调用方显式传入（秒）。`dt <= 0` 或非有限值时本次调用
|
||||
/// 直接返回上次输出且不更新内部状态（防御跳帧 / 首帧场景）。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct Pid {
|
||||
kp: f32,
|
||||
ki: f32,
|
||||
kd: f32,
|
||||
out_min: f32,
|
||||
out_max: f32,
|
||||
/// 微分低通时间常数（秒）；`0` 关闭滤波。
|
||||
derivative_tau: f32,
|
||||
integral: f32,
|
||||
prev_measurement: Option<f32>,
|
||||
derivative_filtered: f32,
|
||||
last_output: f32,
|
||||
}
|
||||
|
||||
impl Pid {
|
||||
/// 创建 PID。`out_min` 必须小于 `out_max`。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// 参数非有限或 `out_min >= out_max` 时 panic。
|
||||
#[must_use]
|
||||
pub fn new(kp: f32, ki: f32, kd: f32, out_min: f32, out_max: f32) -> Self {
|
||||
assert!(
|
||||
out_min < out_max && [kp, ki, kd, out_min, out_max].iter().all(|v| v.is_finite()),
|
||||
"invalid PID parameters"
|
||||
);
|
||||
Self {
|
||||
kp,
|
||||
ki,
|
||||
kd,
|
||||
out_min,
|
||||
out_max,
|
||||
derivative_tau: 0.0,
|
||||
integral: 0.0,
|
||||
prev_measurement: None,
|
||||
derivative_filtered: 0.0,
|
||||
last_output: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 设置微分低通时间常数（秒），`0` 关闭滤波。链式调用。
|
||||
#[must_use]
|
||||
pub fn with_derivative_tau(mut self, tau: f32) -> Self {
|
||||
self.derivative_tau = tau.max(0.0);
|
||||
self
|
||||
}
|
||||
|
||||
/// 单步更新，返回限幅后的输出。
|
||||
pub fn update(&mut self, setpoint: f32, measurement: f32, dt: f32) -> f32 {
|
||||
// NaN 与 +inf 的比较结果均为 false，故 `dt > 0` 已隐含有限性。
|
||||
let input_valid = dt > 0.0 && setpoint.is_finite() && measurement.is_finite();
|
||||
if !input_valid {
|
||||
return self.last_output;
|
||||
}
|
||||
let error = setpoint - measurement;
|
||||
|
||||
// 微分作用于测量值（反向），一阶低通滤波。
|
||||
let d_raw = match self.prev_measurement {
|
||||
Some(prev) => -(measurement - prev) / dt,
|
||||
None => 0.0,
|
||||
};
|
||||
if self.derivative_tau > 0.0 {
|
||||
let alpha = dt / (self.derivative_tau + dt);
|
||||
self.derivative_filtered += alpha * (d_raw - self.derivative_filtered);
|
||||
} else {
|
||||
self.derivative_filtered = d_raw;
|
||||
}
|
||||
let d_term = self.kd * self.derivative_filtered;
|
||||
|
||||
// 积分抗饱和（clamping）：仅在不会把输出进一步推入饱和时积分。
|
||||
let i_candidate = self.integral + self.ki * error * dt;
|
||||
let unsat = self.kp * error + i_candidate + d_term;
|
||||
let saturating_up = unsat > self.out_max && error > 0.0;
|
||||
let saturating_down = unsat < self.out_min && error < 0.0;
|
||||
if !saturating_up && !saturating_down {
|
||||
self.integral = i_candidate;
|
||||
}
|
||||
|
||||
let output = (self.kp * error + self.integral + d_term).clamp(self.out_min, self.out_max);
|
||||
self.prev_measurement = Some(measurement);
|
||||
self.last_output = output;
|
||||
output
|
||||
}
|
||||
|
||||
/// 清空积分、微分记忆与上次输出。
|
||||
pub fn reset(&mut self) {
|
||||
self.integral = 0.0;
|
||||
self.prev_measurement = None;
|
||||
self.derivative_filtered = 0.0;
|
||||
self.last_output = 0.0;
|
||||
}
|
||||
|
||||
/// 上一次 `update` 的输出（诊断 / 串级外环取速度目标用）。
|
||||
#[must_use]
|
||||
pub fn last_output(&self) -> f32 {
|
||||
self.last_output
|
||||
}
|
||||
|
||||
/// 当前积分累加项。
|
||||
#[must_use]
|
||||
pub fn integral(&self) -> f32 {
|
||||
self.integral
|
||||
}
|
||||
|
||||
/// 比例增益。
|
||||
#[must_use]
|
||||
pub fn kp(&self) -> f32 {
|
||||
self.kp
|
||||
}
|
||||
|
||||
/// 积分增益。
|
||||
#[must_use]
|
||||
pub fn ki(&self) -> f32 {
|
||||
self.ki
|
||||
}
|
||||
|
||||
/// 微分增益。
|
||||
#[must_use]
|
||||
pub fn kd(&self) -> f32 {
|
||||
self.kd
|
||||
}
|
||||
|
||||
/// 输出下限。
|
||||
#[must_use]
|
||||
pub fn output_min(&self) -> f32 {
|
||||
self.out_min
|
||||
}
|
||||
|
||||
/// 输出上限。
|
||||
#[must_use]
|
||||
pub fn output_max(&self) -> f32 {
|
||||
self.out_max
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const DT: f32 = 0.001;
|
||||
|
||||
#[test]
|
||||
fn pure_proportional() {
|
||||
let mut pid = Pid::new(2.0, 0.0, 0.0, -100.0, 100.0);
|
||||
let out = pid.update(10.0, 4.0, DT);
|
||||
assert!((out - 12.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn output_clamps_both_ends() {
|
||||
let mut pid = Pid::new(100.0, 0.0, 0.0, -10.0, 10.0);
|
||||
assert!((pid.update(1.0, 0.0, DT) - 10.0).abs() < 1e-5);
|
||||
assert!((pid.update(-1.0, 0.0, DT) - (-10.0)).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn integral_accumulates() {
|
||||
let mut pid = Pid::new(0.0, 10.0, 0.0, -1e6, 1e6);
|
||||
pid.update(1.0, 0.0, DT); // integral += 10 * 1 * 0.001
|
||||
pid.update(1.0, 0.0, DT);
|
||||
let out = pid.update(1.0, 0.0, DT);
|
||||
let expected = 10.0 * 1.0 * DT * 3.0;
|
||||
assert!((out - expected).abs() < 1e-4, "out = {out}, expected {expected}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn anti_windup_bounds_integral_when_saturated() {
|
||||
// 大误差持续饱和 200 步：kp = 0 时输出即积分项。clamping 抗饱和要求
|
||||
// 积分不超过 out_max，且进入保持区后输出不再增长。
|
||||
let ki = 5.0;
|
||||
let mut pid = Pid::new(0.0, ki, 0.0, -16384.0, 16384.0);
|
||||
for _ in 0..200 {
|
||||
pid.update(1e6, 0.0, DT);
|
||||
}
|
||||
assert!(pid.integral() <= 16384.0, "integral = {}", pid.integral());
|
||||
let frozen = pid.update(1e6, 0.0, DT);
|
||||
let still = pid.update(1e6, 0.0, DT);
|
||||
assert!(
|
||||
(frozen - still).abs() < 1e-3,
|
||||
"saturated output must stop growing: {frozen} vs {still}"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn anti_windup_recovers_quickly_on_reverse() {
|
||||
// 饱和后目标反向，输出应在一个反向误差步内脱离正饱和。
|
||||
let mut pid = Pid::new(1.0, 10.0, 0.0, -100.0, 100.0);
|
||||
for _ in 0..500 {
|
||||
pid.update(1000.0, 0.0, DT);
|
||||
}
|
||||
assert!((pid.update(1000.0, 0.0, DT) - 100.0).abs() < 1e-3);
|
||||
// 一步反向大误差（kp=1, e=-1000）足以击穿下限，说明无深度饱和。
|
||||
let out = pid.update(-1000.0, 0.0, DT);
|
||||
assert!(out <= 0.0, "expected non-positive output, got {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn derivative_acts_on_measurement() {
|
||||
// 设定值恒 0，测量值阶跃：输出应为负向微分冲击（-kd*Δ/dt）。
|
||||
let mut pid = Pid::new(0.0, 0.0, 0.01, -1e6, 1e6);
|
||||
pid.update(0.0, 0.0, DT); // 建立初值，首帧微分为 0
|
||||
let out = pid.update(0.0, 0.5, DT);
|
||||
let expected = -0.01 * (0.5 / DT);
|
||||
assert!((out - expected).abs() < 1.0, "out = {out}, expected {expected}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn derivative_filter_softens_spike() {
|
||||
let step = |tau: f32| {
|
||||
let mut pid = Pid::new(0.0, 0.0, 10.0, -1e9, 1e9).with_derivative_tau(tau);
|
||||
pid.update(0.0, 0.0, DT);
|
||||
pid.update(0.0, 1.0, DT).abs()
|
||||
};
|
||||
let unfiltered = step(0.0);
|
||||
let filtered = step(0.05);
|
||||
assert!(
|
||||
filtered < unfiltered * 0.05,
|
||||
"filtered {filtered} should be far below unfiltered {unfiltered}"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nonpositive_dt_returns_last_output_and_keeps_state() {
|
||||
let mut pid = Pid::new(1.0, 1.0, 0.0, -100.0, 100.0);
|
||||
let first = pid.update(10.0, 0.0, DT);
|
||||
let integral_before = pid.integral().to_bits();
|
||||
// 逐位相等：无效输入必须原样返回上次输出。
|
||||
assert_eq!(pid.update(99.0, 0.0, 0.0).to_bits(), first.to_bits());
|
||||
assert_eq!(pid.update(99.0, 0.0, f32::NAN).to_bits(), first.to_bits());
|
||||
assert_eq!(pid.integral().to_bits(), integral_before);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nan_input_returns_last_output() {
|
||||
let mut pid = Pid::new(1.0, 0.0, 0.0, -100.0, 100.0);
|
||||
let first = pid.update(1.0, 0.0, DT);
|
||||
assert_eq!(pid.update(f32::NAN, 0.0, DT).to_bits(), first.to_bits());
|
||||
assert_eq!(pid.update(1.0, f32::INFINITY, DT).to_bits(), first.to_bits());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_history() {
|
||||
let mut pid = Pid::new(1.0, 10.0, 1.0, -1e6, 1e6);
|
||||
pid.update(5.0, 1.0, DT);
|
||||
pid.update(5.0, 1.0, DT);
|
||||
pid.reset();
|
||||
assert_eq!(pid.integral().to_bits(), 0.0f32.to_bits());
|
||||
// 重置后首帧微分为 0，输出 = P 项 + 本步积分（ki*e*dt = 0.04）。
|
||||
let out = pid.update(5.0, 1.0, DT);
|
||||
assert!((out - 4.04).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[should_panic = "invalid PID parameters"]
|
||||
fn new_rejects_inverted_limits() {
|
||||
let _pid = Pid::new(1.0, 0.0, 0.0, 1.0, -1.0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
[package]
|
||||
name = "mecanum"
|
||||
description = "Mecanum chassis kinematics"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
|
||||
[dev-dependencies]
|
||||
proptest = { workspace = true }
|
||||
approx = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "control/chassis"
|
||||
@@ -0,0 +1,286 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! 麦克纳姆轮底盘正 / 逆运动学。
|
||||
//!
|
||||
//! # 约定
|
||||
//!
|
||||
//! - 机器人坐标系（ISO）：x 前、y 左、ω 绕 z 逆时针为正；
|
||||
//! - 轮序固定为 `[前左, 前右, 后左, 后右]`（常量 [`FL`] / [`FR`] / [`RL`] /
|
||||
//! [`RR`]）；
|
||||
//! - 辊子 45°、俯视呈 X 型布置（RoboMaster 底盘标准布局）；
|
||||
//! - 角速度单位 rad/s，线速度 m/s，换算见 [`wheel_rpm_from_angular`]。
|
||||
//!
|
||||
//! 四轮线速度（`a = half_length + half_width`）：
|
||||
//!
|
||||
//! ```text
|
||||
//! v_FL = vx - vy - a·ω
|
||||
//! v_FR = vx + vy + a·ω
|
||||
//! v_RL = vx + vy - a·ω
|
||||
//! v_RR = vx - vy + a·ω
|
||||
//! ```
|
||||
//!
|
||||
//! 正解为逆解的左伪逆（JᵀJ 对角，可解析求平均）。
|
||||
|
||||
use std::f32::consts::TAU;
|
||||
|
||||
/// 前左轮下标。
|
||||
pub const FL: usize = 0;
|
||||
/// 前右轮下标。
|
||||
pub const FR: usize = 1;
|
||||
/// 后左轮下标。
|
||||
pub const RL: usize = 2;
|
||||
/// 后右轮下标。
|
||||
pub const RR: usize = 3;
|
||||
|
||||
/// 底盘平面速度：x 前进（m/s）、y 左移（m/s）、ω 逆时针（rad/s）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct Twist {
|
||||
/// 前进速度（m/s，前正）。
|
||||
pub vx: f32,
|
||||
/// 横移速度（m/s，左正）。
|
||||
pub vy: f32,
|
||||
/// 角速度（rad/s，逆时针正）。
|
||||
pub omega: f32,
|
||||
}
|
||||
|
||||
impl Twist {
|
||||
/// 全零速度。
|
||||
pub const ZERO: Self = Self { vx: 0.0, vy: 0.0, omega: 0.0 };
|
||||
|
||||
/// 幅值限幅：任一分量超界时按比例收缩整体，保持运动方向不变。
|
||||
///
|
||||
/// `max_forward` / `max_strafe` 单位 m/s，`max_spin` 单位 rad/s。
|
||||
#[must_use]
|
||||
pub fn clamped(self, max_forward: f32, max_strafe: f32, max_spin: f32) -> Self {
|
||||
let scale = [
|
||||
(self.vx.abs() / max_forward).max(f32::EPSILON),
|
||||
(self.vy.abs() / max_strafe).max(f32::EPSILON),
|
||||
(self.omega.abs() / max_spin).max(f32::EPSILON),
|
||||
]
|
||||
.into_iter()
|
||||
.fold(1.0f32, f32::max);
|
||||
Self { vx: self.vx / scale, vy: self.vy / scale, omega: self.omega / scale }
|
||||
}
|
||||
}
|
||||
|
||||
/// 麦轮底盘几何参数。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Mecanum {
|
||||
/// 前后轮轴线到底盘中心的纵向半距 lx（米，正）。
|
||||
pub half_length: f32,
|
||||
/// 左右轮到底盘中心的横向半距 ly（米，正）。
|
||||
pub half_width: f32,
|
||||
/// 车轮半径（米，正）。
|
||||
pub wheel_radius: f32,
|
||||
}
|
||||
|
||||
impl Mecanum {
|
||||
/// 按几何参数构造。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// debug 构建下参数非正时 panic（配置层应先行校验）。
|
||||
#[must_use]
|
||||
pub fn new(half_length: f32, half_width: f32, wheel_radius: f32) -> Self {
|
||||
debug_assert!(
|
||||
half_length > 0.0 && half_width > 0.0 && wheel_radius > 0.0,
|
||||
"mecanum geometry must be positive"
|
||||
);
|
||||
Self { half_length, half_width, wheel_radius }
|
||||
}
|
||||
|
||||
/// 逆解：底盘速度 → 四轮角速度（rad/s，顺序 FL/FR/RL/RR）。
|
||||
#[must_use]
|
||||
pub fn wheel_speeds(&self, twist: &Twist) -> [f32; 4] {
|
||||
let a = self.half_length + self.half_width;
|
||||
[
|
||||
twist.vx - twist.vy - a * twist.omega,
|
||||
twist.vx + twist.vy + a * twist.omega,
|
||||
twist.vx + twist.vy - a * twist.omega,
|
||||
twist.vx - twist.vy + a * twist.omega,
|
||||
]
|
||||
.map(|v| v / self.wheel_radius)
|
||||
}
|
||||
|
||||
/// 正解：四轮角速度（rad/s）→ 底盘速度（最小二乘意义）。
|
||||
#[must_use]
|
||||
pub fn twist(&self, wheels: &[f32; 4]) -> Twist {
|
||||
let w = wheels.map(|x| x * self.wheel_radius);
|
||||
let a = self.half_length + self.half_width;
|
||||
Twist {
|
||||
vx: (w[FL] + w[FR] + w[RL] + w[RR]) / 4.0,
|
||||
vy: (-w[FL] + w[FR] + w[RL] - w[RR]) / 4.0,
|
||||
omega: (-w[FL] + w[FR] - w[RL] + w[RR]) / (4.0 * a),
|
||||
}
|
||||
}
|
||||
|
||||
/// 逆解并直接输出四轮转速（rpm）。
|
||||
#[must_use]
|
||||
pub fn wheel_rpms(&self, twist: &Twist) -> [f32; 4] {
|
||||
self.wheel_speeds(twist).map(wheel_rpm_from_angular)
|
||||
}
|
||||
|
||||
/// 由四轮转速（rpm）正解底盘速度。
|
||||
#[must_use]
|
||||
pub fn twist_from_wheel_rpms(&self, wheel_rpms: &[f32; 4]) -> Twist {
|
||||
self.twist(&wheel_rpms.map(wheel_angular_from_rpm))
|
||||
}
|
||||
}
|
||||
|
||||
/// 轮角速度（rad/s）→ 转速（rpm）。
|
||||
#[must_use]
|
||||
pub const fn wheel_rpm_from_angular(rad_per_s: f32) -> f32 {
|
||||
rad_per_s * 60.0 / TAU
|
||||
}
|
||||
|
||||
/// 轮转速（rpm）→ 角速度（rad/s）。
|
||||
#[must_use]
|
||||
pub const fn wheel_angular_from_rpm(rpm: f32) -> f32 {
|
||||
rpm * TAU / 60.0
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use approx::assert_relative_eq;
|
||||
|
||||
/// 0.2m 半轴距 / 0.25m 半轮距 / 76.2mm 轮半径的典型 RM 底盘。
|
||||
fn chassis() -> Mecanum {
|
||||
Mecanum::new(0.2, 0.25, 0.0762)
|
||||
}
|
||||
|
||||
fn assert_twist_eq(a: &Twist, b: &Twist) {
|
||||
assert_relative_eq!(a.vx, b.vx, epsilon = 1e-5);
|
||||
assert_relative_eq!(a.vy, b.vy, epsilon = 1e-5);
|
||||
assert_relative_eq!(a.omega, b.omega, epsilon = 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn pure_forward_drives_all_wheels_equally() {
|
||||
let m = chassis();
|
||||
let w = m.wheel_speeds(&Twist { vx: 1.0, ..Twist::ZERO });
|
||||
for x in w {
|
||||
assert_relative_eq!(x, 1.0 / 0.0762, epsilon = 1e-5);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn pure_strafe_left_opposes_diagonal_wheels() {
|
||||
let m = chassis();
|
||||
let w = m.wheel_speeds(&Twist { vy: 2.0, ..Twist::ZERO });
|
||||
let v = 2.0 / 0.0762;
|
||||
assert_relative_eq!(w[FL], -v, epsilon = 1e-5);
|
||||
assert_relative_eq!(w[FR], v, epsilon = 1e-5);
|
||||
assert_relative_eq!(w[RL], v, epsilon = 1e-5);
|
||||
assert_relative_eq!(w[RR], -v, epsilon = 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn pure_rotation_splits_left_right() {
|
||||
let m = chassis();
|
||||
let w = m.wheel_speeds(&Twist { omega: 1.0, ..Twist::ZERO });
|
||||
let v = (0.2 + 0.25) / 0.0762;
|
||||
// 逆时针：左侧后退、右侧前进。
|
||||
assert_relative_eq!(w[FL], -v, epsilon = 1e-5);
|
||||
assert_relative_eq!(w[FR], v, epsilon = 1e-5);
|
||||
assert_relative_eq!(w[RL], -v, epsilon = 1e-5);
|
||||
assert_relative_eq!(w[RR], v, epsilon = 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn diagonal_motion_idles_two_wheels() {
|
||||
// 前左 45° 对角线：vx = vy ⇒ FL、RR 停转。
|
||||
let m = chassis();
|
||||
let w = m.wheel_speeds(&Twist { vx: 1.0, vy: 1.0, omega: 0.0 });
|
||||
assert_relative_eq!(w[FL], 0.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(w[RR], 0.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(w[FR], 2.0 / 0.0762, epsilon = 1e-5);
|
||||
assert_relative_eq!(w[RL], 2.0 / 0.0762, epsilon = 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_twist_zero_wheels() {
|
||||
let wheels = chassis().wheel_speeds(&Twist::ZERO);
|
||||
assert!(wheels.iter().all(|w| f64::from(*w).abs() < 1e-12), "{wheels:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn inverse_then_forward_recovers_twist() {
|
||||
let m = chassis();
|
||||
let t = Twist { vx: 1.5, vy: -0.8, omega: 2.2 };
|
||||
assert_twist_eq(&m.twist(&m.wheel_speeds(&t)), &t);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rpm_helpers_roundtrip() {
|
||||
assert_relative_eq!(
|
||||
wheel_rpm_from_angular(wheel_angular_from_rpm(482.0)),
|
||||
482.0,
|
||||
epsilon = 1e-4
|
||||
);
|
||||
assert_relative_eq!(wheel_angular_from_rpm(60.0 / TAU), 1.0, epsilon = 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rpm_path_matches_angular_path() {
|
||||
let m = chassis();
|
||||
let t = Twist { vx: 0.3, vy: 1.1, omega: -0.7 };
|
||||
assert_twist_eq(&m.twist_from_wheel_rpms(&m.wheel_rpms(&t)), &t);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn clamp_preserves_direction_and_limits() {
|
||||
let t = Twist { vx: 4.0, vy: 1.0, omega: 0.5 }.clamped(2.0, 2.0, 3.0);
|
||||
assert_relative_eq!(t.vx, 2.0, epsilon = 1e-6);
|
||||
assert_relative_eq!(t.vy, 0.5, epsilon = 1e-6);
|
||||
assert_relative_eq!(t.omega, 0.25, epsilon = 1e-6);
|
||||
// 限内不缩放。
|
||||
let keep = Twist { vx: 1.0, vy: -1.0, omega: 2.0 }.clamped(2.0, 2.0, 3.0);
|
||||
assert_eq!(keep, Twist { vx: 1.0, vy: -1.0, omega: 2.0 });
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod proptests {
|
||||
use super::*;
|
||||
use approx::assert_relative_eq;
|
||||
use proptest::prelude::*;
|
||||
|
||||
proptest! {
|
||||
/// 正逆解互逆：forward(inverse(t)) == t（对任意合理几何与速度）。
|
||||
#[test]
|
||||
fn kinematics_roundtrip(
|
||||
lx in 0.05f32..0.5,
|
||||
ly in 0.05f32..0.5,
|
||||
r in 0.03f32..0.2,
|
||||
vx in -3.0f32..3.0,
|
||||
vy in -3.0f32..3.0,
|
||||
omega in -6.0f32..6.0,
|
||||
) {
|
||||
let m = Mecanum::new(lx, ly, r);
|
||||
let t = Twist { vx, vy, omega };
|
||||
let back = m.twist(&m.wheel_speeds(&t));
|
||||
assert_relative_eq!(back.vx, t.vx, epsilon = 1e-4);
|
||||
assert_relative_eq!(back.vy, t.vy, epsilon = 1e-4);
|
||||
assert_relative_eq!(back.omega, t.omega, epsilon = 1e-4);
|
||||
}
|
||||
|
||||
/// 左右镜像对称：vy、ω 取反（镜像底盘）等价于 FL↔FR、RL↔RR 对调。
|
||||
#[test]
|
||||
fn strafe_symmetry(
|
||||
vx in -2.0f32..2.0,
|
||||
vy in -2.0f32..2.0,
|
||||
omega in -4.0f32..4.0,
|
||||
) {
|
||||
let m = Mecanum::new(0.2, 0.25, 0.0762);
|
||||
let t = Twist { vx, vy, omega };
|
||||
let mirrored = Twist { vx, vy: -vy, omega: -omega };
|
||||
let w = m.wheel_speeds(&t);
|
||||
let w2 = m.wheel_speeds(&mirrored);
|
||||
assert_relative_eq!(w[FL], w2[FR], epsilon = 1e-4);
|
||||
assert_relative_eq!(w[FR], w2[FL], epsilon = 1e-4);
|
||||
assert_relative_eq!(w[RL], w2[RR], epsilon = 1e-4);
|
||||
assert_relative_eq!(w[RR], w2[RL], epsilon = 1e-4);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
[package]
|
||||
name = "motor-control"
|
||||
description = "Motor velocity, position, cascade, and feedforward control"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
pid = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
|
||||
[package.metadata.sentry-chassis]
|
||||
layer = "control/motor"
|
||||
@@ -0,0 +1,798 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod feedforward {
|
||||
//! 前馈补偿（ARCHITECTURE.md §5.5）。
|
||||
//!
|
||||
//! 速度环的前馈是**比例前馈**：稳态下所需控制量与目标转速近似成正比，
|
||||
//! 因此 `u_ff = k_ff · setpoint` 可以直接把积分项从"慢慢顶"变成"一开始就到位"。
|
||||
//!
|
||||
//! # 为什么前馈在空载台架上必须为 0
|
||||
//!
|
||||
//! `k_ff ≈ 1/K`，其中 `K` 是被控对象的稳态增益（GM3508 空载实测
|
||||
//! `K ≈ 0.57 rpm/raw` → `k_ff ≈ 1.77`）。空载时维持转速只需要克服摩擦的极小
|
||||
//! 电流，任何接近 1.77 的前馈都会把轮子直接推到最高转速（见 `docs/chassis.md` §7.2）。
|
||||
|
||||
/// 比例前馈：`输出 = k_ff · setpoint`。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct ProportionalFeedforward {
|
||||
/// 前馈系数（控制量 / 目标量）。
|
||||
pub k_ff: f32,
|
||||
}
|
||||
|
||||
impl ProportionalFeedforward {
|
||||
/// 构造。
|
||||
#[must_use]
|
||||
pub const fn new(k_ff: f32) -> Self {
|
||||
Self { k_ff }
|
||||
}
|
||||
|
||||
/// 由被控对象稳态增益换算：`k_ff = 1 / K`。
|
||||
///
|
||||
/// `K` 非有限或 ≤ 0 时返回 0（宁可不前馈，也不要一个发散的系数）。
|
||||
#[must_use]
|
||||
pub fn from_plant_gain(plant_gain: f32) -> Self {
|
||||
if plant_gain.is_finite() && plant_gain > 0.0 {
|
||||
Self { k_ff: 1.0 / plant_gain }
|
||||
} else {
|
||||
Self { k_ff: 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
/// 计算前馈量。
|
||||
#[must_use]
|
||||
pub fn compute(&self, setpoint: f32) -> f32 {
|
||||
if setpoint.is_finite() { self.k_ff * setpoint } else { 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn inverse_plant_gain_matches_the_documented_bench_feedforward() {
|
||||
// GM3508 空载 K ≈ 0.5648 rpm/raw → k_ff ≈ 1.77 raw/rpm
|
||||
let feedforward = ProportionalFeedforward::from_plant_gain(9254.0 / 16384.0);
|
||||
assert!((feedforward.k_ff - 1.77).abs() < 0.02, "{}", feedforward.k_ff);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn degenerate_plant_gain_yields_no_feedforward() {
|
||||
assert!((ProportionalFeedforward::from_plant_gain(0.0).k_ff).abs() < 1e-6);
|
||||
assert!((ProportionalFeedforward::from_plant_gain(-1.0).k_ff).abs() < 1e-6);
|
||||
assert!((ProportionalFeedforward::from_plant_gain(f32::NAN).k_ff).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn compute_is_linear_and_safe() {
|
||||
let feedforward = ProportionalFeedforward::new(2.0);
|
||||
assert!((feedforward.compute(3.0) - 6.0).abs() < 1e-6);
|
||||
assert!((feedforward.compute(f32::INFINITY)).abs() < 1e-6);
|
||||
assert!((ProportionalFeedforward::default().compute(100.0)).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use feedforward::*;
|
||||
|
||||
pub mod plant {
|
||||
//! 被控对象抽象：让**仿真与实机共用同一套 PID**（ARCHITECTURE.md §5.5）。
|
||||
//!
|
||||
//! 这是整定能否迁移到实机的关键 seam：整定器只认识 [`ControlPlant`]，
|
||||
//! 因此"在仿真里搜出来的增益"与"在台架上验出来的增益"评估的是同一条接口。
|
||||
//!
|
||||
//! 实现方：
|
||||
//! - 仿真：`sim-harness::SimPlant`（浮点积分，无 IO，可并行上千次）；
|
||||
//! - 实机/台架：`bench` 的台架采样（每次试验 = 真机跑一条场景）。
|
||||
|
||||
/// 单输入单输出的被控对象。
|
||||
///
|
||||
/// 约定：`step` 推进 `dt` 秒并返回**本次的测量输出**；输入是控制量
|
||||
/// （`motor-control` 里即 effort raw）。实现必须保证 `dt <= 0` 时不推进状态。
|
||||
pub trait ControlPlant {
|
||||
/// 推进一拍的误差类型（平台相关实现可以自定义）。
|
||||
type Error;
|
||||
|
||||
/// 推进一步，返回测量输出。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 数值发散或输入非法时返回错误，由调用方折算成硬约束罚值。
|
||||
fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error>;
|
||||
|
||||
/// 复位到初始状态（两次试验之间必须调用，否则结果依赖历史）。
|
||||
fn reset(&mut self);
|
||||
|
||||
/// 人类可读描述（写进报告，便于复现实验条件）。
|
||||
fn describe(&self) -> String;
|
||||
}
|
||||
|
||||
/// 无错误的被控对象（纯代数模型 / 已在内部处理发散的实现）用它的适配器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub struct Infallible;
|
||||
|
||||
impl core::fmt::Display for Infallible {
|
||||
fn fmt(&self, _f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl core::error::Error for Infallible {}
|
||||
|
||||
/// 一阶惯性环节：`y' = (k·u − y) / tau`，显式欧拉。
|
||||
///
|
||||
/// 这是最小可用的 [`ControlPlant`] 实现，用来给控制律 / 整定器做单元测试；
|
||||
/// 带摩擦与延迟的完整电机模型在 `sim-harness`。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct FirstOrderPlant {
|
||||
/// 稳态增益。
|
||||
pub gain: f32,
|
||||
/// 时间常数（秒）。
|
||||
pub tau_s: f32,
|
||||
/// 当前输出。
|
||||
pub output: f32,
|
||||
}
|
||||
|
||||
impl FirstOrderPlant {
|
||||
/// 构造（`tau_s` 必须为正，否则退化为直通）。
|
||||
#[must_use]
|
||||
pub fn new(gain: f32, tau_s: f32) -> Self {
|
||||
Self { gain, tau_s: if tau_s > 0.0 { tau_s } else { f32::EPSILON }, output: 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
impl ControlPlant for FirstOrderPlant {
|
||||
type Error = Infallible;
|
||||
|
||||
fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error> {
|
||||
if dt > 0.0 && input.is_finite() {
|
||||
let alpha = (dt / self.tau_s).clamp(0.0, 1.0);
|
||||
let target = self.gain * input;
|
||||
self.output += alpha * (target - self.output);
|
||||
}
|
||||
Ok(self.output)
|
||||
}
|
||||
|
||||
fn reset(&mut self) {
|
||||
self.output = 0.0;
|
||||
}
|
||||
|
||||
fn describe(&self) -> String {
|
||||
format!("first-order(gain={}, tau={}s)", self.gain, self.tau_s)
|
||||
}
|
||||
}
|
||||
|
||||
/// 标准二阶系统：`G(s) = gain·wn² / (s² + 2ζ·wn·s + wn²)`，显式欧拉。
|
||||
///
|
||||
/// 用作"整定算法是否真的收敛"的试金石——ARCHITECTURE.md §5.5 的验收标准就要求
|
||||
/// 继电整定在标准二阶系统上收敛。
|
||||
///
|
||||
/// # 相频特性（做继电实验时必须知道）
|
||||
///
|
||||
/// `∠G(jω) = −atan2(2ζ·wn·ω, wn² − ω²)`：在 `ω = wn` 处只有 **−90°**，
|
||||
/// 随 `ω → ∞` **渐近**趋于 −180°，**不存在有限的 −180° 穿越点**。
|
||||
/// 因此纯二阶对象在继电反馈下的极限环由**滞环宽度**决定（幅值 ≈ 滞环量级、
|
||||
/// 周期远小于 `2π/wn`），而不是由 `Tu = 2π/wn` 决定。
|
||||
/// 真实电机带延迟（`sim-harness` 的模型有 2 拍命令延迟 + 摩擦），相频会真正穿过
|
||||
/// −180°，那时的 `Tu` 才是有物理意义的临界周期。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct SecondOrderPlant {
|
||||
/// 自然频率（rad/s）。
|
||||
pub wn: f32,
|
||||
/// 阻尼比。
|
||||
pub zeta: f32,
|
||||
/// 直流增益。
|
||||
pub gain: f32,
|
||||
/// 位置状态。
|
||||
pub position: f32,
|
||||
/// 速度状态。
|
||||
pub velocity: f32,
|
||||
}
|
||||
|
||||
impl SecondOrderPlant {
|
||||
/// 构造（`wn > 0`；`zeta` 取绝对值）。
|
||||
#[must_use]
|
||||
pub fn new(wn: f32, zeta: f32, gain: f32) -> Self {
|
||||
Self {
|
||||
wn: if wn > 0.0 { wn } else { f32::EPSILON },
|
||||
zeta: zeta.abs(),
|
||||
gain,
|
||||
position: 0.0,
|
||||
velocity: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ControlPlant for SecondOrderPlant {
|
||||
type Error = Infallible;
|
||||
|
||||
fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error> {
|
||||
if dt > 0.0 && input.is_finite() {
|
||||
let accel = self.gain * self.wn * self.wn * input
|
||||
- 2.0 * self.zeta * self.wn * self.velocity
|
||||
- self.wn * self.wn * self.position;
|
||||
self.velocity += accel * dt;
|
||||
self.position += self.velocity * dt;
|
||||
}
|
||||
Ok(self.position)
|
||||
}
|
||||
|
||||
fn reset(&mut self) {
|
||||
self.position = 0.0;
|
||||
self.velocity = 0.0;
|
||||
}
|
||||
|
||||
fn describe(&self) -> String {
|
||||
format!("second-order(wn={} rad/s, zeta={}, gain={})", self.wn, self.zeta, self.gain)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn second_order_settles_at_dc_gain() {
|
||||
let mut plant = SecondOrderPlant::new(20.0, 0.7, 2.0);
|
||||
for _ in 0..20_000 {
|
||||
plant.step(1.0, 0.0001).unwrap();
|
||||
}
|
||||
assert!((plant.position - 2.0).abs() < 5e-2, "{}", plant.position);
|
||||
plant.reset();
|
||||
assert!((plant.position).abs() < 1e-6);
|
||||
assert!(plant.describe().contains("second-order"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn second_order_ignores_non_positive_dt() {
|
||||
let mut plant = SecondOrderPlant::new(20.0, 0.5, 1.0);
|
||||
assert!((plant.step(1.0, 0.0).unwrap()).abs() < 1e-6);
|
||||
assert!((plant.velocity).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn first_order_converges_to_gain_times_input() {
|
||||
let mut plant = FirstOrderPlant::new(2.0, 0.1);
|
||||
for _ in 0..2000 {
|
||||
plant.step(1.0, 0.001).unwrap();
|
||||
}
|
||||
assert!((plant.output - 2.0).abs() < 1e-3, "{}", plant.output);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_positive_dt_does_not_advance() {
|
||||
let mut plant = FirstOrderPlant::new(1.0, 0.1);
|
||||
assert!((plant.step(5.0, 0.0).unwrap()).abs() < 1e-6);
|
||||
assert!((plant.step(5.0, -1.0).unwrap()).abs() < 1e-6);
|
||||
assert!((plant.step(f32::NAN, 0.001).unwrap()).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_history() {
|
||||
let mut plant = FirstOrderPlant::new(1.0, 0.05);
|
||||
for _ in 0..100 {
|
||||
plant.step(1.0, 0.001).unwrap();
|
||||
}
|
||||
assert!(plant.output > 0.0);
|
||||
plant.reset();
|
||||
assert!((plant.output).abs() < 1e-6);
|
||||
assert!(plant.describe().contains("first-order"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use plant::*;
|
||||
|
||||
pub mod position {
|
||||
//! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。
|
||||
|
||||
use crate::velocity::VelocityController;
|
||||
use pid::Pid;
|
||||
|
||||
/// 位置环拓扑。
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum PositionLoop {
|
||||
/// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。
|
||||
Direct(Pid),
|
||||
/// 级联：位置 → 目标转速 → 速度环 → effort_raw。
|
||||
Cascade {
|
||||
/// 外环位置 PID（输出限幅 = 最大 rpm）。
|
||||
pos: Pid,
|
||||
/// 内环速度控制器（输出限幅 = 最大 effort）。
|
||||
vel: VelocityController,
|
||||
},
|
||||
}
|
||||
|
||||
/// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PositionController {
|
||||
inner: PositionLoop,
|
||||
}
|
||||
|
||||
impl PositionController {
|
||||
/// 单环位置控制器。
|
||||
#[must_use]
|
||||
pub fn direct(pid: Pid) -> Self {
|
||||
Self { inner: PositionLoop::Direct(pid) }
|
||||
}
|
||||
|
||||
/// 级联位置控制器（限速）。
|
||||
#[must_use]
|
||||
pub fn cascade(pos: Pid, vel: VelocityController) -> Self {
|
||||
Self { inner: PositionLoop::Cascade { pos, vel } }
|
||||
}
|
||||
|
||||
/// 单步更新，返回 effort_raw（已含限幅）。
|
||||
///
|
||||
/// `measured_rpm` 仅级联内环使用；单环忽略该参数。
|
||||
pub fn update(
|
||||
&mut self,
|
||||
target_rev: f32,
|
||||
measured_rev: f32,
|
||||
measured_rpm: f32,
|
||||
dt: f32,
|
||||
) -> f32 {
|
||||
match &mut self.inner {
|
||||
PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt),
|
||||
PositionLoop::Cascade { pos, vel } => {
|
||||
let vel_target = pos.update(target_rev, measured_rev, dt);
|
||||
vel.update(vel_target, measured_rpm, dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 清空内部状态（切换模式 / 重新使能时调用）。
|
||||
pub fn reset(&mut self) {
|
||||
match &mut self.inner {
|
||||
PositionLoop::Direct(pid) => pid.reset(),
|
||||
PositionLoop::Cascade { pos, vel } => {
|
||||
pos.reset();
|
||||
vel.reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const DT: f32 = 0.001;
|
||||
|
||||
#[test]
|
||||
fn direct_outputs_pid() {
|
||||
let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0));
|
||||
let out = pc.update(1.0, 0.5, 0.0, DT);
|
||||
assert!((out - 1.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cascade_limits_velocity_then_effort() {
|
||||
// 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。
|
||||
let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0);
|
||||
let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0);
|
||||
let mut pc = PositionController::cascade(pos, vel);
|
||||
let out = pc.update(1.0, 0.0, 0.0, DT);
|
||||
// 外环饱和到 5 rpm，内环 ff=2 → 10 raw。
|
||||
assert!((out - 10.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_state() {
|
||||
let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6));
|
||||
pc.update(1.0, 0.0, 0.0, DT);
|
||||
pc.reset();
|
||||
let out = pc.update(1.0, 0.0, 0.0, DT);
|
||||
// 重置后只剩本步积分：10 * 1 * 0.001。
|
||||
assert!((out - 0.01).abs() < 1e-5, "out = {out}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use position::*;
|
||||
|
||||
pub mod velocity {
|
||||
//! 速度环控制器：PID + 速度前馈。
|
||||
|
||||
use pid::Pid;
|
||||
|
||||
/// 速度环增益组（可整定参数）。
|
||||
///
|
||||
/// 单位：`kp` 为 raw / rpm，`ki` 为 raw / (rpm·s)，`kd` 为 raw / (rpm/s)，
|
||||
/// `feedforward` 为 raw / rpm。整套增益是**纯数据**，便于整定工具在候选之间
|
||||
/// 传递、落盘与热更新，而无需重建整个速度环。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct VelocityGains {
|
||||
/// 比例增益（raw / rpm）。
|
||||
pub kp: f32,
|
||||
/// 积分增益（raw / (rpm·s)）。
|
||||
pub ki: f32,
|
||||
/// 微分增益（raw / (rpm/s)）。
|
||||
pub kd: f32,
|
||||
/// 速度前馈（raw / rpm）。
|
||||
pub feedforward: f32,
|
||||
}
|
||||
|
||||
impl VelocityGains {
|
||||
/// 全零增益。
|
||||
pub const ZERO: Self = Self { kp: 0.0, ki: 0.0, kd: 0.0, feedforward: 0.0 };
|
||||
|
||||
/// 由四个分量构造。
|
||||
#[must_use]
|
||||
pub const fn new(kp: f32, ki: f32, kd: f32, feedforward: f32) -> Self {
|
||||
Self { kp, ki, kd, feedforward }
|
||||
}
|
||||
|
||||
/// 所有增益是否有限（NaN / inf 不得进入 [`Pid::new`]，它会 panic）。
|
||||
#[must_use]
|
||||
pub fn is_finite(&self) -> bool {
|
||||
self.kp.is_finite()
|
||||
&& self.ki.is_finite()
|
||||
&& self.kd.is_finite()
|
||||
&& self.feedforward.is_finite()
|
||||
}
|
||||
}
|
||||
|
||||
/// 速度环封装：在 [`Pid`] 之上叠加比例前馈。
|
||||
///
|
||||
/// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量
|
||||
/// effort_raw（协议 raw：ESC=电流，GM6020=电压）。
|
||||
/// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量
|
||||
/// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。
|
||||
///
|
||||
/// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct VelocityController {
|
||||
pid: Pid,
|
||||
feedforward: f32,
|
||||
}
|
||||
|
||||
impl VelocityController {
|
||||
/// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。
|
||||
#[must_use]
|
||||
pub fn new(pid: Pid, feedforward: f32) -> Self {
|
||||
Self { pid, feedforward }
|
||||
}
|
||||
|
||||
/// 由增益组与 effort 限幅直接创建（把 [`VelocityGains`] 展开成 [`Pid`]）。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `output_limit_raw` 为 0，或增益含 NaN / inf 时 panic（同 [`Pid::new`]）。
|
||||
#[must_use]
|
||||
pub fn from_gains(output_limit_raw: i16, gains: VelocityGains) -> Self {
|
||||
let limit = f32::from(output_limit_raw);
|
||||
let pid = Pid::new(gains.kp, gains.ki, gains.kd, -limit, limit);
|
||||
Self { pid, feedforward: gains.feedforward }
|
||||
}
|
||||
|
||||
/// 当前增益组（含前馈），用于诊断与热更新前的读取。
|
||||
#[must_use]
|
||||
pub fn gains(&self) -> VelocityGains {
|
||||
VelocityGains {
|
||||
kp: self.pid.kp(),
|
||||
ki: self.pid.ki(),
|
||||
kd: self.pid.kd(),
|
||||
feedforward: self.feedforward,
|
||||
}
|
||||
}
|
||||
|
||||
/// 单步更新，返回 effort_raw（已含 PID 输出限幅）。
|
||||
pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 {
|
||||
let pid_out = self.pid.update(target_rpm, measured_rpm, dt);
|
||||
(pid_out + self.feedforward * target_rpm).clamp(
|
||||
// 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。
|
||||
self.pid.output_min(),
|
||||
self.pid.output_max(),
|
||||
)
|
||||
}
|
||||
|
||||
/// 清空 PID 内部状态（切换模式 / 重新使能时调用）。
|
||||
pub fn reset(&mut self) {
|
||||
self.pid.reset();
|
||||
}
|
||||
|
||||
/// 前馈系数（raw / rpm）。
|
||||
#[must_use]
|
||||
pub fn feedforward(&self) -> f32 {
|
||||
self.feedforward
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn pure_feedforward_tracks_target() {
|
||||
let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0);
|
||||
assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3);
|
||||
// 前馈 -18000 被限幅到 -16384。
|
||||
assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedforward_plus_pid_respects_limit() {
|
||||
let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0);
|
||||
// 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。
|
||||
let out = vc.update(1000.0, 0.0, 0.001);
|
||||
assert!((out - 12000.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_error_yields_pure_feedforward() {
|
||||
// 误差为 0 时 PID 贡献为 0，输出只剩前馈。
|
||||
let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0);
|
||||
let out = vc.update(1000.0, 1000.0, 0.001);
|
||||
assert!((out - 9000.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_pid_state() {
|
||||
let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0);
|
||||
for _ in 0..10 {
|
||||
vc.update(100.0, 0.0, 0.001);
|
||||
}
|
||||
vc.reset();
|
||||
// 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。
|
||||
let out = vc.update(100.0, 0.0, 0.001);
|
||||
assert!((out - 1010.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_round_trips_through_gains() {
|
||||
let gains = VelocityGains::new(8.0, 2.0, 0.5, 1.77);
|
||||
let mut from_data = VelocityController::from_gains(12_000, gains);
|
||||
let mut by_hand =
|
||||
VelocityController::new(Pid::new(8.0, 2.0, 0.5, -12_000.0, 12_000.0), 1.77);
|
||||
|
||||
assert_eq!(from_data.gains(), gains);
|
||||
// 两条构造路径必须逐位一致。
|
||||
for step in 0..16 {
|
||||
let target = 1000.0 + step as f32;
|
||||
let measured = 900.0 + step as f32 * 3.0;
|
||||
assert_eq!(
|
||||
from_data.update(target, measured, 0.001).to_bits(),
|
||||
by_hand.update(target, measured, 0.001).to_bits()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_clamps_to_output_limit() {
|
||||
let mut vc =
|
||||
VelocityController::from_gains(1000, VelocityGains::new(100.0, 0.0, 0.0, 0.0));
|
||||
assert!((vc.update(1e5, 0.0, 0.001) - 1000.0).abs() < 1e-3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn gains_finiteness_is_checkable() {
|
||||
assert!(VelocityGains::new(1.0, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(f32::NAN, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(1.0, f32::INFINITY, 3.0, 4.0).is_finite());
|
||||
assert_eq!(VelocityGains::ZERO, VelocityGains::new(0.0, 0.0, 0.0, 0.0));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use velocity::*;
|
||||
|
||||
pub mod cascade {
|
||||
//! 串级控制：位置环（外）+ 速度环（内）（ARCHITECTURE.md §5.5）。
|
||||
//!
|
||||
//! 外环输出的是**速度目标**而不是控制量，因此内环必须在每个节拍都跑一次——
|
||||
//! 这也是云台（位置）与底盘（速度）共用的结构：底盘只用内环，云台两环都用。
|
||||
//!
|
||||
//! ```text
|
||||
//! 位置目标 ──►[ 外环 PID ]──► 速度目标 ──►[ 内环 PID ]──► effort
|
||||
//! ▲ ▲
|
||||
//! 位置反馈 速度反馈
|
||||
//! ```
|
||||
|
||||
use pid::Pid;
|
||||
|
||||
/// 位置-速度串级控制器。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct CascadeController {
|
||||
/// 外环：位置 → 速度目标。
|
||||
outer: Pid,
|
||||
/// 内环：速度 → 控制量。
|
||||
inner: Pid,
|
||||
}
|
||||
|
||||
impl CascadeController {
|
||||
/// 由两个已配置好的 PID 组装。
|
||||
#[must_use]
|
||||
pub const fn new(outer: Pid, inner: Pid) -> Self {
|
||||
Self { outer, inner }
|
||||
}
|
||||
|
||||
/// 单步更新，返回内环输出（控制量）。
|
||||
///
|
||||
/// 非有限输入会被内环防御性忽略（见 [`Pid::update`]），因此本函数不会 panic。
|
||||
pub fn compute(
|
||||
&mut self,
|
||||
position_setpoint: f32,
|
||||
position_feedback: f32,
|
||||
velocity_feedback: f32,
|
||||
dt: f32,
|
||||
) -> f32 {
|
||||
let velocity_setpoint = self.outer.update(position_setpoint, position_feedback, dt);
|
||||
self.inner.update(velocity_setpoint, velocity_feedback, dt)
|
||||
}
|
||||
|
||||
/// 外环当前建议的速度目标（诊断 / 限幅用）。
|
||||
#[must_use]
|
||||
pub fn last_velocity_setpoint(&self) -> f32 {
|
||||
self.outer.last_output()
|
||||
}
|
||||
|
||||
/// 清空两环的积分与微分记忆。
|
||||
pub fn reset(&mut self) {
|
||||
self.outer.reset();
|
||||
self.inner.reset();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::plant::{ControlPlant, FirstOrderPlant};
|
||||
|
||||
#[test]
|
||||
fn cascade_drives_a_first_order_plant_to_the_position_setpoint() {
|
||||
// 内环（速度）与对象同量纲简化：直接把外环输出当对象输入，
|
||||
// 用真实的一阶对象验证"两环串联能收敛"。
|
||||
let mut cascade = CascadeController::new(
|
||||
Pid::new(4.0, 8.0, 0.0, -100.0, 100.0),
|
||||
Pid::new(6.0, 20.0, 0.0, -100.0, 100.0),
|
||||
);
|
||||
let mut plant = FirstOrderPlant::new(1.0, 0.05);
|
||||
let dt = 0.001;
|
||||
let mut position = 0.0f32;
|
||||
for _ in 0..4000 {
|
||||
let effort = cascade.compute(1.0, position, 0.0, dt);
|
||||
let velocity = plant.step(effort, dt).unwrap();
|
||||
position += velocity * dt;
|
||||
}
|
||||
assert!((position - 1.0).abs() < 0.05, "position = {position}");
|
||||
assert!(plant.describe().contains("first-order"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_integral_windup() {
|
||||
let mut cascade = CascadeController::new(
|
||||
Pid::new(1.0, 1.0, 0.0, -10.0, 10.0),
|
||||
Pid::new(1.0, 1.0, 0.0, -10.0, 10.0),
|
||||
);
|
||||
for _ in 0..100 {
|
||||
cascade.compute(100.0, 0.0, 0.0, 0.001);
|
||||
}
|
||||
assert!(cascade.last_velocity_setpoint().abs() > 0.0);
|
||||
cascade.reset();
|
||||
assert!((cascade.last_velocity_setpoint()).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use cascade::*;
|
||||
|
||||
/// Per-motor controller state used by [`MotorPolicy`].
|
||||
struct PolicyLoop {
|
||||
current_limit: i16,
|
||||
gains: VelocityGains,
|
||||
velocity: VelocityController,
|
||||
position: PositionController,
|
||||
last_mode: motor::CommandMode,
|
||||
}
|
||||
|
||||
impl PolicyLoop {
|
||||
fn new(limit: i16, gains: VelocityGains) -> Self {
|
||||
let bound = f32::from(limit);
|
||||
Self {
|
||||
current_limit: limit,
|
||||
gains,
|
||||
velocity: VelocityController::from_gains(limit, gains),
|
||||
position: PositionController::direct(pid::Pid::new(
|
||||
gains.kp, gains.ki, gains.kd, -bound, bound,
|
||||
)),
|
||||
last_mode: motor::CommandMode::Disabled,
|
||||
}
|
||||
}
|
||||
|
||||
fn reset(&mut self) {
|
||||
self.velocity.reset();
|
||||
self.position.reset();
|
||||
self.last_mode = motor::CommandMode::Disabled;
|
||||
}
|
||||
|
||||
fn effort(&mut self, sample: motor::PolicySample, dt: f32) -> i16 {
|
||||
if !sample.online {
|
||||
self.reset();
|
||||
return 0;
|
||||
}
|
||||
if self.last_mode != sample.command.mode() {
|
||||
self.reset();
|
||||
self.last_mode = sample.command.mode();
|
||||
}
|
||||
match sample.command {
|
||||
motor::Command::Disabled => {
|
||||
self.reset();
|
||||
0
|
||||
}
|
||||
motor::Command::EffortRaw(raw) => raw,
|
||||
motor::Command::Velocity(target) => {
|
||||
self.velocity.update(target, sample.state.rotor_speed_rpm, dt) as i16
|
||||
}
|
||||
motor::Command::Position(target) => self.position.update(
|
||||
target,
|
||||
sample.state.rotor_revolutions,
|
||||
sample.state.rotor_speed_rpm,
|
||||
dt,
|
||||
) as i16,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Concrete velocity/position policy for up to eight ESCs.
|
||||
pub struct MotorPolicy {
|
||||
loops: [Option<PolicyLoop>; 8],
|
||||
}
|
||||
|
||||
impl MotorPolicy {
|
||||
/// Create an empty policy.
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self { loops: std::array::from_fn(|_| None) }
|
||||
}
|
||||
|
||||
/// Configure one ESC controller.
|
||||
pub fn configure(&mut self, esc_id: u8, current_limit: i16, gains: VelocityGains) {
|
||||
if let Some(slot) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
|
||||
*slot = Some(PolicyLoop::new(current_limit, gains));
|
||||
}
|
||||
}
|
||||
|
||||
/// Read one controller's current gains.
|
||||
#[must_use]
|
||||
pub fn velocity_gains(&self, esc_id: u8) -> Option<VelocityGains> {
|
||||
self.loops
|
||||
.get(usize::from(esc_id.saturating_sub(1)))
|
||||
.and_then(Option::as_ref)
|
||||
.map(|loop_| loop_.gains)
|
||||
}
|
||||
|
||||
/// Replace one controller's gains without changing its output limit.
|
||||
pub fn set_velocity_gains(&mut self, esc_id: u8, gains: VelocityGains) {
|
||||
if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
|
||||
let limit = loop_.current_limit;
|
||||
*loop_ = PolicyLoop::new(limit, gains);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for MotorPolicy {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl motor::EffortPolicy for MotorPolicy {
|
||||
fn step(&mut self, input: &motor::PolicyInput) -> [i16; 8] {
|
||||
std::array::from_fn(|index| {
|
||||
let sample = input.samples[index];
|
||||
match self.loops[index].as_mut() {
|
||||
Some(loop_) if sample.id == u8::try_from(index + 1).unwrap_or(0) => {
|
||||
loop_.effort(sample, input.dt)
|
||||
}
|
||||
_ => 0,
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
fn reset(&mut self, esc_id: u8) {
|
||||
if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
|
||||
loop_.reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user