diff --git a/src/builder.rs b/src/builder.rs index 55cc50c..3b8ccc5 100644 --- a/src/builder.rs +++ b/src/builder.rs @@ -549,12 +549,22 @@ mod test { let message = MessageBuilder::new() .id(0xFD) - .s_jog(60, JogMode::Continuous { speed: 320 }, JogColor::Blue, 0xFD) + .s_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::CounterClockwise }, JogColor::Blue, 0xFD) .build(); assert_eq!( message.as_slice(), &[0xFF, 0xFF, 0x0C, 0xFD, 0x06, 124, 130, 0x3C, 0x40, 0x01, 0x0A, 0xFD,] + ); + + let message = MessageBuilder::new() + .id(0xFD) + .s_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::Clockwise }, JogColor::Blue, 0xFD) + .build(); + + assert_eq!( + message.as_slice(), + &[0xFF, 0xFF, 0x0C, 0xFD, 0x06, 0x3C, 0xC2, 0x3C, 0x40, 0x41, 0x0A, 0xFD,] ) } @@ -571,11 +581,20 @@ mod test { let message = MessageBuilder::new() .id(0xFD) - .i_jog(60, JogMode::Continuous { speed: 320 }, JogColor::Blue, 0xFD) + .i_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::CounterClockwise }, JogColor::Blue, 0xFD) .build(); assert_eq!( message.as_slice(), &[0xFF, 0xFF, 0x0C, 0xFD, 0x05, 0x7E, 0x80, 0x40, 0x01, 0x0A, 0xFD, 0x3C,] + ); + + let message = MessageBuilder::new() + .id(0xFD) + .i_jog(60, JogMode::Continuous { speed: 320, rotation: Rotation::Clockwise }, JogColor::Blue, 0xFD) + .build(); + assert_eq!( + message.as_slice(), + &[0xFF, 0xFF, 0x0C, 0xFD, 0x05, 0x3E, 0xC0, 0x40, 0x41, 0x0A, 0xFD, 0x3C,] ) } diff --git a/src/lib.rs b/src/lib.rs index 13a439c..d0fa3c3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,7 +12,7 @@ //! use drs_0x01::*; //! //! let servo = Servo::new(0x40); -//! let message = servo.set_speed(512); +//! let message = servo.set_speed(512, Rotation::Clockwise); //! ``` //! //! To reboot all the servomotors you can use this message : @@ -53,5 +53,5 @@ pub mod reader; mod servo; pub use addr::{ReadableEEPAddr, ReadableRamAddr, WritableEEPAddr, WritableRamAddr}; -pub use message::{JogColor, JogMode}; +pub use message::{JogColor, JogMode, Rotation}; pub use servo::Servo; diff --git a/src/message.rs b/src/message.rs index f40d404..29c4c1a 100644 --- a/src/message.rs +++ b/src/message.rs @@ -34,6 +34,15 @@ pub enum Rollback { SkipNone, } +/// This represent the rotation sense of the servomotor while controlled in `Speed`. +#[derive(Debug)] +pub enum Rotation { + /// CounterClockwise rotation, which is the default rotation sense. + CounterClockwise, + /// Clockwise rotation, representing the inverted rotation. + Clockwise, +} + /// This represent the servomotor control mode. /// The servomotor is either controlled in `Position` or `Speed`. #[derive(Debug)] @@ -52,6 +61,8 @@ pub enum JogMode { /// The desired PWM value. /// The value must be in the 0..1023 range speed: u16, + /// Inverts the rotation sense of the servo by modifying the 14th bit. + rotation: Rotation, }, } @@ -59,7 +70,8 @@ impl JogMode { pub(crate) fn associated_data(&self) -> u16 { match *self { JogMode::Normal { position } => position, - JogMode::Continuous { speed } => speed, + JogMode::Continuous { speed, rotation: Rotation::Clockwise } => 0x4000 | speed, + JogMode::Continuous { speed, rotation: Rotation::CounterClockwise } => speed, } } } diff --git a/src/servo.rs b/src/servo.rs index 5859e8e..66bdf71 100644 --- a/src/servo.rs +++ b/src/servo.rs @@ -1,6 +1,6 @@ use builder::{HerkulexMessage, MessageBuilder}; -use message::{JogColor, JogMode}; +use message::{JogColor, JogMode, Rotation}; use addr::*; @@ -47,7 +47,7 @@ impl Servo { /// Request the servo to go to a position. /// The value can be between 0 and 1023 but should be between 21 and 1002 if you don't want - /// the servomotor fo go in error mode. + /// the servomotor to go in error mode. pub fn set_position(self, position: u16) -> HerkulexMessage { MessageBuilder::new_with_id(self.id) .s_jog( @@ -62,12 +62,13 @@ impl Servo { /// Request the servo to have a certain speed. /// The value should be between 0 and 1023. - pub fn set_speed(self, speed: u16) -> HerkulexMessage { + pub fn set_speed(self, speed: u16, rotation: Rotation) -> HerkulexMessage { MessageBuilder::new_with_id(self.id) .s_jog( 60, JogMode::Continuous { speed: min(speed, 1023), + rotation }, JogColor::Blue, self.id,