1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

use std::time::{Duration, Instant};

use super::{instruction_id, packet_id};
use crate::bus::StatusPacket;
use crate::{Bus, ReadError, Response, TransferError, WriteError};

/// A response from a motor to a ping instruction.
#[derive(Debug, Clone, Eq, PartialEq)]
pub struct Ping {
	/// The model of the motor.
	///
	/// Refer to the online manual to find the codes for each model.
	pub model: u16,

	/// The firmware version of the motor.
	pub firmware: u8,
}

impl<'a> TryFrom<StatusPacket<'a>> for Response<Ping> {
	type Error = crate::InvalidParameterCount;

	fn try_from(status_packet: StatusPacket<'a>) -> Result<Self, Self::Error> {
		let parameters = status_packet.parameters();
		crate::InvalidParameterCount::check(parameters.len(), 3)?;
		Ok(Self {
			motor_id: status_packet.packet_id(),
			alert: status_packet.alert(),
			data: Ping {
				model: crate::endian::read_u16_le(&parameters[0..]),
				firmware: crate::endian::read_u8_le(&parameters[2..]),
			},
		})
	}
}

impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
where
	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
{
	/// Ping a specific motor by ID.
	///
	/// This will not work correctly if the motor ID is [`packet_id::BROADCAST`].
	/// Use [`Self::scan`] or [`Self::scan_cb`] instead.
	pub fn ping(&mut self, motor_id: u8) -> Result<Response<Ping>, TransferError> {
		let response = self.transfer_single(motor_id, instruction_id::PING, 0, 3, |_| ())?;
		Ok(response.try_into()?)
	}

	/// Scan a bus for motors with a broadcast ping, returning the responses in a [`Vec`].
	///
	/// Only timeouts are filtered out since they indicate a lack of response.
	/// All other responses (including errors) are collected.
	pub fn scan(&mut self) -> Result<Vec<Result<Response<Ping>, ReadError>>, WriteError> {
		let mut result = Vec::with_capacity(253);
		match self.scan_cb(|x| result.push(Ok(x))) {
			Ok(()) => (),
			Err(TransferError::WriteError(e)) => return Err(e),
			Err(TransferError::ReadError(e)) => {
				result.push(Err(e));
			}
		}
		Ok(result)
	}

	/// Scan a bus for motors with a broadcast ping, calling an [`FnMut`] for each response.
	///
	/// Only timeouts are filtered out since they indicate a lack of response.
	/// All other responses (including errors) are passed to the handler.
	pub fn scan_cb<F>(&mut self, mut on_response: F) -> Result<(), TransferError>
	where
		F: FnMut(Response<Ping>),
	{
		self.write_instruction(packet_id::BROADCAST, instruction_id::PING, 0, |_| ())?;
		let response_time = crate::bus::message_transfer_time(14, self.baud_rate());
		let timeout = response_time * 253 + Duration::from_millis(34);
		let deadline = Instant::now() + timeout;

		loop {
			let response = self.read_status_response_deadline(deadline);
			match response {
				Ok(response) => {
					let response = response.try_into()?;
					on_response(response);
				},
				Err(e) => {
					if let ReadError::Io(e) = &e {
						if e.kind() == std::io::ErrorKind::TimedOut {
							trace!("Ping response timed out.");
							return Ok(());
						}
					}
					return Err(e.into())
				},
			}
		}
	}
}