mirror of
https://github.com/pollen-robotics/microduck.git
synced 2026-11-22 09:38:03 +08:00
Merge pull request #603 from pollen-robotics/dynamic-pwm-limit
robotd: Goal PWM follows the battery so the motors see ~6 V
This commit is contained in:
@@ -62,6 +62,14 @@ port = "/dev/ttyS2"
|
||||
# more often than any other (2026-11-09).
|
||||
# impact_guard = false
|
||||
|
||||
# Cap every servo's Goal PWM at 6 V / battery voltage, so the motors see at most ~6 V whatever
|
||||
# the charge: 73 % of full duty on a full 8.2 V pack, 97 % at 6.2 V, 100 % below 6 V. It
|
||||
# follows the smoothed battery voltage (the same one the percentage comes from) every 10 s.
|
||||
# Goal PWM is RAM and is rewritten each time, so it survives a servo reboot. OFF writes Goal PWM
|
||||
# back to 100 % once at startup, so a cap from an earlier run does not linger in the servos.
|
||||
# Needs a robotd restart.
|
||||
# battery_voltage_based_pwm_limit = false
|
||||
|
||||
# What ON writes, in the registers' units. These defaults come from a robot test: ROBOTIS's
|
||||
# factory values, the gear bench's (V Min 1, brake 50, hold 500 mA, 50 % at 9 rad/s), tripped
|
||||
# the brake on nearly every step and their hold softened the stance leg, so the robot fell.
|
||||
|
||||
@@ -331,6 +331,16 @@ a `robotd` restart takes the write too. New settings are written with torque off
|
||||
next torque on, without a reboot: on V3, each restart's hold current and taper showed in the very
|
||||
next walk on lavandiere (2026-11-05); on V4, the gear bench's first shot after the write ran them.
|
||||
|
||||
**Goal PWM can follow the battery.** With `bus.battery_voltage_based_pwm_limit` on (off by
|
||||
default), every 10 s the loop writes Goal PWM (100, RAM) on every servo at `6 V / battery EMA`
|
||||
out of 885, the factory PWM Limit: 648 (73 %) on a full 8.2 V pack, 856 at 6.2 V, 885 below
|
||||
6 V. The motor then sees ~6 V whatever the charge (`duck_control::model::goal_pwm_for`). It is
|
||||
Goal PWM and not PWM Limit (36, EEPROM) because PWM Limit reaches Goal PWM only at boot. It is
|
||||
rewritten even when unchanged, and the first apply after a servo reboot writes it again. Off
|
||||
writes 885 once at startup: Goal PWM survives a `robotd` restart, so without that a cap from an
|
||||
earlier run would stay in place. While on, `robot.health` carries the last value as `goal_pwm`, and `robotctl
|
||||
monitor` shows it beside the battery.
|
||||
|
||||
Voltage is averaged across the servos: all fifteen sit on one pack, so a single reading is the
|
||||
same measurement with more noise, and a device answering zero is filtered out rather than
|
||||
averaged in as if the pack were half flat. Temperature is *not* averaged — the caller gets
|
||||
|
||||
@@ -296,7 +296,7 @@ and absent means *not known*, never zero:
|
||||
|
||||
| field | what |
|
||||
|---|---|
|
||||
| `health` | `robot.health` as `robotd` answered it: `healthy`, `degraded`, `reason`, `battery`, `motors`, `cpu_temp_c`, `cpu_throttle`, `control_loop`, `bus`, `imu` |
|
||||
| `health` | `robot.health` as `robotd` answered it: `healthy`, `degraded`, `reason`, `battery`, `goal_pwm`, `motors`, `cpu_temp_c`, `cpu_throttle`, `control_loop`, `bus`, `imu` |
|
||||
| `health_error` | why there is no `health` — `robotd` not answering |
|
||||
| `posture` | `policy`, `fallen`, `limp`, `picked_up`, from `robot.state` |
|
||||
| `posture_error` | why there is no `posture` |
|
||||
|
||||
@@ -755,6 +755,15 @@ impl RobotIo for DynamixelIo {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// One `sync_write` to every joint. Goal PWM is RAM and writable with torque on, so this
|
||||
/// lands at once, mid-walk, without a reboot — which is why it is Goal PWM and not the
|
||||
/// EEPROM PWM Limit, which only reaches Goal PWM at boot.
|
||||
fn set_goal_pwm(&mut self, pwm: u16) -> Result<()> {
|
||||
self.controller
|
||||
.sync_write_goal_pwm(&JOINT_IDS, &[pwm; NUM_JOINTS])
|
||||
.map_err(|e| IoError::Bus(format!("sync_write goal_pwm {pwm}: {e}")))
|
||||
}
|
||||
|
||||
/// Supply voltage and case temperatures, in one `sync_read` over registers 144–146.
|
||||
///
|
||||
/// Voltage is averaged because all 15 servos sit on one pack: a single reading is the
|
||||
|
||||
@@ -144,6 +144,16 @@ pub trait RobotIo {
|
||||
/// a running value of 200.
|
||||
fn set_gain(&mut self, kp: u16) -> Result<()>;
|
||||
|
||||
/// Set Goal PWM (register 100, RAM) on every joint: the duty-cycle cap, out of
|
||||
/// [`crate::model::GOAL_PWM_MAX`], that keeps the motor at
|
||||
/// [`crate::model::MOTOR_MAX_V`] whatever the pack's charge ([`crate::model::goal_pwm_for`]).
|
||||
///
|
||||
/// Defaulted to a no-op because only real servos have a pack behind them: the simulator's
|
||||
/// motors are already modelled at the voltage the policy was trained on.
|
||||
fn set_goal_pwm(&mut self, _pwm: u16) -> Result<()> {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Torque on or off, every joint.
|
||||
///
|
||||
/// On the trait — rather than only on the bus — because bringing the robot up is now something
|
||||
@@ -268,6 +278,8 @@ pub struct FakeIo {
|
||||
pub writes: usize,
|
||||
/// Last gain commanded, so a test can tell "went limp" from "stopped commanding".
|
||||
pub last_gain: Option<u16>,
|
||||
/// Every Goal PWM commanded, in order, so a test can tell "refreshed" from "written once".
|
||||
pub goal_pwms: Vec<u16>,
|
||||
/// Whether the orientation filter reports converged. False models the first seconds after
|
||||
/// startup, when projected gravity is not yet a measurement.
|
||||
pub imu_ready: bool,
|
||||
@@ -320,6 +332,7 @@ impl FakeIo {
|
||||
reads: 0,
|
||||
writes: 0,
|
||||
last_gain: None,
|
||||
goal_pwms: Vec::new(),
|
||||
imu_ready: true,
|
||||
slow: Some(SlowSensors {
|
||||
volts: 7.4,
|
||||
@@ -414,6 +427,11 @@ impl RobotIo for FakeIo {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn set_goal_pwm(&mut self, pwm: u16) -> Result<()> {
|
||||
self.goal_pwms.push(pwm);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn set_torque(&mut self, on: bool) -> Result<()> {
|
||||
self.torque = Some(on);
|
||||
self.torque_writes += 1;
|
||||
|
||||
@@ -23,6 +23,6 @@ pub use imu::ImuData;
|
||||
pub use io::{Census, FakeIo, IoError, JointTargets, RobotIo, Sensors, SlowSensors};
|
||||
pub use model::{
|
||||
BATTERY_EMPTY_V, BATTERY_FULL_V, DEFAULT_POSITION, JOINT_IDS, JOINT_NAMES, NUM_JOINTS,
|
||||
REST_POSITION, battery_percent,
|
||||
REST_POSITION, battery_percent, goal_pwm_for,
|
||||
};
|
||||
pub use obs::{ACTION_LEN, Command, OBS_LEN, Observation};
|
||||
|
||||
@@ -278,6 +278,38 @@ pub fn battery_percent(volts: f64) -> f64 {
|
||||
((volts - BATTERY_EMPTY_V) / (BATTERY_FULL_V - BATTERY_EMPTY_V)).clamp(0.0, 1.0) * 100.0
|
||||
}
|
||||
|
||||
// ── voltage-compensated PWM ──────────────────────────────────────────────────
|
||||
//
|
||||
// The servos drive their motor with PWM straight off the pack, so a full 8.2 V pack puts
|
||||
// 8.2 V across the motor at 100 % duty, where a nearly flat one puts 6.2 V. Capping Goal PWM
|
||||
// at `MOTOR_MAX_V / pack` keeps the most the motor ever sees at about 6 V whatever the charge:
|
||||
// the robot behaves the same at the top of the pack as near the bottom, and the gears are not
|
||||
// asked for the extra torque a fresh pack would otherwise hand them.
|
||||
//
|
||||
// Goal PWM (100, RAM) rather than PWM Limit (36, EEPROM): PWM Limit only reaches Goal PWM at
|
||||
// boot, so changing it would need a reboot per servo. Goal PWM takes effect at once and is
|
||||
// itself capped by PWM Limit, which nothing here writes and so stays at its factory 885.
|
||||
|
||||
/// Goal PWM at 100 % duty: the XL330's factory PWM Limit.
|
||||
pub const GOAL_PWM_MAX: u16 = 885;
|
||||
|
||||
/// The most the motor should see, in volts. Goal PWM is set to this fraction of the pack.
|
||||
pub const MOTOR_MAX_V: f64 = 6.0;
|
||||
|
||||
/// Goal PWM that puts at most [`MOTOR_MAX_V`] across the motor on a pack at `volts`.
|
||||
///
|
||||
/// 8.2 V (full) gives 648, 73 %; 6.2 V gives 856, 97 %; at or below 6 V, 885. `None` for a
|
||||
/// non-finite or non-positive reading — "no answer from the bus", the same meaning
|
||||
/// [`battery_percent`] gives it — so the caller leaves the servos where they are rather than
|
||||
/// guessing a pack.
|
||||
pub fn goal_pwm_for(volts: f64) -> Option<u16> {
|
||||
if !volts.is_finite() || volts <= 0.0 {
|
||||
return None;
|
||||
}
|
||||
let fraction = (MOTOR_MAX_V / volts).min(1.0);
|
||||
Some((fraction * GOAL_PWM_MAX as f64).round() as u16)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
@@ -434,6 +466,30 @@ mod tests {
|
||||
|
||||
/// A bus that did not answer arrives here as 0.0, and NaN is what a mean over an empty
|
||||
/// set produces. Both must be 0 rather than a wild number a caller might print.
|
||||
#[test]
|
||||
fn goal_pwm_holds_the_motor_at_six_volts() {
|
||||
assert_eq!(goal_pwm_for(BATTERY_FULL_V), Some(648));
|
||||
assert_eq!(goal_pwm_for(6.2), Some(856));
|
||||
assert_eq!(goal_pwm_for(MOTOR_MAX_V), Some(GOAL_PWM_MAX));
|
||||
// A pack below the motor's voltage cannot be compensated for: full duty, never more.
|
||||
assert_eq!(goal_pwm_for(5.0), Some(GOAL_PWM_MAX));
|
||||
for v in [6.5, 7.0, 7.4, 8.0, 8.4] {
|
||||
let pwm = goal_pwm_for(v).unwrap();
|
||||
let motor_v = v * pwm as f64 / GOAL_PWM_MAX as f64;
|
||||
assert!(
|
||||
(motor_v - MOTOR_MAX_V).abs() < 0.01,
|
||||
"{v} V → {pwm} → {motor_v} V"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn goal_pwm_is_not_guessed_without_a_reading() {
|
||||
assert_eq!(goal_pwm_for(0.0), None);
|
||||
assert_eq!(goal_pwm_for(-1.0), None);
|
||||
assert_eq!(goal_pwm_for(f64::NAN), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn battery_percent_treats_no_reading_as_zero() {
|
||||
assert_eq!(battery_percent(0.0), 0.0);
|
||||
|
||||
@@ -154,6 +154,10 @@ pub struct Safety<T: RobotIo> {
|
||||
/// Tracks the last gain written so an unchanged one is not rewritten every tick — that
|
||||
/// would be fifteen bus writes per tick for no reason.
|
||||
gain: Option<u16>,
|
||||
/// Goal PWM the caller last asked for, and whether the servos still hold it. Kept apart
|
||||
/// because a reboot loses the register but not the wish: [`Self::apply`] puts it back.
|
||||
goal_pwm: Option<u16>,
|
||||
goal_pwm_written: bool,
|
||||
/// Trip counts, for rate-limiting what the layer says about itself.
|
||||
runs: LimitRuns,
|
||||
/// Whether an intent has ever arrived inside the deadman, which is what makes a stale
|
||||
@@ -170,6 +174,8 @@ impl<T: RobotIo> Safety<T> {
|
||||
falling_for: Duration::ZERO,
|
||||
fallen: false,
|
||||
gain: None,
|
||||
goal_pwm: None,
|
||||
goal_pwm_written: false,
|
||||
runs: LimitRuns::default(),
|
||||
deadman_armed: false,
|
||||
}
|
||||
@@ -245,9 +251,29 @@ impl<T: RobotIo> Safety<T> {
|
||||
self.io.reboot(id)?;
|
||||
}
|
||||
self.gain = None;
|
||||
self.goal_pwm_written = false;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Cap the motors' duty cycle at `pwm` out of [`crate::model::GOAL_PWM_MAX`] — see
|
||||
/// [`crate::model::goal_pwm_for`].
|
||||
///
|
||||
/// Written now, and always, even when unchanged: the caller asks rarely (robotd every ten
|
||||
/// seconds), and a rewrite is what catches a servo that lost its RAM in a way nothing here
|
||||
/// saw — a brown-out, a swap. A failed write is retried by the next [`Self::apply`].
|
||||
pub fn set_goal_pwm(&mut self, pwm: u16) -> Result<(), IoError> {
|
||||
self.goal_pwm = Some(pwm);
|
||||
self.goal_pwm_written = false;
|
||||
self.io.set_goal_pwm(pwm)?;
|
||||
self.goal_pwm_written = true;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// The Goal PWM last asked for, or `None` before the first request.
|
||||
pub fn goal_pwm(&self) -> Option<u16> {
|
||||
self.goal_pwm
|
||||
}
|
||||
|
||||
/// The gain last written to the servos, or `None` before the first write. This is what
|
||||
/// the robot is running at, which is not always what the caller asked for.
|
||||
pub fn gain(&self) -> Option<u16> {
|
||||
@@ -381,6 +407,9 @@ impl<T: RobotIo> Safety<T> {
|
||||
// worth doing about is decided above, and arrives as ordinary targets and an
|
||||
// ordinary gain.
|
||||
self.set_gain(running_gain)?;
|
||||
if let Some(pwm) = self.goal_pwm.filter(|_| !self.goal_pwm_written) {
|
||||
self.set_goal_pwm(pwm)?;
|
||||
}
|
||||
|
||||
// Non-finite is refused, not clamped. Clamping `NaN` silently produces a boundary
|
||||
// value, which is a plausible-looking joint angle — far worse than declining to
|
||||
@@ -489,6 +518,31 @@ mod tests {
|
||||
assert_eq!(s.gain(), Some(200));
|
||||
}
|
||||
|
||||
/// Goal PWM is written when asked, not again on every tick, and comes back on the first
|
||||
/// apply after a reboot — a rebooted servo is at its EEPROM PWM Limit, 100 %, otherwise.
|
||||
#[test]
|
||||
fn goal_pwm_is_written_once_and_restored_after_a_reboot() {
|
||||
use crate::io::FakeIo;
|
||||
use crate::model::DEFAULT_POSITION;
|
||||
let mut s = Safety::new(FakeIo::at(DEFAULT_POSITION), SafetyConfig::default());
|
||||
s.apply(DEFAULT_POSITION, DEFAULT_POSITION, 200).unwrap();
|
||||
assert!(
|
||||
s.io().goal_pwms.is_empty(),
|
||||
"nothing asked, nothing written"
|
||||
);
|
||||
|
||||
s.set_goal_pwm(648).unwrap();
|
||||
s.apply(DEFAULT_POSITION, DEFAULT_POSITION, 200).unwrap();
|
||||
s.apply(DEFAULT_POSITION, DEFAULT_POSITION, 200).unwrap();
|
||||
assert_eq!(s.io().goal_pwms, vec![648]);
|
||||
|
||||
s.reboot_motors(&[3]).unwrap();
|
||||
s.apply(DEFAULT_POSITION, DEFAULT_POSITION, 200).unwrap();
|
||||
s.apply(DEFAULT_POSITION, DEFAULT_POSITION, 200).unwrap();
|
||||
assert_eq!(s.io().goal_pwms, vec![648, 648]);
|
||||
assert_eq!(s.goal_pwm(), Some(648));
|
||||
}
|
||||
|
||||
use super::*;
|
||||
use crate::imu::ImuData;
|
||||
use crate::io::FakeIo;
|
||||
|
||||
@@ -614,7 +614,13 @@ pub const JSONRPC_VERSION: &str = "2.0";
|
||||
/// [`method::NFC_READ`]: `nfcd` reads the next tag touched and says what is on it, on the same
|
||||
/// socket as [`method::NFC_WRITE`]. A v60 `nfcd` answers it METHOD_NOT_FOUND, by name; nothing
|
||||
/// else on the wire changed.
|
||||
pub const API_VERSION: u32 = 61;
|
||||
///
|
||||
/// # v62 — the battery's PWM cap
|
||||
///
|
||||
/// [`HealthResult::goal_pwm`]: the Goal PWM `robotd` writes when
|
||||
/// `bus.battery_voltage_based_pwm_limit` is on. Optional and absent when off, so a v61 client
|
||||
/// reading a v62 robot ignores it and a v62 client reading a v61 robot shows nothing.
|
||||
pub const API_VERSION: u32 = 62;
|
||||
|
||||
/// The observation width every policy this robot family runs is built against.
|
||||
///
|
||||
@@ -4661,6 +4667,12 @@ pub struct HealthResult {
|
||||
/// it as an empty battery.
|
||||
#[serde(default, skip_serializing_if = "Option::is_none")]
|
||||
pub battery: Option<Battery>,
|
||||
/// The Goal PWM `robotd` last wrote to every servo, out of 885 (100 % duty), when
|
||||
/// `bus.battery_voltage_based_pwm_limit` is on: it follows the battery so the motors see
|
||||
/// ~6 V. Absent when the switch is off, before the first write, and from an older `robotd`.
|
||||
/// Reported, never judged — same rule as the battery.
|
||||
#[serde(default, skip_serializing_if = "Option::is_none")]
|
||||
pub goal_pwm: Option<u16>,
|
||||
/// Hottest servo, when temperatures have been read. Same rule as the battery: reported,
|
||||
/// never judged.
|
||||
#[serde(default, skip_serializing_if = "Option::is_none")]
|
||||
@@ -4902,6 +4914,12 @@ impl Battery {
|
||||
pub const CRITICAL_PCT: f64 = 15.0;
|
||||
}
|
||||
|
||||
impl HealthResult {
|
||||
/// [`Self::goal_pwm`] at 100 % duty: the XL330's factory PWM Limit, which
|
||||
/// `duck_control::model::GOAL_PWM_MAX` also names — that crate does not depend on this one.
|
||||
pub const GOAL_PWM_FULL: u16 = 885;
|
||||
}
|
||||
|
||||
/// Answer to a discrete intent — [`Call::RobotStop`], [`Call::RobotEnable`].
|
||||
///
|
||||
/// `accepted: false` is a normal outcome, not an error: safety may refuse to enable a
|
||||
|
||||
@@ -2332,6 +2332,15 @@ impl View {
|
||||
None => Span::raw("batt not read yet").dim(),
|
||||
});
|
||||
|
||||
// The PWM cap the battery sets, beside the battery it comes from — only when
|
||||
// `bus.battery_voltage_based_pwm_limit` is on, which is the only time robotd sends it.
|
||||
if let Some(pwm) = health.goal_pwm {
|
||||
said.push(Span::raw(format!(
|
||||
" · pwm {:.0}% ({pwm})",
|
||||
f64::from(pwm) * 100.0 / f64::from(proto::HealthResult::GOAL_PWM_FULL)
|
||||
)));
|
||||
}
|
||||
|
||||
// Said early, because it qualifies everything after it: these are the numbers from the
|
||||
// last answer, and this is how long ago that answer was.
|
||||
if let Some(age) = self.health_age() {
|
||||
@@ -3914,6 +3923,27 @@ mod tests {
|
||||
assert!(!power.contains("motors"), "{screen}");
|
||||
}
|
||||
|
||||
/// The battery's PWM cap sits beside the battery when robotd sends it, and is not mentioned
|
||||
/// at all when it does not — the switch is off, which is the default.
|
||||
#[test]
|
||||
fn the_pwm_cap_shows_only_when_robotd_sends_it() {
|
||||
let power_row = |health| {
|
||||
draw_with_health(110, 32, &a_state(), health)
|
||||
.lines()
|
||||
.find(|line| line.contains(" power "))
|
||||
.expect("the row is drawn")
|
||||
.to_owned()
|
||||
};
|
||||
let on = power_row(proto::HealthResult {
|
||||
goal_pwm: Some(718),
|
||||
..a_health()
|
||||
});
|
||||
assert!(on.contains("batt 7.66 V 66% · pwm 81% (718)"), "{on}");
|
||||
|
||||
let off = power_row(a_health());
|
||||
assert!(!off.contains("pwm"), "{off}");
|
||||
}
|
||||
|
||||
/// An unread battery says so. Rendered as `0.00 V, 0%` it would put a flat-pack warning in
|
||||
/// front of somebody whose robot has a full one — for the first second of every uptime, and
|
||||
/// forever on a board whose bus cannot answer.
|
||||
|
||||
@@ -3025,6 +3025,12 @@ pub struct Bus {
|
||||
/// values, the gear-breakage bench's, made a walking robot fall. Changing them is how a robot
|
||||
/// test looks for a guard that keeps the gears whole and still lets the policy walk.
|
||||
pub impact_guard: bool,
|
||||
/// Cap Goal PWM at `6 V / battery` so the motors see at most ~6 V whatever the charge: 73 %
|
||||
/// on a full 8.2 V pack, nearly 100 % on a flat one. Follows the battery EMA every ten
|
||||
/// seconds; written to Goal PWM (RAM) rather than PWM Limit (EEPROM), which would need a
|
||||
/// reboot per servo. **Off by default**, and off writes Goal PWM back to 100 % once at
|
||||
/// startup, so a cap left by an earlier run does not outlive the setting.
|
||||
pub battery_voltage_based_pwm_limit: bool,
|
||||
/// V Min, rad/s: an impact counts only while the servo moves toward its goal faster.
|
||||
pub impact_guard_v_min: u8,
|
||||
/// Brake Time, ms: how long the H-bridge is shorted after an impact.
|
||||
@@ -3156,6 +3162,7 @@ impl Default for Bus {
|
||||
port: "/dev/ttyS2".into(),
|
||||
fast_sync_read: true,
|
||||
impact_guard: false,
|
||||
battery_voltage_based_pwm_limit: false,
|
||||
impact_guard_v_min: 6,
|
||||
impact_guard_brake_ms: 20,
|
||||
impact_guard_decel: 400,
|
||||
@@ -5266,6 +5273,10 @@ mod tests {
|
||||
assert_eq!(from_file.bus.port, built_in.bus.port);
|
||||
assert_eq!(from_file.bus.fast_sync_read, built_in.bus.fast_sync_read);
|
||||
assert_eq!(from_file.bus.impact_guard, built_in.bus.impact_guard);
|
||||
assert_eq!(
|
||||
from_file.bus.battery_voltage_based_pwm_limit,
|
||||
built_in.bus.battery_voltage_based_pwm_limit
|
||||
);
|
||||
let guard = |b: &Bus| {
|
||||
(
|
||||
b.impact_guard_v_min,
|
||||
|
||||
@@ -121,6 +121,11 @@ pub const REGISTRY: &[Entry] = &[
|
||||
Kind::Bool,
|
||||
"Impact guard on V3 and V4 servos; off = factory firmware with Current Limit only",
|
||||
),
|
||||
feature(
|
||||
"bus.battery_voltage_based_pwm_limit",
|
||||
Kind::Bool,
|
||||
"Cap motor PWM so the motors see ~6 V whatever the battery charge",
|
||||
),
|
||||
entry(
|
||||
"bus.impact_guard_v_min",
|
||||
Kind::Integer,
|
||||
@@ -1661,6 +1666,7 @@ mod tests {
|
||||
assert_eq!(
|
||||
features,
|
||||
vec![
|
||||
"bus.battery_voltage_based_pwm_limit",
|
||||
"policy.enabled",
|
||||
"policy.mode",
|
||||
"policy.voltage_adapt",
|
||||
|
||||
@@ -141,6 +141,11 @@ const HOME_RAMP: Duration = Duration::from_secs(2);
|
||||
/// `microduck_runtime`.
|
||||
const BATTERY_EMA_ALPHA: f64 = 0.1;
|
||||
|
||||
/// How often Goal PWM follows the battery ([`duck_control::goal_pwm_for`]). The EMA it reads
|
||||
/// already moves over ~10 s, and a pack drains over an hour: anything faster rewrites fifteen
|
||||
/// servos to learn nothing, anything much slower leaves a fresh pack's first minutes uncapped.
|
||||
const GOAL_PWM_INTERVAL: Duration = Duration::from_secs(10);
|
||||
|
||||
/// How long the policy holds the shutdown sit before the rest ramp, when no controller is there
|
||||
/// to say. The sitstand descent is a deliberate ~2 s glide and the robot then sits still for a
|
||||
/// second; a running controller derives the same from the set's `ramp_s`
|
||||
@@ -762,6 +767,10 @@ struct RobotState {
|
||||
motor_mean_c: AtomicU64,
|
||||
/// Index into [`duck_control::JOINT_NAMES`] of the hottest joint.
|
||||
motor_hottest: AtomicU32,
|
||||
/// The Goal PWM last written while `bus.battery_voltage_based_pwm_limit` is on, for
|
||||
/// `robot.health`. Zero means *not written* — off, or before the first write — since a
|
||||
/// written Goal PWM is never 0.
|
||||
goal_pwm: AtomicU32,
|
||||
/// Hottest board thermal zone, as `f64::to_bits`. Zero means no reading — off Linux, or a
|
||||
/// kernel with no thermal sysfs ([`soc`]).
|
||||
cpu_temp_c: AtomicU64,
|
||||
@@ -938,6 +947,7 @@ impl RobotState {
|
||||
motor_max_c: AtomicU64::new(0),
|
||||
motor_mean_c: AtomicU64::new(0),
|
||||
motor_hottest: AtomicU32::new(0),
|
||||
goal_pwm: AtomicU32::new(0),
|
||||
cpu_temp_c: AtomicU64::new(0),
|
||||
cpu_throttle: ArcSwapOption::empty(),
|
||||
imu_stale_blocks: AtomicU64::new(0),
|
||||
@@ -998,6 +1008,10 @@ impl RobotState {
|
||||
degraded,
|
||||
reason,
|
||||
battery: self.battery(),
|
||||
goal_pwm: match self.goal_pwm.load(Ordering::Relaxed) {
|
||||
0 => None,
|
||||
pwm => Some(pwm as u16),
|
||||
},
|
||||
motors: self.motor_thermal(),
|
||||
cpu_temp_c: {
|
||||
// Zero is "never read", the same sentinel the battery uses: a board at
|
||||
@@ -2481,6 +2495,10 @@ async fn control_loop<T: RobotIo>(
|
||||
Err(e) => tracing::debug!(error = %e, "vet book: servo identity read failed"),
|
||||
}
|
||||
let mut last_alert_read = Instant::now();
|
||||
// `None` until the first voltage reading, so the cap goes on with the first sample rather
|
||||
// than ten seconds after it.
|
||||
let mut last_goal_pwm: Option<Instant> = None;
|
||||
let pwm_follows_battery = params.bus.battery_voltage_based_pwm_limit;
|
||||
|
||||
tracing::warn!(
|
||||
joints = NUM_JOINTS,
|
||||
@@ -4665,6 +4683,39 @@ async fn control_loop<T: RobotIo>(
|
||||
}
|
||||
vettally.handoff(&state.vetbook, volts, false);
|
||||
|
||||
// `bus.battery_voltage_based_pwm_limit`: Goal PWM follows the pack, so the motors see
|
||||
// ~6 V whether it is full or nearly flat, rewritten every interval even when
|
||||
// unchanged — see `Safety::set_goal_pwm`. Off writes full duty once: Goal PWM is
|
||||
// servo RAM, so a cap from an earlier run outlives a robotd restart otherwise.
|
||||
let want = if pwm_follows_battery {
|
||||
duck_control::goal_pwm_for(volts)
|
||||
} else {
|
||||
last_goal_pwm
|
||||
.is_none()
|
||||
.then_some(duck_control::model::GOAL_PWM_MAX)
|
||||
};
|
||||
if last_goal_pwm.is_none_or(|t| t.elapsed() >= GOAL_PWM_INTERVAL)
|
||||
&& let Some(pwm) = want
|
||||
{
|
||||
last_goal_pwm = Some(Instant::now());
|
||||
let previous = safety.goal_pwm();
|
||||
let result = safety.set_goal_pwm(pwm);
|
||||
if pwm_follows_battery && result.is_ok() {
|
||||
state.goal_pwm.store(u32::from(pwm), Ordering::Relaxed);
|
||||
}
|
||||
match result {
|
||||
Ok(()) if previous.is_none() => tracing::info!(
|
||||
goal_pwm = pwm,
|
||||
battery_v = format!("{volts:.2}"),
|
||||
follows_battery = pwm_follows_battery,
|
||||
"goal PWM set"
|
||||
),
|
||||
Ok(()) => tracing::debug!(goal_pwm = pwm, battery_v = volts, "goal PWM"),
|
||||
// Retried by the next apply, and again in ten seconds.
|
||||
Err(e) => tracing::debug!(error = %e, goal_pwm = pwm, "goal PWM write failed"),
|
||||
}
|
||||
}
|
||||
|
||||
if last_summary.elapsed() >= LOOP_SUMMARY_INTERVAL {
|
||||
tracing::info!(
|
||||
total = ticks,
|
||||
@@ -4679,6 +4730,7 @@ async fn control_loop<T: RobotIo>(
|
||||
"{:.2}",
|
||||
f64::from_bits(state.battery_v.load(Ordering::Relaxed))
|
||||
),
|
||||
goal_pwm = safety.goal_pwm(),
|
||||
motor_max_c = format!(
|
||||
"{:.0}",
|
||||
f64::from_bits(state.motor_max_c.load(Ordering::Relaxed))
|
||||
@@ -8094,6 +8146,75 @@ mod tests {
|
||||
);
|
||||
}
|
||||
|
||||
/// Run the loop on a `FakeIo` until the first battery sample lands, and a few ticks more;
|
||||
/// return every Goal PWM it wrote. The slow-sensor window is a `std` second, so this takes
|
||||
/// one real second whatever tokio's clock does.
|
||||
async fn goal_pwms_after_first_battery_sample(params: Params) -> Vec<u16> {
|
||||
let s = Arc::new(RobotState::new(
|
||||
¶ms,
|
||||
std::path::Path::new("/test/robotd.toml"),
|
||||
false,
|
||||
false,
|
||||
));
|
||||
let (tx, rx) = std::sync::mpsc::channel();
|
||||
let loop_state = Arc::clone(&s);
|
||||
let handle = tokio::spawn(async move {
|
||||
let mut io = FakeIo::at(DEFAULT_POSITION);
|
||||
control_loop(
|
||||
Borrowed(&mut io),
|
||||
loop_state,
|
||||
Arc::new(Intents::new()),
|
||||
params,
|
||||
PathBuf::from("/nonexistent/robotd.toml"),
|
||||
Duration::from_millis(2),
|
||||
Arc::new(|| {}),
|
||||
)
|
||||
.await;
|
||||
tx.send(io.goal_pwms).unwrap();
|
||||
});
|
||||
|
||||
let deadline = Instant::now() + Duration::from_secs(5);
|
||||
while s.battery_v.load(Ordering::Relaxed) == 0 && Instant::now() < deadline {
|
||||
tokio::time::sleep(Duration::from_millis(2)).await;
|
||||
}
|
||||
assert_ne!(
|
||||
s.battery_v.load(Ordering::Relaxed),
|
||||
0,
|
||||
"a battery sample landed"
|
||||
);
|
||||
let ticks = s.ticks.load(Ordering::Relaxed);
|
||||
while s.ticks.load(Ordering::Relaxed) < ticks + 10 {
|
||||
tokio::time::sleep(Duration::from_millis(2)).await;
|
||||
}
|
||||
s.shutdown.store(true, Ordering::Relaxed);
|
||||
handle.await.unwrap();
|
||||
rx.recv().unwrap()
|
||||
}
|
||||
|
||||
/// On, the cap goes on with the first voltage sample — `FakeIo`'s 7.4 V pack is 718 of 885 —
|
||||
/// and is not rewritten every tick.
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn goal_pwm_follows_the_battery_when_asked() {
|
||||
let mut params = Params::default();
|
||||
params.bus.battery_voltage_based_pwm_limit = true;
|
||||
let written = goal_pwms_after_first_battery_sample(params).await;
|
||||
assert_eq!(written, vec![duck_control::goal_pwm_for(7.4).unwrap()]);
|
||||
assert_eq!(written, vec![718]);
|
||||
}
|
||||
|
||||
/// Off, the default, writes full duty once: Goal PWM is servo RAM, and a cap from an earlier
|
||||
/// run with the setting on would otherwise outlive the restart that turned it off.
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn goal_pwm_is_reset_to_full_duty_when_off() {
|
||||
let params = Params::default();
|
||||
assert!(
|
||||
!params.bus.battery_voltage_based_pwm_limit,
|
||||
"off by default"
|
||||
);
|
||||
let written = goal_pwms_after_first_battery_sample(params).await;
|
||||
assert_eq!(written, vec![duck_control::model::GOAL_PWM_MAX]);
|
||||
}
|
||||
|
||||
/// `FakeIo` by reference, so a test can read what the loop did to it after the loop ends.
|
||||
struct Borrowed<'a, T>(&'a mut T);
|
||||
impl<T: RobotIo> RobotIo for Borrowed<'_, T> {
|
||||
@@ -8106,6 +8227,9 @@ mod tests {
|
||||
fn set_gain(&mut self, kp: u16) -> duck_control::io::Result<()> {
|
||||
self.0.set_gain(kp)
|
||||
}
|
||||
fn set_goal_pwm(&mut self, pwm: u16) -> duck_control::io::Result<()> {
|
||||
self.0.set_goal_pwm(pwm)
|
||||
}
|
||||
fn set_torque(&mut self, on: bool) -> duck_control::io::Result<()> {
|
||||
self.0.set_torque(on)
|
||||
}
|
||||
@@ -10659,6 +10783,9 @@ mod tests {
|
||||
fn set_gain(&mut self, kp: u16) -> duck_control::io::Result<()> {
|
||||
self.0.set_gain(kp)
|
||||
}
|
||||
fn set_goal_pwm(&mut self, pwm: u16) -> duck_control::io::Result<()> {
|
||||
self.0.set_goal_pwm(pwm)
|
||||
}
|
||||
fn set_torque(&mut self, on: bool) -> duck_control::io::Result<()> {
|
||||
self.0.set_torque(on)
|
||||
}
|
||||
@@ -11058,6 +11185,9 @@ mod tests {
|
||||
fn set_gain(&mut self, kp: u16) -> duck_control::io::Result<()> {
|
||||
self.inner.set_gain(kp)
|
||||
}
|
||||
fn set_goal_pwm(&mut self, pwm: u16) -> duck_control::io::Result<()> {
|
||||
self.inner.set_goal_pwm(pwm)
|
||||
}
|
||||
fn set_torque(&mut self, on: bool) -> duck_control::io::Result<()> {
|
||||
self.inner.set_torque(on)
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user