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:
Antoine Pirrone
2026-11-16 18:16:01 +08:00
committed by GitHub
13 changed files with 353 additions and 3 deletions
+8
View File
@@ -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.
+10
View File
@@ -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
+1 -1
View File
@@ -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` |
+9
View File
@@ -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
+18
View File
@@ -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;
+1 -1
View File
@@ -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};
+56
View File
@@ -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);
+54
View File
@@ -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;
+19 -1
View File
@@ -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
+30
View File
@@ -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.
+11
View File
@@ -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,
+6
View File
@@ -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",
+130
View File
@@ -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(
&params,
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)
}