dynamixel2/instructions/
sync_read.rs

1use super::{instruction_id, packet_id};
2use crate::endian::write_u16_le;
3use crate::{Bus, ReadError, Response, TransferError, WriteError};
4
5impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
6where
7	ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
8	WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
9{
10	/// Synchronously read an arbitrary number of bytes from multiple motors in one command.
11	///
12	/// The `on_response` function is called for the reply from each motor.
13	/// If the function fails to write the instruction, an error is returned and the function is not called.
14	pub fn sync_read_cb<'a, F>(&'a mut self, motor_ids: &'a [u8], address: u16, count: u16, mut on_response: F) -> Result<(), WriteError>
15	where
16		F: FnMut(Result<Response<&[u8]>, ReadError>),
17	{
18		self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_READ, 4 + motor_ids.len(), |buffer| {
19			write_u16_le(&mut buffer[0..], address);
20			write_u16_le(&mut buffer[2..], count);
21			buffer[4..].copy_from_slice(motor_ids);
22		})?;
23		for &motor_id in motor_ids {
24			let response = self.read_status_response(count).and_then(|response| {
25				crate::InvalidPacketId::check(response.packet_id(), motor_id)?;
26				crate::InvalidParameterCount::check(response.parameters().len(), count.into())?;
27				Ok(response)
28			});
29
30			match response {
31				Ok(response) => on_response(Ok((&response).into())),
32				Err(e) => on_response(Err(e)),
33			}
34		}
35		Ok(())
36	}
37
38	/// Synchronously read an 8 bit value from multiple motors in one command.
39	///
40	/// The `on_response` function is called for the reply from each motor.
41	/// If the function fails to write the instruction, an error is returned and the function is not called.
42	pub fn sync_read_u8_cb<'a, F>(&'a mut self, motor_ids: &'a [u8], address: u16, mut on_response: F) -> Result<(), WriteError>
43	where
44		F: FnMut(Result<Response<u8>, ReadError>),
45	{
46		let count = 1;
47		self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_READ, 4 + motor_ids.len(), |buffer| {
48			write_u16_le(&mut buffer[0..], address);
49			write_u16_le(&mut buffer[2..], count);
50			buffer[4..].copy_from_slice(motor_ids);
51		})?;
52		for &motor_id in motor_ids {
53			let data = self.read_status_response(count).and_then(|response| {
54				crate::InvalidPacketId::check(response.packet_id(), motor_id)?;
55				Ok(response.try_into()?)
56			});
57			on_response(data);
58		}
59		Ok(())
60	}
61
62	/// Synchronously read a 16 bit value from multiple motors in one command.
63	///
64	/// The `on_response` function is called for the reply from each motor.
65	/// If the function fails to write the instruction, an error is returned and the function is not called.
66	pub fn sync_read_u16_cb<'a, F>(&'a mut self, motor_ids: &'a [u8], address: u16, mut on_response: F) -> Result<(), WriteError>
67	where
68		F: FnMut(Result<Response<u16>, ReadError>),
69	{
70		let count = 2;
71		self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_READ, 4 + motor_ids.len(), |buffer| {
72			write_u16_le(&mut buffer[0..], address);
73			write_u16_le(&mut buffer[2..], count);
74			buffer[4..].copy_from_slice(motor_ids);
75		})?;
76		for &motor_id in motor_ids {
77			let data = self.read_status_response(count).and_then(|response| {
78				crate::InvalidPacketId::check(response.packet_id(), motor_id)?;
79				Ok(response.try_into()?)
80			});
81			on_response(data);
82		}
83		Ok(())
84	}
85
86	/// Synchronously read a 32 bit value from multiple motors in one command.
87	///
88	/// The `on_response` function is called for the reply from each motor.
89	/// If the function fails to write the instruction, an error is returned and the function is not called.
90	pub fn sync_read_u32_cb<'a, F>(&'a mut self, motor_ids: &'a [u8], address: u16, mut on_response: F) -> Result<(), WriteError>
91	where
92		F: FnMut(Result<Response<u32>, ReadError>),
93	{
94		let count = 4;
95		self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_READ, 4 + motor_ids.len(), |buffer| {
96			write_u16_le(&mut buffer[0..], address);
97			write_u16_le(&mut buffer[2..], count);
98			buffer[4..].copy_from_slice(motor_ids);
99		})?;
100		for &motor_id in motor_ids {
101			let data = self.read_status_response(count).and_then(|response| {
102				crate::InvalidPacketId::check(response.packet_id(), motor_id)?;
103				crate::InvalidParameterCount::check(response.parameters().len(), count.into())?;
104				Ok(response.try_into()?)
105			});
106			on_response(data);
107		}
108		Ok(())
109	}
110
111	/// Synchronously read an arbitrary number of bytes from multiple motors in one command.
112	///
113	/// If this function fails to get the data from any of the motors, the entire function retrns an error.
114	/// If you need access to the data from other motors, or if you want acces to the error for each motor, see [`Self::sync_read_cb`].
115	pub fn sync_read<'a>(&'a mut self, motor_ids: &'a [u8], address: u16, count: u16) -> Result<Vec<Response<Vec<u8>>>, TransferError> {
116		let mut result = Vec::with_capacity(motor_ids.len());
117		let mut read_error = None;
118		self.sync_read_cb(motor_ids, address, count, |data| match data {
119			Err(e) if read_error.is_none() => read_error = Some(e),
120			Err(_) => (),
121			Ok(response) => result.push(Response {
122				motor_id: response.motor_id,
123				alert: response.alert,
124				data: response.data.to_owned(),
125			}),
126		})?;
127		Ok(result)
128	}
129
130	/// Synchronously read an 8 bit value from multiple motors in one command.
131	///
132	/// If this function fails to get the data from any of the motors, the entire function retrns an error.
133	/// If you need access to the data from other motors, or if you want acces to the error for each motor, see [`Self::sync_read_u8_cb`].
134	pub fn sync_read_u8<'a>(&'a mut self, motor_ids: &'a [u8], address: u16) -> Result<Vec<Response<u8>>, TransferError> {
135		let mut result = Vec::with_capacity(motor_ids.len());
136		let mut read_error = None;
137		self.sync_read_u8_cb(motor_ids, address, |data| match data {
138			Err(e) if read_error.is_none() => read_error = Some(e),
139			Err(_) => (),
140			Ok(data) => result.push(data),
141		})?;
142		Ok(result)
143	}
144
145	/// Synchronously read a 16 bit value from multiple motors in one command.
146	///
147	/// If this function fails to get the data from any of the motors, the entire function retrns an error.
148	/// If you need access to the data from other motors, or if you want acces to the error for each motor, see [`Self::sync_read_u16_cb`].
149	pub fn sync_read_u16<'a>(&'a mut self, motor_ids: &'a [u8], address: u16) -> Result<Vec<Response<u16>>, TransferError> {
150		let mut result = Vec::with_capacity(motor_ids.len());
151		let mut read_error = None;
152		self.sync_read_u16_cb(motor_ids, address, |data| match data {
153			Err(e) if read_error.is_none() => read_error = Some(e),
154			Err(_) => (),
155			Ok(data) => result.push(data),
156		})?;
157		Ok(result)
158	}
159
160	/// Synchronously read a 32 bit value from multiple motors in one command.
161	///
162	/// If this function fails to get the data from any of the motors, the entire function retrns an error.
163	/// If you need access to the data from other motors, or if you want acces to the error for each motor, see [`Self::sync_read_u32_cb`].
164	pub fn sync_read_u32<'a>(&'a mut self, motor_ids: &'a [u8], address: u16) -> Result<Vec<Response<u32>>, TransferError> {
165		let mut result = Vec::with_capacity(motor_ids.len());
166		let mut read_error = None;
167		self.sync_read_u32_cb(motor_ids, address, |data| match data {
168			Err(e) if read_error.is_none() => read_error = Some(e),
169			Err(_) => (),
170			Ok(data) => result.push(data),
171		})?;
172		Ok(result)
173	}
174}
dynamixel2/instructions/sync_read.rs

dynamixel2/instructions/
sync_read.rs
