mks_servo42_rs/
lib.rs

1//! A generic, `no_std` Rust driver for **MKS SERVO42** closed-loop stepper motors.
2//!
3//! This library provides a type-safe interface for generating the serial protocol commands
4//! used by the MKS SERVO42C firmware (V1.0+). It is transport-agnostic, meaning it generates
5//! byte buffers that you can send over any serial interface (UART, USB-Serial, etc.).
6
7#![no_std]
8
9pub mod enums;
10mod errors;
11pub mod helpers;
12pub mod response;
13
14pub use enums::{
15    BaudRate, EnLogic, MotorType, RotationDirection, SaveClearStatus, ShaftStatus, WorkMode,
16    ZeroMode,
17};
18pub use errors::Error;
19pub use helpers::{
20    EnPinStatus, EncoderValue, MotorShaftAngle, ShaftErrValue, angle_to_steps,
21    encoder_val_to_degrees, estimate_move_duration, parse_en_pin_status_response,
22    parse_encoder_response, parse_motor_shaft_angle_error, parse_motor_shaft_angle_response,
23    parse_pulse_count_response, parse_shaft_status_response, parse_success_response,
24    speed_setting_to_rpm, strip_leading_garbage,
25};
26pub use response::{InvalidResponse, Response};
27
28/// Default hardware address for MKS SERVO42 targets.
29pub const DEFAULT_ADDRESS: u8 = 0xE0;
30/// Minimum allowed slave address.
31pub const MIN_ADDRESS: u8 = 0xE0;
32/// Maximum allowed slave address.
33pub const MAX_ADDRESS: u8 = 0xE9;
34
35/// Maximum speed value for move commands.
36pub const MAX_SPEED: u8 = 0x7F;
37/// Maximum index for current limit settings.
38pub const MAX_CURRENT_INDEX: u8 = 0x0F;
39/// Direct subdivision (microstepping) value is passed. 0 indicates 256.
40pub const MAX_SUBDIVISION: u8 = 0xFF;
41/// Maximum speed index for return-to-zero.
42pub const MAX_ZERO_SPEED: u8 = 0x04;
43
44/// Milliamps per unit of current limit index.
45pub const CURRENT_STEP_MA: u16 = 200;
46
47/// Maximum torque limit (0x4B0).
48pub const MAX_TORQUE_LIMIT: u16 = 0x4B0;
49
50const CMD_BUFFER_SIZE: usize = 10;
51
52mod cmd {
53    pub const READ_ENCODER_VALUE: u8 = 0x30;
54    pub const READ_PULSE_COUNT: u8 = 0x33;
55    pub const READ_MOTOR_SHAFT_ANGLE: u8 = 0x36;
56    pub const READ_MOTOR_SHAFT_ANGLE_ERROR: u8 = 0x39;
57    pub const READ_EN_PIN_STATUS: u8 = 0x3A;
58    pub const READ_RELEASE_STATUS: u8 = 0x3D;
59    pub const READ_SHAFT_STATUS: u8 = 0x3E;
60    pub const SAVE_CLEAR_STATUS: u8 = 0xFF;
61
62    pub const CALIBRATE_ENCODER: u8 = 0x80;
63    pub const SET_CURRENT_LIMIT: u8 = 0x83;
64    pub const SET_SUBDIVISION: u8 = 0x84;
65    pub const SET_EN_LOGIC: u8 = 0x85;
66    pub const SET_DIRECTION: u8 = 0x86;
67    pub const SET_AUTO_SCREEN_OFF: u8 = 0x87;
68    pub const SET_PROTECTION: u8 = 0x88;
69    pub const SET_INTERPOLATION: u8 = 0x89;
70
71    pub const SET_ZERO_MODE: u8 = 0x90;
72    pub const SET_CURRENT_AS_ZERO: u8 = 0x91;
73    pub const SET_ZERO_SPEED: u8 = 0x92;
74    pub const SET_ZERO_DIRECTION: u8 = 0x93;
75    pub const GO_TO_ZERO: u8 = 0x94;
76
77    pub const SET_POSITION_KP: u8 = 0xA1;
78    pub const SET_POSITION_KI: u8 = 0xA2;
79    pub const SET_POSITION_KD: u8 = 0xA3;
80    pub const SET_ACCELERATION: u8 = 0xA4;
81    pub const SET_MAX_TORQUE: u8 = 0xA5;
82
83    pub const ENABLE_MOTOR: u8 = 0xF3;
84    pub const RUN_WITH_CONSTANT_SPEED: u8 = 0xF6;
85    pub const STOP: u8 = 0xF7;
86    pub const RUN_MOTOR: u8 = 0xFD;
87}
88
89/// Main driver for communicating with an MKS SERVO42 motor.
90///
91/// This struct manages the slave address and an internal buffer used to
92/// construct serial commands.
93#[derive(Debug, Copy, Clone)]
94pub struct Driver {
95    address: u8,
96    buffer: [u8; CMD_BUFFER_SIZE],
97}
98
99type Result<T> = core::result::Result<T, Error>;
100
101impl Default for Driver {
102    /// Creates a new driver with the default address (0xE0).
103    fn default() -> Self {
104        Self {
105            address: DEFAULT_ADDRESS,
106            buffer: [0; CMD_BUFFER_SIZE],
107        }
108    }
109}
110
111impl Driver {
112    /// Creates a new driver instance with a specific target address.
113    #[must_use]
114    pub fn with_address(address: u8) -> Self {
115        Self {
116            address,
117            ..Default::default()
118        }
119    }
120
121    /// Generates a command to enable or disable the motor.
122    pub fn enable_motor(&mut self, enable: bool) -> &[u8] {
123        self.build_command(&[self.address, cmd::ENABLE_MOTOR, u8::from(enable)])
124    }
125
126    /// Generates a command to run the motor at a constant speed.
127    ///
128    /// # Errors
129    /// Returns `Error::InvalidValue` if speed exceeds `MAX_SPEED`.
130    pub fn run_with_constant_speed(
131        &mut self,
132        direction: RotationDirection,
133        speed: u8,
134    ) -> Result<&[u8]> {
135        if speed > MAX_SPEED {
136            return Err(Error::InvalidValue);
137        }
138        let dir_mask = match direction {
139            RotationDirection::Clockwise => 0x00,
140            RotationDirection::CounterClockwise => 0x80,
141        };
142        Ok(self.build_command(&[self.address, cmd::RUN_WITH_CONSTANT_SPEED, speed | dir_mask]))
143    }
144
145    /// Generates a command to stop the motor immediately.
146    pub fn stop(&mut self) -> &[u8] {
147        self.build_command(&[self.address, cmd::STOP])
148    }
149
150    /// Generates a command to save or clear the current status.
151    ///
152    /// This command is used to save or clear the status set by the `set_work_mode` command.
153    /// After saving successfully, the driver board will be disabled and needs to be re-enabled.
154    pub fn save_clear_status(&mut self, operation: SaveClearStatus) -> &[u8] {
155        self.build_command(&[self.address, cmd::SAVE_CLEAR_STATUS, operation as u8])
156    }
157
158    /// Generates a command to move the motor to a specific position (relative pulses).
159    ///
160    /// # Errors
161    /// Returns `Error::InvalidValue` if speed exceeds `MAX_SPEED`.
162    pub fn run_motor(
163        &mut self,
164        direction: RotationDirection,
165        speed: u8,
166        pulses: u32,
167    ) -> Result<&[u8]> {
168        if speed > MAX_SPEED {
169            return Err(Error::InvalidValue);
170        }
171        let dir_mask = match direction {
172            RotationDirection::Clockwise => 0x00,
173            RotationDirection::CounterClockwise => 0x80,
174        };
175        let pulse_bytes = pulses.to_be_bytes();
176        Ok(self.build_command(&[
177            self.address,
178            cmd::RUN_MOTOR,
179            speed | dir_mask,
180            pulse_bytes[0], // Data4: MSB
181            pulse_bytes[1], // Data3
182            pulse_bytes[2], // Data2
183            pulse_bytes[3], // Data1: LSB
184        ]))
185    }
186
187    /// Generates a command to trigger encoder calibration.
188    pub fn calibrate_encoder(&mut self) -> &[u8] {
189        self.build_command(&[self.address, cmd::CALIBRATE_ENCODER, 0x00])
190    }
191
192    /// Generates a command to set the current limit index.
193    ///
194    /// # Errors
195    /// Returns `Error::InvalidValue` if index exceeds `MAX_CURRENT_INDEX`.
196    pub fn set_current_limit(&mut self, index: u8) -> Result<&[u8]> {
197        if index > MAX_CURRENT_INDEX {
198            return Err(Error::InvalidValue);
199        }
200        Ok(self.build_command(&[self.address, cmd::SET_CURRENT_LIMIT, index]))
201    }
202
203    /// Generates a command to set the subdivision (microstepping) level.
204    /// Values 1-255 map directly to 1-255 microsteps. The value 0 sets 256 microsteps.
205    pub fn set_subdivision(&mut self, subdivision: u8) -> Result<&[u8]> {
206        Ok(self.build_command(&[self.address, cmd::SET_SUBDIVISION, subdivision]))
207    }
208
209    /// Generates a command to set the enable logic.
210    pub fn set_enable_logic(&mut self, logic: EnLogic) -> &[u8] {
211        self.build_command(&[self.address, cmd::SET_EN_LOGIC, logic as u8])
212    }
213
214    /// Generates a command to set the motor direction polarity.
215    pub fn set_direction(&mut self, direction: RotationDirection) -> &[u8] {
216        self.build_command(&[self.address, cmd::SET_DIRECTION, direction as u8])
217    }
218
219    /// Generates a command to enable or disable automatic screen off.
220    pub fn set_auto_screen_off(&mut self, enable: bool) -> &[u8] {
221        self.build_command(&[self.address, cmd::SET_AUTO_SCREEN_OFF, u8::from(!enable)])
222    }
223
224    /// Generates a command to enable or disable stall protection.
225    pub fn set_stall_protection(&mut self, enable: bool) -> &[u8] {
226        self.build_command(&[self.address, cmd::SET_PROTECTION, u8::from(!enable)])
227    }
228
229    /// Generates a command to enable or disable step interpolation.
230    pub fn set_interpolation(&mut self, enable: bool) -> &[u8] {
231        self.build_command(&[self.address, cmd::SET_INTERPOLATION, u8::from(!enable)])
232    }
233
234    /// Generates a command to set the return-to-zero mode.
235    pub fn set_zero_mode(&mut self, mode: ZeroMode) -> &[u8] {
236        self.build_command(&[self.address, cmd::SET_ZERO_MODE, mode as u8])
237    }
238
239    /// Generates a command to set the current position as zero.
240    pub fn set_current_as_zero(&mut self) -> &[u8] {
241        self.build_command(&[self.address, cmd::SET_CURRENT_AS_ZERO, 0x00])
242    }
243
244    /// Generates a command to set the return-to-zero speed.
245    ///
246    /// # Errors
247    /// Returns `Error::InvalidValue` if speed index exceeds `MAX_ZERO_SPEED`.
248    pub fn set_zero_speed(&mut self, speed: u8) -> Result<&[u8]> {
249        if speed > MAX_ZERO_SPEED {
250            return Err(Error::InvalidValue);
251        }
252        Ok(self.build_command(&[self.address, cmd::SET_ZERO_SPEED, speed]))
253    }
254
255    /// Generates a command to initiate return-to-zero sequence.
256    pub fn go_to_zero(&mut self) -> &[u8] {
257        self.build_command(&[self.address, cmd::GO_TO_ZERO, 0x00])
258    }
259
260    /// Generates a command to set the return-to-zero direction.
261    pub fn set_zero_direction(&mut self, direction: RotationDirection) -> &[u8] {
262        self.build_command(&[self.address, cmd::SET_ZERO_DIRECTION, direction as u8])
263    }
264
265    /// Generates a command to set the position loop Proportional (Kp) coefficient.
266    pub fn set_position_kp(&mut self, value: u16) -> &[u8] {
267        let bytes = value.to_be_bytes();
268        self.build_command(&[self.address, cmd::SET_POSITION_KP, bytes[0], bytes[1]])
269    }
270
271    /// Generates a command to set the position loop Integral (Ki) coefficient.
272    pub fn set_position_ki(&mut self, value: u16) -> &[u8] {
273        let bytes = value.to_be_bytes();
274        self.build_command(&[self.address, cmd::SET_POSITION_KI, bytes[0], bytes[1]])
275    }
276
277    /// Generates a command to set the position loop Derivative (Kd) coefficient.
278    pub fn set_position_kd(&mut self, value: u16) -> &[u8] {
279        let bytes = value.to_be_bytes();
280        self.build_command(&[self.address, cmd::SET_POSITION_KD, bytes[0], bytes[1]])
281    }
282
283    /// Generates a command to set the motor acceleration.
284    pub fn set_acceleration(&mut self, value: u16) -> &[u8] {
285        let bytes = value.to_be_bytes();
286        self.build_command(&[self.address, cmd::SET_ACCELERATION, bytes[0], bytes[1]])
287    }
288
289    /// Generates a command to set the maximum torque limit.
290    ///
291    /// # Errors
292    /// Returns `Error::InvalidValue` if value exceeds `MAX_TORQUE_LIMIT`.
293    pub fn set_max_torque(&mut self, value: u16) -> Result<&[u8]> {
294        if value > MAX_TORQUE_LIMIT {
295            return Err(Error::InvalidValue);
296        }
297        let bytes = value.to_be_bytes();
298        Ok(self.build_command(&[self.address, cmd::SET_MAX_TORQUE, bytes[0], bytes[1]]))
299    }
300
301    /// Generates a command to read the motor shaft status (Blocked/Unblocked/Error).
302    pub fn read_shaft_status(&mut self) -> &[u8] {
303        self.build_command(&[self.address, cmd::READ_SHAFT_STATUS])
304    }
305
306    /// Generates a command to read the current encoder value.
307    pub fn read_encoder_value(&mut self) -> &[u8] {
308        self.build_command(&[self.address, cmd::READ_ENCODER_VALUE])
309    }
310
311    /// Generates a command to read the total pulse count.
312    pub fn read_pulse_count(&mut self) -> &[u8] {
313        self.build_command(&[self.address, cmd::READ_PULSE_COUNT])
314    }
315
316    /// Generates a command to read the motor shaft angle.
317    ///
318    /// Returns a 4-byte signed integer representing the angle in encoder units.
319    /// One full rotation corresponds to 0-65535.
320    pub fn read_motor_shaft_angle(&mut self) -> &[u8] {
321        self.build_command(&[self.address, cmd::READ_MOTOR_SHAFT_ANGLE])
322    }
323
324    /// Generates a command to read the EN pin status.
325    ///
326    /// Returns:
327    /// - 0x01: Enable
328    /// - 0x02: Disable  
329    /// - 0x00: Error
330    pub fn read_en_pin_status(&mut self) -> &[u8] {
331        self.build_command(&[self.address, cmd::READ_EN_PIN_STATUS])
332    }
333
334    /// Generates a command to read the motor shaft angle error.
335    pub fn read_motor_shaft_angle_error(&mut self) -> &[u8] {
336        self.build_command(&[self.address, cmd::READ_MOTOR_SHAFT_ANGLE_ERROR])
337    }
338
339    /// Generates a command to read the release status of the motor.
340    pub fn read_release_status(&mut self) -> &[u8] {
341        self.build_command(&[self.address, cmd::READ_RELEASE_STATUS])
342    }
343
344    fn build_command(&mut self, cmd: &[u8]) -> &[u8] {
345        let len = cmd.len();
346        self.buffer[..len].copy_from_slice(cmd);
347        self.buffer[len] = calculate_checksum(cmd);
348        &self.buffer[..=len]
349    }
350}
351
352fn calculate_checksum(bytes: &[u8]) -> u8 {
353    bytes.iter().fold(0u8, |acc, &b| acc.wrapping_add(b))
354}
355
356#[cfg(test)]
357mod tests {
358    use super::*;
359
360    #[test]
361    fn test_checksum() {
362        assert_eq!(0xD7, calculate_checksum(&[0xE0, 0xF6, 0x01]));
363    }
364
365    #[test]
366    fn test_default_address() {
367        let driver = Driver::default();
368        assert_eq!(driver.address, DEFAULT_ADDRESS);
369    }
370
371    #[test]
372    fn test_with_address() {
373        let driver = Driver::with_address(0xE5);
374        assert_eq!(driver.address, 0xE5);
375
376        // Test edge addresses
377        let driver_min = Driver::with_address(MIN_ADDRESS);
378        assert_eq!(driver_min.address, MIN_ADDRESS);
379
380        let driver_max = Driver::with_address(MAX_ADDRESS);
381        assert_eq!(driver_max.address, MAX_ADDRESS);
382    }
383
384    #[test]
385    fn test_set_current_limit_invalid_value() {
386        let mut driver = Driver::default();
387        // MAX_CURRENT_INDEX is 0x0F, so 0x10 should fail
388        let result = driver.set_current_limit(MAX_CURRENT_INDEX + 1);
389        assert!(matches!(result, Err(Error::InvalidValue)));
390
391        // Valid value should succeed
392        let result = driver.set_current_limit(MAX_CURRENT_INDEX);
393        assert!(result.is_ok());
394    }
395
396    #[test]
397    fn test_run_motor_invalid_speed() {
398        let mut driver = Driver::default();
399        // MAX_SPEED is 0x7F, so 0x80 should fail
400        let result = driver.run_motor(RotationDirection::Clockwise, MAX_SPEED + 1, 100);
401        assert!(matches!(result, Err(Error::InvalidValue)));
402
403        // Valid speed should succeed
404        let result = driver.run_motor(RotationDirection::Clockwise, MAX_SPEED, 100);
405        assert!(result.is_ok());
406    }
407
408    #[test]
409    fn test_run_with_constant_speed_invalid_speed() {
410        let mut driver = Driver::default();
411        // MAX_SPEED is 0x7F, so 0x80 should fail
412        let result = driver.run_with_constant_speed(RotationDirection::Clockwise, MAX_SPEED + 1);
413        assert!(matches!(result, Err(Error::InvalidValue)));
414
415        // Valid speed should succeed
416        let result = driver.run_with_constant_speed(RotationDirection::Clockwise, MAX_SPEED);
417        assert!(result.is_ok());
418    }
419
420    #[test]
421    fn test_calibrate_encoder() {
422        // This command is too slow (40-60s) and dangerous to test on real hardware
423        let mut driver = Driver::default();
424        let cmd = driver.calibrate_encoder();
425
426        // Verify command format: [address, cmd::CALIBRATE_ENCODER, 0x00, checksum]
427        assert_eq!(cmd.len(), 4);
428        assert_eq!(cmd[0], DEFAULT_ADDRESS);
429        assert_eq!(cmd[1], 0x80); // cmd::CALIBRATE_ENCODER
430        assert_eq!(cmd[2], 0x00);
431        // Checksum: 0xE0 + 0x80 + 0x00 = 0x160 -> low byte 0x60
432        assert_eq!(cmd[3], 0x60);
433    }
434}
mks_servo42_rs/lib.rs

mks_servo42_rs/
lib.rs
