diff --git a/deploy/robotd.toml b/deploy/robotd.toml index 0b5696ef..65589ca4 100644 --- a/deploy/robotd.toml +++ b/deploy/robotd.toml @@ -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. diff --git a/docs/design/robotd-design.md b/docs/design/robotd-design.md index 47f18391..801b9980 100644 --- a/docs/design/robotd-design.md +++ b/docs/design/robotd-design.md @@ -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 diff --git a/docs/robot/udp-pad.md b/docs/robot/udp-pad.md index fd578b0c..b15aef6f 100644 --- a/docs/robot/udp-pad.md +++ b/docs/robot/udp-pad.md @@ -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` | diff --git a/duck-control/src/bus.rs b/duck-control/src/bus.rs index b20b87ef..8439ac9e 100644 --- a/duck-control/src/bus.rs +++ b/duck-control/src/bus.rs @@ -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 diff --git a/duck-control/src/io.rs b/duck-control/src/io.rs index 765bfec9..bdfbdb5c 100644 --- a/duck-control/src/io.rs +++ b/duck-control/src/io.rs @@ -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, + /// Every Goal PWM commanded, in order, so a test can tell "refreshed" from "written once". + pub goal_pwms: Vec, /// 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; diff --git a/duck-control/src/lib.rs b/duck-control/src/lib.rs index 900e3ff3..22a4330e 100644 --- a/duck-control/src/lib.rs +++ b/duck-control/src/lib.rs @@ -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}; diff --git a/duck-control/src/model.rs b/duck-control/src/model.rs index 0d935ec0..850d8609 100644 --- a/duck-control/src/model.rs +++ b/duck-control/src/model.rs @@ -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 { + 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); diff --git a/duck-control/src/safety.rs b/duck-control/src/safety.rs index a258258f..2c21405b 100644 --- a/duck-control/src/safety.rs +++ b/duck-control/src/safety.rs @@ -154,6 +154,10 @@ pub struct Safety { /// 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, + /// 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, + 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 Safety { 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 Safety { 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 { + 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 { @@ -381,6 +407,9 @@ impl Safety { // 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; diff --git a/duck-ipc-proto/src/lib.rs b/duck-ipc-proto/src/lib.rs index f6ae22e4..93ee5348 100644 --- a/duck-ipc-proto/src/lib.rs +++ b/duck-ipc-proto/src/lib.rs @@ -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, + /// 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, /// 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 diff --git a/robotctl/src/monitor.rs b/robotctl/src/monitor.rs index 26b2a5b1..f947aa3f 100644 --- a/robotctl/src/monitor.rs +++ b/robotctl/src/monitor.rs @@ -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. diff --git a/robotd-params/src/lib.rs b/robotd-params/src/lib.rs index ea254778..aa59f8bd 100644 --- a/robotd-params/src/lib.rs +++ b/robotd-params/src/lib.rs @@ -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, diff --git a/robotd-params/src/registry.rs b/robotd-params/src/registry.rs index 60e7ec40..52c27ac9 100644 --- a/robotd-params/src/registry.rs +++ b/robotd-params/src/registry.rs @@ -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", diff --git a/robotd/src/main.rs b/robotd/src/main.rs index 20e72e4b..22a703c7 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -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( 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 = 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( } 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( "{:.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 { + 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 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) }