dynamixel2/instructions/
ping.rs

1use std::time::{Duration, Instant};
2
3use super::{instruction_id, packet_id};
4use crate::bus::StatusPacket;
5use crate::{Bus, ReadError, Response, TransferError, WriteError};
6
7/// A response from a motor to a ping instruction.
8#[derive(Debug, Clone, Eq, PartialEq)]
9pub struct Ping {
10	/// The model of the motor.
11	///
12	/// Refer to the online manual to find the codes for each model.
13	pub model: u16,
14
15	/// The firmware version of the motor.
16	pub firmware: u8,
17}
18
19impl<'a> TryFrom<StatusPacket<'a>> for Response<Ping> {
20	type Error = crate::InvalidParameterCount;
21
22	fn try_from(status_packet: StatusPacket<'a>) -> Result<Self, Self::Error> {
23		let parameters = status_packet.parameters();
24		crate::InvalidParameterCount::check(parameters.len(), 3)?;
25		Ok(Self {
26			motor_id: status_packet.packet_id(),
27			alert: status_packet.alert(),
28			data: Ping {
29				model: crate::endian::read_u16_le(&parameters[0..]),
30				firmware: crate::endian::read_u8_le(&parameters[2..]),
31			},
32		})
33	}
34}
35
36impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
37where
38	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
39	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
40{
41	/// Ping a specific motor by ID.
42	///
43	/// This will not work correctly if the motor ID is [`packet_id::BROADCAST`].
44	/// Use [`Self::scan`] or [`Self::scan_cb`] instead.
45	pub fn ping(&mut self, motor_id: u8) -> Result<Response<Ping>, TransferError> {
46		let response = self.transfer_single(motor_id, instruction_id::PING, 0, 3, |_| ())?;
47		Ok(response.try_into()?)
48	}
49
50	/// Scan a bus for motors with a broadcast ping, returning the responses in a [`Vec`].
51	///
52	/// Only timeouts are filtered out since they indicate a lack of response.
53	/// All other responses (including errors) are collected.
54	pub fn scan(&mut self) -> Result<Vec<Result<Response<Ping>, ReadError>>, WriteError> {
55		let mut result = Vec::with_capacity(253);
56		match self.scan_cb(|x| result.push(Ok(x))) {
57			Ok(()) => (),
58			Err(TransferError::WriteError(e)) => return Err(e),
59			Err(TransferError::ReadError(e)) => {
60				result.push(Err(e));
61			}
62		}
63		Ok(result)
64	}
65
66	/// Scan a bus for motors with a broadcast ping, calling an [`FnMut`] for each response.
67	///
68	/// Only timeouts are filtered out since they indicate a lack of response.
69	/// All other responses (including errors) are passed to the handler.
70	pub fn scan_cb<F>(&mut self, mut on_response: F) -> Result<(), TransferError>
71	where
72		F: FnMut(Response<Ping>),
73	{
74		self.write_instruction(packet_id::BROADCAST, instruction_id::PING, 0, |_| ())?;
75		let response_time = crate::bus::message_transfer_time(14, self.baud_rate());
76		let timeout = response_time * 253 + Duration::from_millis(34);
77		let deadline = Instant::now() + timeout;
78
79		loop {
80			let response = self.read_status_response_deadline(deadline);
81			match response {
82				Ok(response) => {
83					let response = response.try_into()?;
84					on_response(response);
85				},
86				Err(e) => {
87					if let ReadError::Io(e) = &e {
88						if e.kind() == std::io::ErrorKind::TimedOut {
89							trace!("Ping response timed out.");
90							return Ok(());
91						}
92					}
93					return Err(e.into())
94				},
95			}
96		}
97	}
98}
dynamixel2/instructions/ping.rs

dynamixel2/instructions/
ping.rs
