repose_io/
lib.rs

1//! Robot-agnostic digital IO for `copper-rs`, and a gripper built on it.
2//!
3//! A bridge that exposes digital IO carries exactly two payloads:
4//! - [`CuDigitalInputPayload<N>`] on its `digital_in` channel (read values
5//!   plus the IO device status),
6//! - [`CuDigitalOutputPayload<N>`] on its `digital_out` channel (values plus
7//!   a mask of the outputs this command drives; unmasked outputs keep their
8//!   current level).
9//!
10//! End-effector hardware wired to that IO needs no payload of its own.
11//! [`CuGripperTask`] maps open/close commands onto configured output bits
12//! and reads optional sensor inputs back into a [`CuGripperState`], so a
13//! pneumatic or electric gripper on any robot is a RON configuration, not new
14//! code.
15
16#![cfg_attr(not(feature = "std"), no_std)]
17
18use bincode::Decode;
19use bincode::Encode;
20use bincode::de::Decoder;
21use bincode::enc::Encoder;
22use bincode::error::DecodeError;
23use bincode::error::EncodeError;
24use cu29::prelude::*;
25use cu29::units::si::f64::Time;
26use cu29::units::si::time::second;
27use serde::Deserialize;
28use serde::Serialize;
29
30mod serde_arrays;
31
32/// Health reported by the IO device behind a bridge.
33#[derive(
34    Default, Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
35)]
36#[serde(rename_all = "snake_case")]
37pub enum CuIoStatus {
38    /// The bridge has not confirmed the IO device yet.
39    #[default]
40    NotReady,
41    /// Input values are valid and outputs are applied.
42    Ready,
43    /// The IO device reports a device-specific fault code.
44    Fault(u32),
45}
46
47/// Digital input levels read by a bridge (`digital_in` channel).
48#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
49#[reflect(no_field_bounds, from_reflect = false)]
50pub struct CuDigitalInputPayload<const N: usize> {
51    pub status: CuIoStatus,
52    /// Level of each input; `true` is the active (energized) level.
53    #[serde(with = "crate::serde_arrays::bool_array")]
54    pub values: [bool; N],
55}
56
57impl<const N: usize> Default for CuDigitalInputPayload<N> {
58    fn default() -> Self {
59        Self {
60            status: CuIoStatus::NotReady,
61            values: [false; N],
62        }
63    }
64}
65
66/// Digital output command for a bridge (`digital_out` channel).
67///
68/// Only outputs whose `mask` bit is set are driven to `values`; the bridge
69/// leaves the others at their current level, so a command never touches IO
70/// it does not own.
71#[derive(Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
72#[reflect(no_field_bounds, from_reflect = false)]
73pub struct CuDigitalOutputPayload<const N: usize> {
74    #[serde(with = "crate::serde_arrays::bool_array")]
75    pub values: [bool; N],
76    #[serde(with = "crate::serde_arrays::bool_array")]
77    pub mask: [bool; N],
78}
79
80impl<const N: usize> Default for CuDigitalOutputPayload<N> {
81    fn default() -> Self {
82        Self {
83            values: [false; N],
84            mask: [false; N],
85        }
86    }
87}
88
89/// Requested gripper action.
90#[derive(
91    Default, Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
92)]
93#[serde(rename_all = "snake_case")]
94pub enum CuGripperAction {
95    #[default]
96    Open,
97    Close,
98}
99
100/// Gripper command; an empty message keeps the last action.
101#[derive(Default, Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
102pub struct CuGripperCommandPayload {
103    pub action: CuGripperAction,
104}
105
106/// Gripper state derived from the commanded action and its sensors.
107#[derive(
108    Default, Debug, Clone, Copy, PartialEq, Eq, Encode, Decode, Serialize, Deserialize, Reflect,
109)]
110#[serde(rename_all = "snake_case")]
111pub enum CuGripperState {
112    /// No action has been commanded yet; outputs are not driven.
113    #[default]
114    Unknown,
115    Opening,
116    Open,
117    Closing,
118    Closed,
119    /// IO not ready or faulted, contradictory sensors, or the motion did not
120    /// reach its sensor in time. Outputs hold their last commanded levels.
121    Fault,
122}
123
124/// Gripper state reported every cycle.
125#[derive(Default, Debug, Clone, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
126pub struct CuGripperStatePayload {
127    pub state: CuGripperState,
128}
129
130/// Parameters of a gripper wired to digital IO; this is the gripper task's
131/// node `config:` block.
132///
133/// `close_output` is required. `open_output` is set for double-acting
134/// actuators (two valves or an explicit open line); otherwise opening means
135/// releasing `close_output`. Without a sensor for the target position the
136/// motion counts as done after `settle`; with one it must assert within
137/// `motion_timeout`. Indices count from 0 and must address the task's `IN`
138/// inputs / `OUT` outputs.
139///
140/// RON (times in seconds):
141///
142/// ```ron
143/// config: {
144///     "close_output": 0,
145///     "open_output": 1,      // optional
146///     "closed_input": 0,     // optional
147///     "open_input": 1,       // optional
148///     "settle": 0.3,         // default 0.3 s
149///     "motion_timeout": 2.0, // default 2 s
150/// }
151/// ```
152///
153/// ```
154/// use repose_io::{CuGripperParams, CuGripperTask};
155/// use cu29::units::si::f64::Time;
156/// use cu29::units::si::time::second;
157///
158/// let params = CuGripperParams {
159///     open_output: Some(1),
160///     motion_timeout: Time::new::<second>(1.0),
161///     ..CuGripperParams::single_acting(0)
162/// };
163/// let task = CuGripperTask::<2, 2>::from_params(params).unwrap();
164/// # let _ = task;
165/// ```
166#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
167#[serde(default, deny_unknown_fields)]
168pub struct CuGripperParams {
169    /// Output that closes the gripper (required).
170    pub close_output: Option<usize>,
171    /// Output that opens the gripper (double-acting actuators).
172    pub open_output: Option<usize>,
173    /// Input asserted when the gripper is closed.
174    pub closed_input: Option<usize>,
175    /// Input asserted when the gripper is open.
176    pub open_input: Option<usize>,
177    /// Time after which a motion without a target sensor counts as done.
178    pub settle: Time,
179    /// Time within which a target sensor must assert.
180    pub motion_timeout: Time,
181}
182
183impl Default for CuGripperParams {
184    /// No close output (rejected by [`validate`](Self::validate)), 0.3 s
185    /// settle, 2 s sensor timeout.
186    fn default() -> Self {
187        Self {
188            close_output: None,
189            open_output: None,
190            closed_input: None,
191            open_input: None,
192            settle: Time::new::<second>(0.3),
193            motion_timeout: Time::new::<second>(2.0),
194        }
195    }
196}
197
198impl CuGripperParams {
199    /// Params with only a close output and default timings (0.3 s settle,
200    /// 2 s sensor timeout).
201    pub fn single_acting(close_output: usize) -> Self {
202        Self {
203            close_output: Some(close_output),
204            ..Self::default()
205        }
206    }
207
208    /// Checks that `close_output` is set, indices fit `IN` inputs / `OUT`
209    /// outputs and are not shared, and timings are positive.
210    pub fn validate<const IN: usize, const OUT: usize>(&self) -> CuResult<()> {
211        let close_output = self
212            .close_output
213            .ok_or_else(|| CuError::from("repose-io: gripper needs 'close_output'"))?;
214        let output_ok = |i: usize| i < OUT;
215        let input_ok = |i: usize| i < IN;
216        let positive = |t: Time| t.get::<second>() > 0.0;
217        if !output_ok(close_output)
218            || self
219                .open_output
220                .is_some_and(|o| !output_ok(o) || o == close_output)
221            || self.closed_input.is_some_and(|i| !input_ok(i))
222            || self.open_input.is_some_and(|i| !input_ok(i))
223            || self
224                .closed_input
225                .is_some_and(|i| self.open_input == Some(i))
226            || !positive(self.settle)
227            || !positive(self.motion_timeout)
228        {
229            return Err(CuError::from(
230                "repose-io: gripper indices out of range, shared, or non-positive timings",
231            ));
232        }
233        Ok(())
234    }
235}
236
237/// Gripper driven through digital IO.
238///
239/// Inputs are `(command, digital_in)`; outputs are `(digital_out, state)`.
240/// Only the configured output bits are masked in the output command, so the
241/// task can share a bridge's `digital_out` with other IO users.
242///
243/// The node `config:` block is a [`CuGripperParams`].
244#[derive(Reflect)]
245#[reflect(no_field_bounds, from_reflect = false)]
246pub struct CuGripperTask<const IN: usize, const OUT: usize> {
247    #[reflect(ignore)]
248    params: CuGripperParams,
249    #[reflect(ignore)]
250    close_output: usize,
251    #[reflect(ignore)]
252    settle_ns: u64,
253    #[reflect(ignore)]
254    motion_timeout_ns: u64,
255    #[reflect(ignore)]
256    action: Option<CuGripperAction>,
257    #[reflect(ignore)]
258    state: CuGripperState,
259    motion_start: Option<CuTime>,
260}
261
262/// Converts a positive time to nanoseconds, rounded to nearest and at
263/// least 1 ns.
264fn to_nanos(t: Time) -> u64 {
265    // `as` saturates for values beyond `u64::MAX`.
266    ((t.get::<second>() * 1e9 + 0.5) as u64).max(1)
267}
268
269impl<const IN: usize, const OUT: usize> CuGripperTask<IN, OUT> {
270    /// Creates a gripper task from validated params.
271    pub fn from_params(params: CuGripperParams) -> CuResult<Self> {
272        params.validate::<IN, OUT>()?;
273        Ok(Self {
274            close_output: params.close_output.unwrap_or_default(),
275            settle_ns: to_nanos(params.settle),
276            motion_timeout_ns: to_nanos(params.motion_timeout),
277            params,
278            action: None,
279            state: CuGripperState::Unknown,
280            motion_start: None,
281        })
282    }
283
284    /// Params this task was built from.
285    pub fn params(&self) -> &CuGripperParams {
286        &self.params
287    }
288
289    /// Current gripper state.
290    pub fn state(&self) -> CuGripperState {
291        self.state
292    }
293
294    fn sensor(&self, input: Option<usize>, values: &[bool; IN]) -> Option<bool> {
295        input.map(|i| values[i])
296    }
297}
298
299impl<const IN: usize, const OUT: usize> Freezable for CuGripperTask<IN, OUT> {
300    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
301        Encode::encode(&self.action, encoder)?;
302        Encode::encode(&self.state, encoder)?;
303        Encode::encode(&self.motion_start, encoder)
304    }
305
306    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
307        self.action = Decode::decode(decoder)?;
308        self.state = Decode::decode(decoder)?;
309        self.motion_start = Decode::decode(decoder)?;
310        Ok(())
311    }
312}
313
314impl<const IN: usize, const OUT: usize> CuTask for CuGripperTask<IN, OUT> {
315    type Resources<'r> = ();
316    type Input<'m> = input_msg!('m, CuGripperCommandPayload, CuDigitalInputPayload<IN>);
317    type Output<'m> = output_msg!(CuDigitalOutputPayload<OUT>, CuGripperStatePayload);
318
319    fn new(cfg: Option<&ComponentConfig>, _res: Self::Resources<'_>) -> CuResult<Self>
320    where
321        Self: Sized,
322    {
323        let params = cfg
324            .map(ComponentConfig::deserialize_into::<CuGripperParams>)
325            .transpose()?
326            .unwrap_or_default();
327        Self::from_params(params)
328    }
329
330    fn process<'i, 'o>(
331        &mut self,
332        ctx: &CuContext,
333        input: &Self::Input<'i>,
334        output: &mut Self::Output<'o>,
335    ) -> CuResult<()> {
336        let (command, io) = *input;
337        let (digital_out, state_out) = output;
338        digital_out.tov = io.tov;
339        state_out.tov = io.tov;
340        let now = ctx.now();
341
342        // A new action restarts the motion; repeating the active one does not.
343        if let Some(command) = command.payload()
344            && self.action != Some(command.action)
345        {
346            self.action = Some(command.action);
347            self.motion_start = Some(now);
348            self.state = match command.action {
349                CuGripperAction::Open => CuGripperState::Opening,
350                CuGripperAction::Close => CuGripperState::Closing,
351            };
352        }
353
354        let io_ready = io.payload().filter(|io| io.status == CuIoStatus::Ready);
355        self.state = match (self.action, io_ready) {
356            (None, _) => CuGripperState::Unknown,
357            (Some(_), None) => CuGripperState::Fault,
358            // A fault latches until a new action is commanded.
359            (Some(_), Some(_)) if self.state == CuGripperState::Fault => CuGripperState::Fault,
360            (Some(action), Some(io)) => {
361                let closed = self.sensor(self.params.closed_input, &io.values);
362                let open = self.sensor(self.params.open_input, &io.values);
363                let (reached, other, done, moving) = match action {
364                    CuGripperAction::Close => (
365                        closed,
366                        open,
367                        CuGripperState::Closed,
368                        CuGripperState::Closing,
369                    ),
370                    CuGripperAction::Open => {
371                        (open, closed, CuGripperState::Open, CuGripperState::Opening)
372                    }
373                };
374                let elapsed = self
375                    .motion_start
376                    .map_or(0, |start| now.as_nanos().saturating_sub(start.as_nanos()));
377                if closed == Some(true) && open == Some(true) {
378                    CuGripperState::Fault
379                } else {
380                    match reached {
381                        Some(true) => done,
382                        Some(false) if elapsed >= self.motion_timeout_ns => CuGripperState::Fault,
383                        Some(false) => moving,
384                        // No target sensor: trust the settle time, but an
385                        // opposite sensor still asserted after it is a jam.
386                        None if elapsed >= self.settle_ns => {
387                            if other == Some(true) {
388                                CuGripperState::Fault
389                            } else {
390                                done
391                            }
392                        }
393                        None => moving,
394                    }
395                }
396            }
397        };
398
399        match self.action {
400            None => digital_out.clear_payload(),
401            Some(action) => {
402                let close = action == CuGripperAction::Close;
403                let mut command = CuDigitalOutputPayload::<OUT>::default();
404                command.values[self.close_output] = close;
405                command.mask[self.close_output] = true;
406                if let Some(open_output) = self.params.open_output {
407                    command.values[open_output] = !close;
408                    command.mask[open_output] = true;
409                }
410                digital_out.set_payload(command);
411            }
412        }
413        state_out.set_payload(CuGripperStatePayload { state: self.state });
414        Ok(())
415    }
416}
417
418#[cfg(test)]
419mod tests {
420    use super::*;
421
422    fn params() -> CuGripperParams {
423        CuGripperParams {
424            open_output: Some(1),
425            closed_input: Some(0),
426            ..CuGripperParams::single_acting(0)
427        }
428    }
429
430    fn step(
431        task: &mut CuGripperTask<2, 2>,
432        ctx: &CuContext,
433        action: Option<CuGripperAction>,
434        io: Option<CuDigitalInputPayload<2>>,
435    ) -> (Option<CuDigitalOutputPayload<2>>, CuGripperState) {
436        let command = CuMsg::new(action.map(|action| CuGripperCommandPayload { action }));
437        let io = CuMsg::new(io);
438        let input = (&command, &io);
439        let mut output = (
440            CuMsg::<CuDigitalOutputPayload<2>>::default(),
441            CuMsg::<CuGripperStatePayload>::default(),
442        );
443        task.process(ctx, &input, &mut output).unwrap();
444        (
445            output.0.payload().cloned(),
446            output.1.payload().unwrap().state,
447        )
448    }
449
450    fn ready(closed: bool) -> Option<CuDigitalInputPayload<2>> {
451        Some(CuDigitalInputPayload {
452            status: CuIoStatus::Ready,
453            values: [closed, false],
454        })
455    }
456
457    #[test]
458    fn close_drives_masked_outputs_until_sensor_confirms() {
459        let mut task = CuGripperTask::<2, 2>::from_params(params()).unwrap();
460        let (ctx, _clock) = CuContext::new_mock_clock();
461        let (out, state) = step(&mut task, &ctx, None, ready(false));
462        assert!(out.is_none());
463        assert_eq!(state, CuGripperState::Unknown);
464        let (out, state) = step(&mut task, &ctx, Some(CuGripperAction::Close), ready(false));
465        let out = out.unwrap();
466        assert_eq!(out.values, [true, false]);
467        assert_eq!(out.mask, [true, true]);
468        assert_eq!(state, CuGripperState::Closing);
469        let (_, state) = step(&mut task, &ctx, None, ready(true));
470        assert_eq!(state, CuGripperState::Closed);
471    }
472
473    #[test]
474    fn missing_sensor_times_out_and_io_fault_latches() {
475        let mut task = CuGripperTask::<2, 2>::from_params(CuGripperParams {
476            motion_timeout: Time::new::<second>(1e-6),
477            ..params()
478        })
479        .unwrap();
480        let (ctx, clock) = CuContext::new_mock_clock();
481        step(&mut task, &ctx, Some(CuGripperAction::Close), ready(false));
482        clock.increment(CuDuration::from_nanos(2_000));
483        let (_, state) = step(&mut task, &ctx, None, ready(false));
484        assert_eq!(state, CuGripperState::Fault);
485        let (out, state) = step(&mut task, &ctx, None, ready(true));
486        assert_eq!(state, CuGripperState::Fault);
487        assert_eq!(out.unwrap().values, [true, false]);
488        let (_, state) = step(&mut task, &ctx, Some(CuGripperAction::Open), None);
489        assert_eq!(state, CuGripperState::Fault);
490    }
491
492    #[test]
493    fn rejects_out_of_range_wiring() {
494        assert!(CuGripperTask::<2, 2>::from_params(CuGripperParams::single_acting(2)).is_err());
495        assert!(
496            CuGripperTask::<2, 2>::from_params(CuGripperParams {
497                open_output: Some(0),
498                ..CuGripperParams::single_acting(0)
499            })
500            .is_err()
501        );
502    }
503
504    #[test]
505    fn missing_close_output_is_rejected() {
506        assert!(CuGripperTask::<2, 2>::new(None, ()).is_err());
507        assert!(CuGripperParams::default().validate::<2, 2>().is_err());
508        assert!(
509            CuGripperTask::<2, 2>::from_params(CuGripperParams {
510                settle: Time::new::<second>(0.0),
511                ..CuGripperParams::single_acting(0)
512            })
513            .is_err()
514        );
515    }
516
517    #[test]
518    fn default_timings_match_previous_nanoseconds() {
519        let task = CuGripperTask::<2, 2>::from_params(params()).unwrap();
520        assert_eq!(task.settle_ns, 300_000_000);
521        assert_eq!(task.motion_timeout_ns, 2_000_000_000);
522    }
523}
repose_io/lib.rs

repose_io/
lib.rs
