drs_0x01/
builder.rs

1use addr::*;
2use message::*;
3
4use arrayvec::ArrayVec;
5
6/// The error returned by [`MessageBuilder`](struct.MessageBuilder.html).
7pub enum MessageBuilderError {
8    /// The maximum number of data has been reached for this message.
9    MaximumDataReached,
10}
11
12struct Packet {
13    pid: u8,
14    cmd: u8,
15    data: [u8; 16],
16    data_size: usize,
17}
18
19impl Default for Packet {
20    fn default() -> Packet {
21        Packet {
22            pid: 0,
23            cmd: 0,
24            data: [0; 16],
25            data_size: 0,
26        }
27    }
28}
29
30impl Packet {
31    fn build(self) -> HerkulexMessage {
32        let mut result = HerkulexMessage::new();
33        let size: u8 = self.data_size as u8 + 7;
34        let mut checksum1: u8 = size ^ self.pid ^ self.cmd;
35        result.push(0xFF);
36        result.push(0xFF);
37        result.push(size);
38        result.push(self.pid);
39        result.push(self.cmd);
40        for i in 0..self.data_size {
41            result.push(self.data[i]);
42            checksum1 ^= self.data[i];
43        }
44        checksum1 &= 0xFE;
45        let checksum2: u8 = (!checksum1) & 0xFE;
46        result.insert(5, checksum1);
47        result.insert(6, checksum2);
48        result
49    }
50
51    fn push_data(&mut self, data: u8) {
52        self.data[self.data_size] = data;
53        self.data_size += 1;
54    }
55}
56
57/// This is the type of all the message provided by this crate.
58pub type HerkulexMessage = ArrayVec<[u8; 128]>;
59
60/// This struct allows you to build message to directly speak to the herkulex servomotors.
61#[derive(Default)]
62pub struct MessageBuilder {}
63
64/// This is a specialized version of the [`MessageBuilder`](struct.MessageBuilder.html) which
65/// contains an ID. It is
66/// used to
67/// build other types of builders such as :
68/// * [MessageBuilderMem](struct.MessageBuilderMem.html)
69/// * [MessageBuilderPosition](struct.MessageBuilderPosition.html)
70/// * [MessageBuilderSpecial](struct.MessageBuilderSpecial.html)
71pub struct MessageBuilderCmd {
72    pid: u8,
73}
74
75/// This is a specialized version of the [`MessageBuilder`](struct.MessageBuilder.html) which contains an ID and a memory
76/// request (read or write, and where).
77pub struct MessageBuilderMem {
78    pid: u8,
79    addr: RegisterRequest,
80    size: u8,
81}
82
83/// This is a specialized version of the [`MessageBuilder`](struct.MessageBuilder.html) which contains an ID and a position
84/// request.
85pub struct MessageBuilderPositionSJOG {
86    pid: u8,
87    pos: SJogRequest,
88}
89
90/// This is a specialized version of the [`MessageBuilder`](struct.MessageBuilder.html) which contains an ID and a position
91/// request.
92pub struct MessageBuilderPositionIJOG {
93    pid: u8,
94    pos: IJogRequest,
95}
96
97/// This is a specialized version of the [`MessageBuilder`](struct.MessageBuilder.html) which contains an ID and a special
98/// request (reboot, reset or stat).
99pub struct MessageBuilderSpecial {
100    pid: u8,
101    kind: SpecialRequest,
102}
103
104impl MessageBuilder {
105    /// Create a new message builder
106    pub fn new() -> MessageBuilder {
107        MessageBuilder {}
108    }
109
110    /// Create a new message builder with a preassigned ID.
111    pub fn new_with_id(id: u8) -> MessageBuilderCmd {
112        MessageBuilderCmd { pid: id }
113    }
114
115    /// Set the message ID to choose the servo
116    pub fn id(self, id: u8) -> MessageBuilderCmd {
117        MessageBuilderCmd { pid: id }
118    }
119}
120
121impl MessageBuilderCmd {
122    /// Create a message of type **RAM_READ** (read from the temporary memory)
123    pub fn read_ram<T: Into<Option<u8>>>(
124        self,
125        ram_addr: ReadableRamAddr,
126        size: T,
127    ) -> MessageBuilderMem {
128        MessageBuilderMem {
129            pid: self.pid,
130            addr: RegisterRequest::RamRead(ram_addr),
131            size: match size.into() {
132                Some(s) => s,
133                None => ram_addr.bytes(),
134            },
135        }
136    }
137
138    /// Create a message of type **RAM_WRITE** (write to the temporary memory, last until the servo
139    /// is restarted)
140    pub fn write_ram(self, ram_addr: WritableRamAddr) -> MessageBuilderMem {
141        MessageBuilderMem {
142            pid: self.pid,
143            addr: RegisterRequest::RamWrite(ram_addr),
144            size: ram_addr.bytes(),
145        }
146    }
147
148    /// Create a message of type **READ_EEP** (read the permanent memory)
149    pub fn read_eep<T: Into<Option<u8>>>(
150        self,
151        eep_addr: ReadableEEPAddr,
152        size: T,
153    ) -> MessageBuilderMem {
154        MessageBuilderMem {
155            pid: self.pid,
156            addr: RegisterRequest::EEPRead(eep_addr),
157            size: match size.into() {
158                Some(s) => s,
159                None => eep_addr.bytes(),
160            },
161        }
162    }
163
164    /// Create a message of type **WRITE_EEP** (write to the permanent memory, require a reboot to
165    /// take effect).
166    pub fn write_eep(self, eep_addr: WritableEEPAddr) -> MessageBuilderMem {
167        MessageBuilderMem {
168            pid: self.pid,
169            addr: RegisterRequest::EEPWrite(eep_addr),
170            size: eep_addr.bytes(),
171        }
172    }
173
174    /// Create a message of type **REBOOT** (reboot the designed servos)
175    pub fn reboot(self) -> MessageBuilderSpecial {
176        MessageBuilderSpecial {
177            pid: self.pid,
178            kind: SpecialRequest::Reboot,
179        }
180    }
181
182    /// Create a message of type **ROLLBACK** (reset EEP memory)
183    pub fn rollback(self, flags: Rollback) -> MessageBuilderSpecial {
184        let kind = match flags {
185            Rollback::SkipId => SpecialRequest::Rollback {
186                skip_id: 1,
187                skip_baud: 0,
188            },
189            Rollback::SkipBaud => SpecialRequest::Rollback {
190                skip_id: 0,
191                skip_baud: 1,
192            },
193            Rollback::SkipBoth => SpecialRequest::Rollback {
194                skip_id: 1,
195                skip_baud: 1,
196            },
197            Rollback::SkipNone => SpecialRequest::Rollback {
198                skip_id: 0,
199                skip_baud: 0,
200            },
201        };
202        MessageBuilderSpecial {
203            pid: self.pid,
204            kind,
205        }
206    }
207
208    /// Create a message of type **STAT** (request servo status)
209    pub fn stat(self) -> MessageBuilderSpecial {
210        MessageBuilderSpecial {
211            pid: self.pid,
212            kind: SpecialRequest::Stat,
213        }
214    }
215
216    /// Create a new message of type **SJog**.
217    /// There is a maximum of 10 servomotors that can be controlled per sjog message.
218    ///
219    /// # Invalid Values
220    ///
221    /// The maximum `playtime` value is `0xFE`.
222    /// The maximum `id` value is `0xFE`.
223    /// The maximum `position` value is 1024.
224    pub fn s_jog(
225        self,
226        playtime: u8,
227        mode: JogMode,
228        color: JogColor,
229        id: u8,
230    ) -> MessageBuilderPositionSJOG {
231        let mut result = MessageBuilderPositionSJOG {
232            pid: self.pid,
233            pos: SJogRequest {
234                data: ArrayVec::new(),
235                playtime,
236            },
237        };
238        result.pos.data.push(SJogData::new(mode, color, id));
239        result
240    }
241
242    /// Create a new message of type **IJOG**
243    /// There is a maximum of 10 servomotors that can be controlled per ijog message.
244    ///
245    /// # Invalid Values
246    ///
247    /// The maximum `playtime` value is `0xFE`.
248    /// The maximum `id` value is `0xFE`.
249    /// The maximum `position` value is 1024.
250    pub fn i_jog(
251        self,
252        playtime: u8,
253        mode: JogMode,
254        color: JogColor,
255        id: u8,
256    ) -> MessageBuilderPositionIJOG {
257        let mut result = MessageBuilderPositionIJOG {
258            pid: self.pid,
259            pos: ArrayVec::new(),
260        };
261        result.pos.push(IJogData::new(mode, color, playtime, id));
262        result
263    }
264}
265
266impl MessageBuilderMem {
267    /// Build the final message to be sent to the servomotor through a serial connection.
268    pub fn build(self) -> HerkulexMessage {
269        let pid = self.pid;
270        let cmd = match self.addr {
271            RegisterRequest::EEPWrite(_) => 0x01,
272            RegisterRequest::EEPRead(_) => 0x02,
273            RegisterRequest::RamWrite(_) => 0x03,
274            RegisterRequest::RamRead(_) => 0x04,
275        };
276        let mut packet = Packet::default();
277        packet.pid = pid;
278        packet.cmd = cmd;
279
280        // TODO : Check write data sizes
281        match self.addr {
282            // EEP Write packet
283            RegisterRequest::EEPWrite(addr) => {
284                packet.push_data(addr.into());
285                packet.push_data(self.size);
286                let (d1, opt_d2) = addr.associated_data();
287                packet.push_data(d1);
288                if let Some(d2) = opt_d2 {
289                    packet.push_data(d2);
290                }
291            }
292
293            // RAM Write packet
294            RegisterRequest::RamWrite(addr) => {
295                packet.push_data(addr.into());
296                packet.push_data(self.size);
297                let (d1, opt_d2) = addr.associated_data();
298                packet.push_data(d1);
299                if let Some(d2) = opt_d2 {
300                    packet.push_data(d2);
301                }
302            }
303
304            // EEP Read packet
305            RegisterRequest::EEPRead(addr) => {
306                packet.push_data(addr.into());
307                packet.push_data(self.size);
308            }
309
310            // Ram Read packet
311            RegisterRequest::RamRead(addr) => {
312                packet.push_data(addr.into());
313                packet.push_data(self.size);
314            }
315        }
316        packet.build()
317    }
318}
319
320impl MessageBuilderSpecial {
321    /// Build the final message to be sent to the servomotor through a serial connection.
322    pub fn build(self) -> HerkulexMessage {
323        let cmd = match self.kind {
324            SpecialRequest::Stat => 0x07,
325            SpecialRequest::Rollback { .. } => 0x08,
326            SpecialRequest::Reboot => 0x09,
327        };
328        let mut packet = Packet::default();
329        packet.pid = self.pid;
330        packet.cmd = cmd;
331        if let SpecialRequest::Rollback {
332            skip_id: id_bit,
333            skip_baud: baud_bit,
334        } = self.kind
335        {
336            packet.push_data(id_bit);
337            packet.push_data(baud_bit);
338        }
339        packet.build()
340    }
341}
342
343impl MessageBuilderPositionSJOG {
344    /// Append a new **SJOG** command to this message.
345    ///
346    /// # Errors
347    ///
348    /// Return [MessageBuilderError::MaximumDataReached](enum.MessageBuilderError.html) if there
349    /// is already 10 sjog command in this message.
350    ///
351    /// # Invalid Values
352    ///
353    /// The maximum `id` value is `0xFE`.
354    /// The maximum `playtime` value is `0xFE`.
355    pub fn s_jog(
356        &mut self,
357        mode: JogMode,
358        color: JogColor,
359        id: u8,
360    ) -> Result<(), MessageBuilderError> {
361        if self.pos.data.is_full() {
362            Err(MessageBuilderError::MaximumDataReached)
363        } else {
364            self.pos.data.push(SJogData::new(mode, color, id));
365            Ok(())
366        }
367    }
368
369    /// Build the final message to be sent to the servomotor through a serial connection.
370    pub fn build(self) -> HerkulexMessage {
371        let mut packet = Packet::default();
372        packet.pid = self.pid;
373        packet.cmd = 6;
374        packet.push_data(self.pos.playtime);
375        for data in self.pos.data {
376            let d = data.mode.associated_data();
377            let lsb = (d) as u8;
378            let msb = (d >> 8) as u8;
379            packet.push_data(lsb);
380            packet.push_data(msb);
381
382            let mut set: u8 = 0;
383            match data.mode {
384                JogMode::Normal { .. } => set |= 0b0000_0000,
385                JogMode::Continuous { .. } => set |= 0b0000_0010,
386            }
387            match data.color {
388                JogColor::Blue => set |= 0b0000_1000,
389                JogColor::Green => set |= 0b0000_0100,
390                JogColor::Red => set |= 0b0001_0000,
391            }
392            packet.push_data(set);
393            packet.push_data(data.id);
394        }
395        packet.build()
396    }
397}
398
399impl MessageBuilderPositionIJOG {
400    /// Append a new **SJOG** command to this message.
401    ///
402    /// # Errors
403    ///
404    /// Return [MessageBuilderError::MaximumDataReached](enum.MessageBuilderError.html) if there
405    /// is already 10 sjog command in this message.
406    ///
407    /// # Invalid Values
408    ///
409    /// The maximum `id` value is `0xFE`.
410    pub fn s_jog(
411        &mut self,
412        mode: JogMode,
413        color: JogColor,
414        playtime: u8,
415        id: u8,
416    ) -> Result<(), MessageBuilderError> {
417        if self.pos.is_full() {
418            Err(MessageBuilderError::MaximumDataReached)
419        } else {
420            self.pos.push(IJogData::new(mode, color, playtime, id));
421            Ok(())
422        }
423    }
424
425    /// Build the final message to be sent to the servomotor through a serial connection.
426    pub fn build(self) -> HerkulexMessage {
427        let mut packet = Packet::default();
428        packet.pid = self.pid;
429        packet.cmd = 5;
430        for data in self.pos {
431            let d = data.mode.associated_data();
432            let lsb = (d) as u8;
433            let msb = (d >> 8) as u8;
434            packet.push_data(lsb);
435            packet.push_data(msb);
436
437            let mut set: u8 = 0;
438            match data.mode {
439                JogMode::Normal { .. } => set |= 0b0000_0000,
440                JogMode::Continuous { .. } => set |= 0b0000_0010,
441            }
442            match data.color {
443                JogColor::Blue => set |= 0b0000_1000,
444                JogColor::Green => set |= 0b0000_0100,
445                JogColor::Red => set |= 0b0001_0000,
446            }
447            packet.push_data(set);
448            packet.push_data(data.id);
449            packet.push_data(data.playtime);
450        }
451        packet.build()
452    }
453}
454
455#[cfg(test)]
456mod test {
457
458    use addr::ReadableEEPAddr;
459    use builder::*;
460
461    #[test]
462    fn reboot_message() {
463        let message = MessageBuilder::new().id(0xFD).reboot().build();
464        assert_eq!(message.len(), 0x07);
465        assert_eq!(
466            message.as_slice(),
467            &[0xFF, 0xFF, 0x07, 0xFD, 0x09, 0xF2, 0x0C]
468        );
469    }
470
471    #[test]
472    fn ram_read_message() {
473        let message = MessageBuilder::new()
474            .id(0xFD)
475            .read_ram(ReadableRamAddr::LEDControl, None)
476            .build();
477        assert_eq!(message.len(), 0x09);
478        assert_eq!(
479            message.as_slice(),
480            &[0xFF, 0xFF, 0x09, 0xFD, 0x04, 0xC4, 0x3A, 0x35, 0x01]
481        )
482    }
483
484    #[test]
485    fn ram_write_message() {
486        let message = MessageBuilder::new()
487            .id(0xFD)
488            .write_ram(WritableRamAddr::LEDControl(0x01))
489            .build();
490        assert_eq!(
491            message.as_slice(),
492            &[0xFF, 0xFF, 0x0A, 0xFD, 0x03, 0xC0, 0x3E, 0x35, 0x01, 0x01]
493        );
494
495        let message = MessageBuilder::new()
496            .id(0xFD)
497            .write_ram(WritableRamAddr::TorqueControl(0x60))
498            .build();
499        assert_eq!(
500            message.as_slice(),
501            &[0xFF, 0xFF, 0x0A, 0xFD, 0x03, 0xA0, 0x5E, 0x34, 0x01, 0x60]
502        );
503    }
504
505    #[test]
506    fn rollback_message() {
507        let message = MessageBuilder::new()
508            .id(0xFD)
509            .rollback(Rollback::SkipBoth)
510            .build();
511        assert_eq!(
512            message.as_slice(),
513            &[0xFF, 0xFF, 0x09, 0xFD, 0x08, 0xFC, 0x02, 1, 1]
514        )
515    }
516
517    #[test]
518    fn stat_message() {
519        let message = MessageBuilder::new().id(0xFD).stat().build();
520        assert_eq!(
521            message.as_slice(),
522            &[0xFF, 0xFF, 0x07, 0xFD, 0x07, 0xFC, 0x02]
523        );
524    }
525
526    #[test]
527    fn eep_read() {
528        let message = MessageBuilder::new()
529            .id(0xFD)
530            .read_eep(ReadableEEPAddr::PositionKp, 4)
531            .build();
532        assert_eq!(
533            message.as_slice(),
534            &[0xFF, 0xFF, 0x09, 0xFD, 0x02, 0xEC, 0x12, 0x1E, 0x04]
535        );
536    }
537
538    #[test]
539    fn sjog_message() {
540        let message = MessageBuilder::new()
541            .id(0xFD)
542            .s_jog(60, JogMode::Normal { position: 512 }, JogColor::Green, 0xFD)
543            .build();
544
545        assert_eq!(
546            message.as_slice(),
547            &[0xFF, 0xFF, 0x0C, 0xFD, 0x06, 0x30, 0xCE, 0x3C, 0x00, 0x02, 0x04, 0xFD,]
548        );
549
550        let message = MessageBuilder::new()
551            .id(0xFD)
552            .s_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::CounterClockwise }, JogColor::Blue, 0xFD)
553            .build();
554
555        assert_eq!(
556            message.as_slice(),
557            &[0xFF, 0xFF, 0x0C, 0xFD, 0x06, 124, 130, 0x3C, 0x40, 0x01, 0x0A, 0xFD,]
558        );
559
560        let message = MessageBuilder::new()
561            .id(0xFD)
562            .s_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::Clockwise }, JogColor::Blue, 0xFD)
563            .build();
564
565        assert_eq!(
566            message.as_slice(),
567            &[0xFF, 0xFF, 0x0C, 0xFD, 0x06, 0x3C, 0xC2, 0x3C, 0x40, 0x41, 0x0A, 0xFD,]
568        )
569    }
570
571    #[test]
572    fn ijog_message() {
573        let message = MessageBuilder::new()
574            .id(0xFD)
575            .i_jog(60, JogMode::Normal { position: 512 }, JogColor::Green, 0xFD)
576            .build();
577        assert_eq!(
578            message.as_slice(),
579            &[0xFF, 0xFF, 0x0C, 0xFD, 0x05, 0x32, 0xCC, 0x00, 0x02, 0x04, 0xFD, 0x3C,]
580        );
581
582        let message = MessageBuilder::new()
583            .id(0xFD)
584            .i_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::CounterClockwise }, JogColor::Blue, 0xFD)
585            .build();
586        assert_eq!(
587            message.as_slice(),
588            &[0xFF, 0xFF, 0x0C, 0xFD, 0x05, 0x7E, 0x80, 0x40, 0x01, 0x0A, 0xFD, 0x3C,]
589        );
590
591        let message = MessageBuilder::new()
592            .id(0xFD)
593            .i_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::Clockwise }, JogColor::Blue, 0xFD)
594            .build();
595        assert_eq!(
596            message.as_slice(),
597            &[0xFF, 0xFF, 0x0C, 0xFD, 0x05, 0x3E, 0xC0, 0x40, 0x41, 0x0A, 0xFD, 0x3C,]
598        )
599    }
600
601}
drs_0x01/builder.rs

drs_0x01/
builder.rs
