dynamixel/protocol2/
mod.rs

1//! Dynamixel Protocol 2
2//!
3//! Documentation can be found in: http://support.robotis.com/en/product/actuator/dynamixel_pro/communication.html
4
5
6pub(crate) mod instruction;
7#[macro_use]
8mod control_table;
9mod crc;
10mod bit_stuffer;
11
12use Interface;
13use BaudRate;
14use CommunicationError;
15
16use bit_field::BitField;
17use self::bit_stuffer::BitStuffer;
18
19/// Write the instruction on the interface
20pub(crate) fn write_instruction<I: ::Interface, T: Instruction>(interface: &mut I, instruction: T) -> Result<(), CommunicationError> {
21    for b in instruction.serialize() {
22        interface.write(&[b])?
23    }
24    Ok(())
25}
26
27/// Read a status from the interface
28///
29/// If no instructions have been sent, there will not be any status to read
30pub(crate) fn read_status<I: ::Interface, T: Status>(interface: &mut I) -> Result<T, Error> {
31    let mut header = [0u8; 9];
32    interface.read(&mut header)?;
33    
34    let mut deserializer = Deserializer::<T>::new()
35        .deserialize_header(header)?;
36
37    let mut body = [0u8; 10];
38
39    loop {
40        let remaining_bytes = deserializer.remaining_bytes() as usize;
41        if remaining_bytes > 10 {
42            interface.read(&mut body)?;
43            deserializer.deserialize(&body)?;
44        } else {
45            interface.read(&mut body[..remaining_bytes])?;
46            deserializer.deserialize(&body[..remaining_bytes])?;
47            break;
48        }
49    }
50    
51    Ok(deserializer.build()?)
52}
53
54/// Enumerate all protocol 2 servos connected to the interface
55///
56/// This functions returns a Vec and thus requires the `std` feature.
57#[cfg(feature="std")]
58pub fn enumerate<I: ::Interface>(interface: &mut I) -> Result<Vec<ServoInfo>, CommunicationError> {
59    let mut servos = Vec::new();
60
61    for b in BaudRate::variants() {
62
63        if let Err(_) = interface.set_baud_rate(*b) {
64            warn!(target: "protocol2", "not able to enumerate devices on baudrate: {}", u32::from(*b));
65        }
66
67        interface.flush();
68        let ping = ::protocol2::instruction::Ping::new(::protocol2::PacketID::Broadcast);
69        write_instruction(interface, ping)?;
70
71        loop {
72            match read_status::<I, instruction::Pong>(interface) {
73                Ok(pong) => servos.push(
74                    ServoInfo{
75                        baud_rate: *b,
76                        model_number: pong.model_number,
77                        fw_version: pong.fw_version,
78                        id: pong.id,
79                    }
80                ),
81                Err(Error::Communication(CommunicationError::TimedOut)) => break,
82                Err(e) => {
83                    warn!(target: "protocol2", "received error: {:?} when waiting for enumeration on baud: {}", e, u32::from(*b));
84                    break;
85                },
86            };
87        }   
88    }
89    Ok(servos)
90}
91
92/// Connect genericly to a protocol 2 servo
93///
94/// Only offers basic functionality. If you need more functionality use the connect method of the correct servo type instead.
95#[cfg(feature="std")]
96pub fn connect<I: Interface + 'static>(_interface: &mut I, info: ServoInfo) -> Result<Box<::Servo<I>>, CommunicationError>{
97    match info.model_number {
98        ::pro::M4210S260R::<I>::MODEL_NUMBER => Ok(Box::new(::pro::M4210S260R::<I>::new(info.id, info.baud_rate))),
99        _ => unimplemented!(),
100    }
101}
102
103macro_rules! protocol2_servo {
104    ($name:ident, $write:path, $read:path, $model_number:expr) => {
105        pub struct $name<I: ::Interface> {
106            id: ::protocol2::ServoID,
107            baudrate: ::BaudRate,
108            interface: ::lib::marker::PhantomData<I>,
109        }
110
111        impl<I: ::Interface> $name<I> {
112            pub const MODEL_NUMBER: u16 = $model_number;
113
114            /// Create a new servo without `ping`ing or taking any other measure to make sure it exists.
115            pub fn new(id: ::protocol2::ServoID, baudrate: ::BaudRate) -> Self {
116                $name{
117                    id: id,
118                    baudrate: baudrate,
119                    interface: ::lib::marker::PhantomData{},
120                }
121            }
122            
123            /// Ping the servo, returning `Ok(ServoInfo)` if it exists.
124            pub fn ping(&mut self, interface: &mut I) -> Result<::protocol2::ServoInfo, ::protocol2::Error> {
125                interface.set_baud_rate(self.baudrate)?;
126                interface.flush();
127                
128                let ping = ::protocol2::instruction::Ping::new(::protocol2::PacketID::from(self.id));
129                ::protocol2::write_instruction(interface, ping)?;
130                let pong = ::protocol2::read_status::<I, ::protocol2::instruction::Pong>(interface)?;
131                Ok(
132                    ::protocol2::ServoInfo{
133                        baud_rate: self.baudrate,
134                        model_number: pong.model_number,
135                        fw_version: pong.fw_version,
136                        id: pong.id,
137                    }
138                )
139            }
140
141            /// Write the given data `register` to the servo.
142            pub fn write<W: $write>(&mut self, interface: &mut I, register: W) -> Result<(), ::protocol2::Error> {
143                interface.set_baud_rate(self.baudrate)?;
144                let write = ::protocol2::instruction::Write::new(::protocol2::PacketID::from(self.id), register);
145                ::protocol2::write_instruction(interface, write)?;
146                ::protocol2::read_status::<I, ::protocol2::instruction::WriteResponse>(interface)?;
147                Ok(())
148            }
149
150            /// Read data from a register
151            pub fn read<R: $read>(&mut self, interface: &mut I) -> Result<R, ::protocol2::Error> {
152                interface.set_baud_rate(self.baudrate)?;
153                interface.flush();
154                
155                let read = ::protocol2::instruction::Read::<R>::new(::protocol2::PacketID::from(self.id));
156                ::protocol2::write_instruction(interface, read)?;
157                Ok(::protocol2::read_status::<I, ::protocol2::instruction::ReadResponse<R>>(interface)?.value)
158            }
159        }
160    };
161}
162
163pub trait Register {
164    const SIZE: u16;
165    const ADDRESS: u16;
166}
167    
168pub trait ReadRegister: Register {
169    fn deserialize(&[u8]) -> Self;
170}
171
172pub trait WriteRegister: Register {
173    // TODO: change 4 to Self::SIZE when const generics land
174    fn serialize(&self) -> [u8; 4];
175}
176
177pub(crate) trait Instruction {
178    const PARAMETERS: u16;
179    const INSTRUCTION_VALUE: u8;
180
181    fn id(&self) -> PacketID;
182    
183    fn parameter(&self, index: usize) -> u8;
184
185    fn serialize<'a>(&'a self) -> Serializer<'a, Self> where Self: Sized {
186        let serializer = Serializer{
187            pos: 0,
188            length: 10 + Self::PARAMETERS,
189            crc: crc::CRC::new(),
190            bit_stuffer: BitStuffer::new(),
191            instruction: self,
192        };
193
194        let mut length = 0;
195        for _b in serializer.skip(7) {
196            length += 1;
197        }
198
199        Serializer{
200            pos: 0,
201            length: length,
202            crc: crc::CRC::new(),
203            bit_stuffer: BitStuffer::new(),
204            instruction: self,
205        }
206    }
207}
208
209pub(crate) trait Status {
210    const PARAMETERS: u16;
211
212    fn deserialize(id: ServoID, parameters: &[u8]) -> Self;
213}
214
215#[derive(Debug, PartialEq, Eq, Clone)]
216pub(crate) struct Serializer<'a, T: Instruction + 'a> {
217    pos: usize,
218    length: u16,
219    crc: crc::CRC,
220    bit_stuffer: BitStuffer,
221    instruction: &'a T,
222}
223
224impl<'a, T: Instruction + 'a> ::lib::iter::Iterator for Serializer<'a, T> {
225    type Item = u8;
226    
227    fn next(&mut self) -> Option<u8> {
228        let should_stuff = self.bit_stuffer.stuff_next() && self.pos < 9+T::PARAMETERS as usize;
229        let next_byte = if should_stuff {
230            Some(0xfd)
231        } else {
232            let next_byte = match self.pos {
233                0 => Some(0xff),
234                1 => Some(0xff),
235                2 => Some(0xfd),
236                3 => Some(0x00),
237                4 => Some(u8::from(self.instruction.id())),
238                5 => Some(self.length as u8),
239                6 => Some((self.length >> 8) as u8),
240                7 => Some(T::INSTRUCTION_VALUE),
241                x if x < 8+T::PARAMETERS as usize => Some(self.instruction.parameter(x-8)),
242                x if x == 8+T::PARAMETERS as usize => Some(u16::from(self.crc) as u8),
243                x if x == 9+T::PARAMETERS as usize => Some((u16::from(self.crc) >> 8) as u8),
244                _ => None,
245            };
246            
247
248            next_byte
249        };
250
251        if self.pos < 8+T::PARAMETERS as usize {
252            self.bit_stuffer = self.bit_stuffer.add_byte(next_byte.unwrap()).unwrap();
253            self.crc.add(&[next_byte.unwrap()]);
254        }
255
256        if next_byte.is_some() && !should_stuff {
257            self.pos += 1;
258        }
259        
260        next_byte
261    }
262}
263
264#[derive(Debug, PartialEq, Eq, Clone, Copy)]
265pub(crate) enum DeserializationStatus {
266    Ok,
267    Finished,
268}
269
270#[derive(Debug, PartialEq, Eq, Clone)]
271pub(crate) struct Deserializer<T: Status> {
272    phantom: ::lib::marker::PhantomData<T>,
273}
274
275impl<T: Status> Deserializer<T> {
276    fn new() -> Self {
277        Deserializer {
278            phantom: ::lib::marker::PhantomData{},
279        }
280    }
281    
282    fn deserialize_header(self, data: [u8; 9]) -> Result<BodyDeserializer<T>, FormatError> {
283        if data[0] != 0xff {return Err(FormatError::Header)};
284        if data[1] != 0xff {return Err(FormatError::Header)};
285        if data[2] != 0xfd {return Err(FormatError::Header)};
286        if data[3] != 0x00 {return Err(FormatError::Header)};
287        if data[7] != 0x55 {return Err(FormatError::Instruction)};
288
289        let length = data[5] as u16 | (data[6] as u16) << 8;
290        
291        let mut crc = crc::CRC::new();
292        crc.add(&data);
293        
294        let mut bit_stuffer = BitStuffer::new();
295        for b in data.iter() {
296            bit_stuffer = bit_stuffer.add_byte(*b)?;
297        }
298        
299        Ok(BodyDeserializer {
300            parameter_index: 0,
301            remaining_bytes: length-2,
302            id: ServoID::new(data[4]),
303            crc_l: None,
304            crc_calc: crc,
305            bit_stuffer: bit_stuffer,
306            alert: data[8].get_bit(7),
307            processing_error: ProcessingError::decode(data[8].get_bits(0..7))?,
308            parameters: [0u8; 6],
309            phantom: ::lib::marker::PhantomData{},
310        })
311    }
312}
313    
314#[derive(Debug, PartialEq, Eq, Clone)]
315pub(crate) struct BodyDeserializer<T: Status> {
316    remaining_bytes: u16,
317    parameter_index: usize,
318    id: ServoID,
319    crc_l: Option<u8>,
320    crc_calc: crc::CRC,
321    bit_stuffer: BitStuffer,
322    alert: bool,
323    processing_error: Option<ProcessingError>,
324    parameters: [u8; 6],
325    phantom: ::lib::marker::PhantomData<T>,
326}
327
328impl<T: Status> BodyDeserializer<T> {
329
330    pub fn is_finished(&self) -> bool {
331        self.remaining_bytes == 0
332    }
333    
334    pub fn remaining_bytes(&self) -> u16 {
335        self.remaining_bytes
336    }
337    
338    pub fn build(self) -> Result<T, Error> {
339        if !self.is_finished() {
340            Err(Error::Unfinished)
341        } else if let Some(error) = self.processing_error {
342            Err(Error::Processing(error))
343        } else {
344            Ok(T::deserialize(self.id, &self.parameters[..T::PARAMETERS as usize]))
345        }
346    }
347    
348    pub fn deserialize(&mut self, data: &[u8]) -> Result<DeserializationStatus, FormatError> {
349        for b in data {
350            if self.bit_stuffer.stuff_next() && self.remaining_bytes > 2 {
351                self.bit_stuffer = self.bit_stuffer.add_byte(*b)?;
352                self.remaining_bytes -= 1;
353            } else if self.remaining_bytes > 2 {
354                self.bit_stuffer = self.bit_stuffer.add_byte(*b)?;
355                self.crc_calc.add(&[*b]);
356                self.parameters[self.parameter_index] = *b;
357                self.parameter_index += 1;
358                self.remaining_bytes -= 1;
359            } else if self.remaining_bytes == 2 {
360                self.crc_l = Some(*b);
361                self.remaining_bytes -= 1;
362            } else if self.remaining_bytes == 1 {
363                let crc = self.crc_l.unwrap() as u16 | (*b as u16) << 8;
364                if crc != u16::from(self.crc_calc) {
365                    return Err(FormatError::CRC);
366                }
367                self.remaining_bytes -= 1;
368            } else {
369                return Err(FormatError::Length);
370            }
371        }
372        
373        if self.remaining_bytes == 0 {
374            Ok(DeserializationStatus::Finished)
375        } else {
376            Ok(DeserializationStatus::Ok)
377        }
378    }
379}
380
381/// All information needed to connect to a protocol 2 servo
382#[derive(Debug, Clone)]
383pub struct ServoInfo {
384    pub baud_rate: ::BaudRate,
385    pub model_number: u16,
386    pub fw_version: u8,
387    pub id: ServoID,
388}
389
390
391impl From<::CommunicationError> for Error {
392    fn from(e: ::CommunicationError) -> Error {
393        Error::Communication(e)
394    }
395}
396
397#[derive(Debug, PartialEq, Eq, Clone, Copy)]
398pub enum Error {
399    Unfinished,
400    Communication(::CommunicationError),
401    Format(FormatError),
402    Processing(ProcessingError),
403}
404
405impl From<::protocol2::Error> for ::Error {
406    fn from(e: ::protocol2::Error) -> ::Error {
407        match e {
408            ::protocol2::Error::Unfinished => ::Error::Unfinished,
409            ::protocol2::Error::Communication(ce) => ::Error::Communication(ce),
410            ::protocol2::Error::Format(_) => ::Error::Format,
411            ::protocol2::Error::Processing(_) => ::Error::Processing,
412        }
413    }
414}
415
416
417#[derive(Debug, PartialEq, Eq, Clone, Copy)]
418pub enum FormatError {
419    Header,
420    ID,
421    Length,
422    Instruction,
423    InvalidError(u8),
424    CRC,
425    StuffByte,
426    NotFinished,
427}
428
429#[derive(Debug, PartialEq, Eq, Clone, Copy)]
430pub enum ProcessingError {
431    ResultFail = 0x01,
432    InstructionError = 0x02,
433    CRCError = 0x03,
434    DataRangeError = 0x04,
435    DataLengthError = 0x05,
436    DataLimitError = 0x06,
437    AccessError = 0x07,
438}
439
440impl From<FormatError> for Error {
441    fn from(e: FormatError) -> Error {
442        Error::Format(e)
443    }
444}
445
446impl ProcessingError {
447    fn decode(e: u8) -> Result<Option<ProcessingError>, FormatError> {
448        match e {
449            0x00 => Ok(None),
450            0x01 => Ok(Some(ProcessingError::ResultFail)),
451            0x02 => Ok(Some(ProcessingError::InstructionError)),
452            0x03 => Ok(Some(ProcessingError::CRCError)),
453            0x04 => Ok(Some(ProcessingError::DataRangeError)),
454            0x05 => Ok(Some(ProcessingError::DataLengthError)),
455            0x06 => Ok(Some(ProcessingError::DataLimitError)),
456            0x07 => Ok(Some(ProcessingError::AccessError)),
457            x => Err(FormatError::InvalidError(x)),
458        }
459    }
460}
461
462impl From<ProcessingError> for u8 {
463    fn from(e: ProcessingError) -> u8 {
464        e as u8
465    }
466}
467
468#[derive(Debug, PartialEq, Eq, Copy, Clone)]
469pub struct ServoID(u8);
470
471impl ServoID {
472    pub fn new(id: u8) -> ServoID {
473        assert!(id <= 252);
474        ServoID(id)
475    }
476}
477
478#[derive(Debug, PartialEq, Eq, Copy, Clone)]
479pub enum PacketID {
480    Unicast(ServoID),
481    Broadcast,
482}
483
484impl PacketID {
485    pub fn unicast(id: u8) -> PacketID {
486        assert!(id <= 252);
487        PacketID::Unicast(ServoID::new(id))
488    }
489
490    pub fn broadcast() -> PacketID {
491        PacketID::Broadcast
492    }
493}
494
495impl From<ServoID> for PacketID {
496    fn from(id: ServoID) -> PacketID {
497        PacketID::Unicast(id)
498    }
499}
500
501impl From<PacketID> for u8 {
502    fn from(id: PacketID) -> u8 {
503        match id {
504            PacketID::Unicast(x) => u8::from(x),
505            PacketID::Broadcast => 254,
506        }
507    }
508}
509
510impl From<ServoID> for u8 {
511    fn from(id: ServoID) -> u8 {
512        id.0
513    }
514}
dynamixel/protocol2/mod.rs

dynamixel/protocol2/
mod.rs
