use super::{instruction_id, packet_id, SyncWriteData};
use crate::endian::{write_u16_le, write_u32_le};
use crate::{Bus, WriteError};
impl<ReadBuffer, WriteBuffer> Bus<ReadBuffer, WriteBuffer>
where
ReadBuffer: AsRef<[u8]> + AsMut<[u8]>,
WriteBuffer: AsRef<[u8]> + AsMut<[u8]>,
{
pub fn sync_write<'a, Iter, Data, Buf>(&mut self, address: u16, count: u16, data: Iter) -> Result<(), WriteError>
where
Iter: IntoIterator<Item = Data>,
Iter::IntoIter: std::iter::ExactSizeIterator,
Data: AsRef<SyncWriteData<Buf>>,
Buf: AsRef<[u8]> + 'a,
{
let data = data.into_iter();
let motors = data.len();
let stride = 1 + usize::from(count);
let parameter_count = 4 + motors * stride;
self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_WRITE, parameter_count, |buffer| {
write_u16_le(&mut buffer[0..], address);
write_u16_le(&mut buffer[2..], count);
for (i, command) in data.enumerate() {
let command = command.as_ref();
assert_eq!(command.data.as_ref().len(), count as usize);
let buffer = &mut buffer[4 + i * stride..][..stride];
buffer[0] = command.motor_id;
buffer[1..].copy_from_slice(command.data.as_ref());
}
})
}
pub fn sync_write_u8<Iter, Data>(&mut self, address: u16, data: Iter) -> Result<(), WriteError>
where
Iter: IntoIterator<Item = Data>,
Iter::IntoIter: std::iter::ExactSizeIterator,
Data: AsRef<SyncWriteData<u8>>,
{
let data = data.into_iter();
let count = core::mem::size_of::<u8>();
let motors = data.len();
let stride = 1 + count;
let parameter_count = 4 + motors * stride;
self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_WRITE, parameter_count, |buffer| {
write_u16_le(&mut buffer[0..], address);
write_u16_le(&mut buffer[2..], count as u16);
for (i, command) in data.enumerate() {
let command = command.as_ref();
let buffer = &mut buffer[4 + i * stride..][..stride];
buffer[0] = command.motor_id;
buffer[1] = command.data;
}
})
}
pub fn sync_write_u16<Iter, Data>(&mut self, address: u16, data: Iter) -> Result<(), WriteError>
where
Iter: IntoIterator<Item = Data>,
Iter::IntoIter: std::iter::ExactSizeIterator,
Data: AsRef<SyncWriteData<u16>>,
{
let data = data.into_iter();
let count = core::mem::size_of::<u16>();
let motors = data.len();
let stride = 1 + count;
let parameter_count = 4 + motors * stride;
self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_WRITE, parameter_count, |buffer| {
write_u16_le(&mut buffer[0..], address);
write_u16_le(&mut buffer[2..], count as u16);
for (i, command) in data.enumerate() {
let command = command.as_ref();
let buffer = &mut buffer[4 + i * stride..][..stride];
buffer[0] = command.motor_id;
write_u16_le(&mut buffer[1..], command.data);
}
})
}
pub fn sync_write_u32<Iter, Data>(&mut self, address: u16, data: Iter) -> Result<(), WriteError>
where
Iter: IntoIterator<Item = Data>,
Iter::IntoIter: std::iter::ExactSizeIterator,
Data: AsRef<SyncWriteData<u32>>,
{
let data = data.into_iter();
let count = core::mem::size_of::<u32>();
let motors = data.len();
let stride = 1 + count;
let parameter_count = 4 + motors * stride;
self.write_instruction(packet_id::BROADCAST, instruction_id::SYNC_WRITE, parameter_count, |buffer| {
write_u16_le(&mut buffer[0..], address);
write_u16_le(&mut buffer[2..], count as u16);
for (i, command) in data.enumerate() {
let command = command.as_ref();
let buffer = &mut buffer[4 + i * stride..][..stride];
buffer[0] = command.motor_id;
write_u32_le(&mut buffer[1..], command.data);
}
})
}
}