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
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
use super::{instruction_id, packet_id, BulkWriteData};
use crate::endian::{write_u16_le, write_u8_le};
use crate::{Bus, WriteError};
impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
where
ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
{
/// Synchronously write arbitrary data ranges to multiple motors.
///
/// Each motor will perform the write as soon as it receives the command.
/// This gives much shorter delays than executing a regular [`Self::write`] for each motor individually.
/// Unlike the sync write instruction, a bulk write allows you to write a different amount of data to a different address for each motor.
///
/// The data for multi-byte registers should serialized as little-endian.
///
/// # Panics
/// The protocol forbids specifying the same motor ID multiple times.
/// This function panics if the same motor ID is used for more than one write.
///
/// This function also panics if the data length for a motor exceeds the capacity of a `u16`.
///
/// # Example
/// ```no_run
/// # fn main() -> Result<(), Box<dyn std::error::Error>> {
/// use dynamixel2::Bus;
/// use dynamixel2::instructions::BulkWriteData;
/// use std::time::Duration;
///
/// let mut bus = Bus::open("/dev/ttyUSB0", 57600)?;
/// bus.bulk_write(&[
/// // Write a u32 value of 2000 to register 116 of motor 1.
/// BulkWriteData {
/// motor_id: 1,
/// address: 116,
/// data: 2000u32.to_le_bytes().as_slice(),
/// },
/// // Write a u16 value of 300 to register 102 of motor 2.
/// BulkWriteData {
/// motor_id: 2,
/// address: 102,
/// data: 300u16.to_le_bytes().as_slice(),
/// },
/// ])?;
/// # Ok(())
/// # }
/// ```
pub fn bulk_write<'a, I, T>(&mut self, writes: &'a I) -> Result<(), WriteError>
where
&'a I: IntoIterator,
<&'a I as IntoIterator>::IntoIter: Clone,
<&'a I as IntoIterator>::Item: std::borrow::Borrow<BulkWriteData<T>>,
T: AsRef<[u8]>,
{
use std::borrow::Borrow;
let writes = writes.into_iter();
let mut parameter_count = 0;
for write in writes.clone() {
let write = write.borrow();
let data = write.data.as_ref();
if data.len() > u16::MAX.into() {
panic!(
"bulk_write: data length ({}) for motor {} exceeds maximum size of {}",
data.len(),
write.motor_id,
u16::MAX
);
}
parameter_count += 5 + data.len();
}
self.write_instruction(packet_id::BROADCAST, instruction_id::BULK_WRITE, parameter_count, |buffer| {
let mut offset = 0;
for write in writes {
let write = write.borrow();
let data = write.data.as_ref();
let buffer = &mut buffer[offset..];
offset += 5 + data.len();
write_u8_le(&mut buffer[0..], write.motor_id);
write_u16_le(&mut buffer[1..], write.address);
write_u16_le(&mut buffer[3..], data.len() as u16);
buffer[5..][..data.len()].copy_from_slice(data);
}
})
}
}
#[cfg(test)]
mod tests {
use super::*;
/// Ensure that `bulk_write` accepts a slice of `BulkWriteData`.
///
/// This is a compile test. It only tests that the test code compiles.
#[allow(dead_code)]
fn bulk_write_accepts_slice(bus: &mut Bus<Vec<u8>, Vec<u8>>) -> Result<(), Box<dyn std::error::Error>> {
bus.bulk_write(&[
BulkWriteData {
motor_id: 1,
address: 116,
data: 2000u32.to_le_bytes().as_slice(),
},
BulkWriteData {
motor_id: 2,
address: 102,
data: 300u16.to_le_bytes().as_slice(),
},
])?;
Ok(())
}
/// Ensure that `bulk_write` accepts a reference to a Vec of `BulkWriteData`.
///
/// This is a compile test. It only tests that the test code compiles.
#[allow(dead_code)]
fn bulk_write_accepts_vec_ref(bus: &mut Bus<Vec<u8>, Vec<u8>>) -> Result<(), Box<dyn std::error::Error>> {
bus.bulk_write(&vec![
BulkWriteData {
motor_id: 1,
address: 116,
data: 2000u32.to_le_bytes().as_slice(),
},
BulkWriteData {
motor_id: 2,
address: 102,
data: 300u16.to_le_bytes().as_slice(),
},
])?;
Ok(())
}
/// Ensure that `bulk_write` accepts a reference to a Vec and doesn't clone the data in the vector.
///
/// This is a compile test. It only tests that the test code compiles.
#[allow(dead_code)]
fn bulk_write_accepts_vec_ref_no_clone(bus: &mut Bus<Vec<u8>, Vec<u8>>) -> Result<(), Box<dyn std::error::Error>> {
/// Non-clonable wrapper around `&[u8]` to ensure `bulk_write` doesn't clone data from vec references.
struct Data<'a> {
data: &'a [u8],
}
impl AsRef<[u8]> for Data<'_> {
fn as_ref(&self) -> &[u8] {
self.data
}
}
impl<'a> Data<'a> {
fn new<const N: usize>(data: &'a [u8; N]) -> Self {
Self {
data: data.as_slice(),
}
}
}
bus.bulk_write(&vec![
BulkWriteData {
motor_id: 1,
address: 116,
data: Data::new(&2000u32.to_le_bytes()),
},
BulkWriteData {
motor_id: 2,
address: 102,
data: Data::new(&300u16.to_le_bytes()),
},
])?;
Ok(())
}
}