moteus_protocol/
command.rs

1// Copyright 2026 mjbots Robotic Systems, LLC.  info@mjbots.com
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15//! Command types for moteus motor control.
16//!
17//! This module provides structures for each control mode supported by moteus:
18//! - Position mode: Primary servo control mode
19//! - Current mode: Direct torque/current control
20//! - VFOC mode: Voltage-mode field-oriented control
21//! - Stay-within mode: Position bounds enforcement
22//! - Zero-velocity mode: Hold position with damping
23//! - Brake mode: Short motor windings
24//! - Stop mode: Disable motor output
25
26use crate::frame::CanFdFrame;
27use crate::mode::Mode;
28use crate::multiplex::{self, WriteCanData, WriteCombiner};
29use crate::register::Register;
30use crate::resolution::Resolution;
31use moteus_derive::Setters;
32
33/// Format (resolution) configuration for position mode commands.
34#[non_exhaustive]
35#[derive(Debug, Clone)]
36pub struct PositionFormat {
37    pub position: Resolution,
38    pub velocity: Resolution,
39    pub feedforward_torque: Resolution,
40    pub kp_scale: Resolution,
41    pub kd_scale: Resolution,
42    pub maximum_torque: Resolution,
43    pub stop_position: Resolution,
44    pub watchdog_timeout: Resolution,
45    pub velocity_limit: Resolution,
46    pub accel_limit: Resolution,
47    pub fixed_voltage_override: Resolution,
48    pub ilimit_scale: Resolution,
49    pub fixed_current_override: Resolution,
50    pub ignore_position_bounds: Resolution,
51}
52
53impl Default for PositionFormat {
54    fn default() -> Self {
55        PositionFormat {
56            position: Resolution::Float,
57            velocity: Resolution::Float,
58            feedforward_torque: Resolution::Float,
59            kp_scale: Resolution::Float,
60            kd_scale: Resolution::Float,
61            maximum_torque: Resolution::Float,
62            stop_position: Resolution::Float,
63            watchdog_timeout: Resolution::Float,
64            velocity_limit: Resolution::Float,
65            accel_limit: Resolution::Float,
66            fixed_voltage_override: Resolution::Float,
67            ilimit_scale: Resolution::Float,
68            fixed_current_override: Resolution::Float,
69            ignore_position_bounds: Resolution::Float,
70        }
71    }
72}
73
74/// Position mode command.
75///
76/// This is the primary control mode for moteus. All fields are optional;
77/// fields set to `None` will not be transmitted (using Ignore resolution).
78///
79/// # Examples
80///
81/// ```
82/// use moteus_protocol::CanFdFrame;
83/// use moteus_protocol::command::{PositionCommand, PositionFormat};
84///
85/// let mut frame = CanFdFrame::new();
86/// frame.arbitration_id = 0x8001;
87///
88/// // Build with the builder pattern -- fields are optional
89/// let cmd = PositionCommand::new()
90///     .position(0.5)
91///     .velocity(1.0)
92///     .maximum_torque(2.0);
93///
94/// cmd.serialize(&mut frame, &PositionFormat::default());
95/// assert!(frame.size > 0);
96/// ```
97#[non_exhaustive]
98#[derive(Debug, Clone, Default, Setters)]
99pub struct PositionCommand {
100    /// Target position in revolutions
101    pub position: Option<f32>,
102    /// Target velocity in revolutions/second
103    pub velocity: Option<f32>,
104    /// Feedforward torque in Nm
105    pub feedforward_torque: Option<f32>,
106    /// Kp scale factor (0-1)
107    pub kp_scale: Option<f32>,
108    /// Kd scale factor (0-1)
109    pub kd_scale: Option<f32>,
110    /// Maximum torque in Nm
111    pub maximum_torque: Option<f32>,
112    /// Stop position for trajectory mode
113    pub stop_position: Option<f32>,
114    /// Watchdog timeout in seconds
115    pub watchdog_timeout: Option<f32>,
116    /// Velocity limit in revolutions/second
117    pub velocity_limit: Option<f32>,
118    /// Acceleration limit in revolutions/second^2
119    pub accel_limit: Option<f32>,
120    /// Fixed voltage override
121    pub fixed_voltage_override: Option<f32>,
122    /// Current limit scale factor
123    pub ilimit_scale: Option<f32>,
124    /// Fixed current override in A
125    pub fixed_current_override: Option<f32>,
126    /// Ignore position bounds flag
127    pub ignore_position_bounds: Option<f32>,
128}
129
130impl PositionCommand {
131    /// Creates a new position command with all fields set to None.
132    pub fn new() -> Self {
133        Self::default()
134    }
135
136    /// Serializes this command to a CAN frame.
137    pub fn serialize(&self, frame: &mut CanFdFrame, format: &PositionFormat) {
138        let mut writer = WriteCanData::new(frame);
139
140        // Write mode
141        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
142        writer.write_u8(Register::Mode.address() as u8);
143        writer.write_i8(Mode::Position as i8);
144
145        // Build resolution array based on which fields are set
146        let resolutions = [
147            if self.position.is_some() {
148                format.position
149            } else {
150                Resolution::Ignore
151            },
152            if self.velocity.is_some() {
153                format.velocity
154            } else {
155                Resolution::Ignore
156            },
157            if self.feedforward_torque.is_some() {
158                format.feedforward_torque
159            } else {
160                Resolution::Ignore
161            },
162            if self.kp_scale.is_some() {
163                format.kp_scale
164            } else {
165                Resolution::Ignore
166            },
167            if self.kd_scale.is_some() {
168                format.kd_scale
169            } else {
170                Resolution::Ignore
171            },
172            if self.maximum_torque.is_some() {
173                format.maximum_torque
174            } else {
175                Resolution::Ignore
176            },
177            if self.stop_position.is_some() {
178                format.stop_position
179            } else {
180                Resolution::Ignore
181            },
182            if self.watchdog_timeout.is_some() {
183                format.watchdog_timeout
184            } else {
185                Resolution::Ignore
186            },
187            if self.velocity_limit.is_some() {
188                format.velocity_limit
189            } else {
190                Resolution::Ignore
191            },
192            if self.accel_limit.is_some() {
193                format.accel_limit
194            } else {
195                Resolution::Ignore
196            },
197            if self.fixed_voltage_override.is_some() {
198                format.fixed_voltage_override
199            } else {
200                Resolution::Ignore
201            },
202            if self.ilimit_scale.is_some() {
203                format.ilimit_scale
204            } else {
205                Resolution::Ignore
206            },
207            if self.fixed_current_override.is_some() {
208                format.fixed_current_override
209            } else {
210                Resolution::Ignore
211            },
212            if self.ignore_position_bounds.is_some() {
213                format.ignore_position_bounds
214            } else {
215                Resolution::Ignore
216            },
217        ];
218
219        let mut combiner =
220            WriteCombiner::new(0x00, Register::CommandPosition.address(), &resolutions);
221
222        if combiner.maybe_write(&mut writer) {
223            writer.write_position(self.position.unwrap_or(0.0), format.position);
224        }
225        if combiner.maybe_write(&mut writer) {
226            writer.write_velocity(self.velocity.unwrap_or(0.0), format.velocity);
227        }
228        if combiner.maybe_write(&mut writer) {
229            writer.write_torque(
230                self.feedforward_torque.unwrap_or(0.0),
231                format.feedforward_torque,
232            );
233        }
234        if combiner.maybe_write(&mut writer) {
235            writer.write_pwm(self.kp_scale.unwrap_or(1.0), format.kp_scale);
236        }
237        if combiner.maybe_write(&mut writer) {
238            writer.write_pwm(self.kd_scale.unwrap_or(1.0), format.kd_scale);
239        }
240        if combiner.maybe_write(&mut writer) {
241            writer.write_torque(
242                self.maximum_torque.unwrap_or(f32::NAN),
243                format.maximum_torque,
244            );
245        }
246        if combiner.maybe_write(&mut writer) {
247            writer.write_position(self.stop_position.unwrap_or(f32::NAN), format.stop_position);
248        }
249        if combiner.maybe_write(&mut writer) {
250            writer.write_time(
251                self.watchdog_timeout.unwrap_or(f32::NAN),
252                format.watchdog_timeout,
253            );
254        }
255        if combiner.maybe_write(&mut writer) {
256            writer.write_velocity(
257                self.velocity_limit.unwrap_or(f32::NAN),
258                format.velocity_limit,
259            );
260        }
261        if combiner.maybe_write(&mut writer) {
262            writer.write_accel(self.accel_limit.unwrap_or(f32::NAN), format.accel_limit);
263        }
264        if combiner.maybe_write(&mut writer) {
265            writer.write_voltage(
266                self.fixed_voltage_override.unwrap_or(f32::NAN),
267                format.fixed_voltage_override,
268            );
269        }
270        if combiner.maybe_write(&mut writer) {
271            writer.write_pwm(self.ilimit_scale.unwrap_or(1.0), format.ilimit_scale);
272        }
273        if combiner.maybe_write(&mut writer) {
274            writer.write_current(
275                self.fixed_current_override.unwrap_or(f32::NAN),
276                format.fixed_current_override,
277            );
278        }
279        if combiner.maybe_write(&mut writer) {
280            writer.write_int(
281                self.ignore_position_bounds.unwrap_or(0.0) as i32,
282                format.ignore_position_bounds,
283            );
284        }
285    }
286}
287
288/// Format for VFOC mode commands.
289#[non_exhaustive]
290#[derive(Debug, Clone)]
291pub struct VFOCFormat {
292    pub theta: Resolution,
293    pub voltage: Resolution,
294    pub theta_rate: Resolution,
295}
296
297impl Default for VFOCFormat {
298    fn default() -> Self {
299        VFOCFormat {
300            theta: Resolution::Float,
301            voltage: Resolution::Float,
302            theta_rate: Resolution::Float,
303        }
304    }
305}
306
307/// Voltage-mode FOC command.
308///
309/// See [`PositionCommand`] for an example of building and serializing commands.
310#[non_exhaustive]
311#[derive(Debug, Clone, Setters)]
312pub struct VFOCCommand {
313    /// Electrical angle in radians
314    pub theta: f32,
315    /// Voltage to apply
316    pub voltage: f32,
317    /// Electrical angle rate in radians/second
318    pub theta_rate: Option<f32>,
319}
320
321impl Default for VFOCCommand {
322    fn default() -> Self {
323        VFOCCommand {
324            theta: 0.0,
325            voltage: 0.0,
326            theta_rate: None,
327        }
328    }
329}
330
331impl VFOCCommand {
332    /// Creates a new VFOC command with default values.
333    pub fn new() -> Self {
334        Self::default()
335    }
336
337    /// Serializes this command to a CAN frame.
338    pub fn serialize(&self, frame: &mut CanFdFrame, format: &VFOCFormat) {
339        let mut writer = WriteCanData::new(frame);
340
341        // Write mode
342        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
343        writer.write_u8(Register::Mode.address() as u8);
344        writer.write_i8(Mode::VoltageFoc as i8);
345
346        // Registers are: theta, voltage, (gaps), theta_rate at offset 6
347        let resolutions = [
348            format.theta,
349            format.voltage,
350            Resolution::Ignore, // VoltageDqD
351            Resolution::Ignore, // VoltageDqQ
352            Resolution::Ignore, // CommandQCurrent
353            Resolution::Ignore, // CommandDCurrent
354            if self.theta_rate.is_some() && self.theta_rate != Some(0.0) {
355                format.theta_rate
356            } else {
357                Resolution::Ignore
358            },
359        ];
360
361        let mut combiner = WriteCombiner::new(0x00, Register::VFocTheta.address(), &resolutions);
362
363        if combiner.maybe_write(&mut writer) {
364            // Theta is stored as PWM-scaled (divided by PI)
365            writer.write_pwm(self.theta / core::f32::consts::PI, format.theta);
366        }
367        if combiner.maybe_write(&mut writer) {
368            writer.write_voltage(self.voltage, format.voltage);
369        }
370        // Skip gaps
371        combiner.maybe_write(&mut writer);
372        combiner.maybe_write(&mut writer);
373        combiner.maybe_write(&mut writer);
374        combiner.maybe_write(&mut writer);
375        if combiner.maybe_write(&mut writer) {
376            writer.write_velocity(
377                self.theta_rate.unwrap_or(0.0) / core::f32::consts::PI,
378                format.theta_rate,
379            );
380        }
381    }
382}
383
384/// Format for current mode commands.
385#[non_exhaustive]
386#[derive(Debug, Clone)]
387pub struct CurrentFormat {
388    pub d_a: Resolution,
389    pub q_a: Resolution,
390}
391
392impl Default for CurrentFormat {
393    fn default() -> Self {
394        CurrentFormat {
395            d_a: Resolution::Float,
396            q_a: Resolution::Float,
397        }
398    }
399}
400
401/// DQ-axis current command.
402///
403/// # Examples
404///
405/// ```
406/// use moteus_protocol::CanFdFrame;
407/// use moteus_protocol::command::{CurrentCommand, CurrentFormat};
408///
409/// let mut frame = CanFdFrame::new();
410/// let cmd = CurrentCommand::new()
411///     .d_current(0.0)
412///     .q_current(0.5);
413/// cmd.serialize(&mut frame, &CurrentFormat::default());
414/// ```
415#[non_exhaustive]
416#[derive(Debug, Clone)]
417pub struct CurrentCommand {
418    /// D-axis current in Amps
419    pub d_a: f32,
420    /// Q-axis current in Amps
421    pub q_a: f32,
422}
423
424impl Default for CurrentCommand {
425    fn default() -> Self {
426        CurrentCommand { d_a: 0.0, q_a: 0.0 }
427    }
428}
429
430impl CurrentCommand {
431    /// Creates a new current command with default values (0, 0).
432    pub fn new() -> Self {
433        Self::default()
434    }
435
436    /// Sets the D-axis current in Amps.
437    #[must_use]
438    pub fn d_current(mut self, v: f32) -> Self {
439        self.d_a = v;
440        self
441    }
442
443    /// Sets the Q-axis current in Amps (torque-producing).
444    #[must_use]
445    pub fn q_current(mut self, v: f32) -> Self {
446        self.q_a = v;
447        self
448    }
449
450    /// Serializes this command to a CAN frame.
451    pub fn serialize(&self, frame: &mut CanFdFrame, format: &CurrentFormat) {
452        let mut writer = WriteCanData::new(frame);
453
454        // Write mode
455        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
456        writer.write_u8(Register::Mode.address() as u8);
457        writer.write_i8(Mode::Current as i8);
458
459        // Note: register order is Q then D
460        let resolutions = [format.q_a, format.d_a];
461
462        let mut combiner =
463            WriteCombiner::new(0x00, Register::CommandQCurrent.address(), &resolutions);
464
465        if combiner.maybe_write(&mut writer) {
466            writer.write_current(self.q_a, format.q_a);
467        }
468        if combiner.maybe_write(&mut writer) {
469            writer.write_current(self.d_a, format.d_a);
470        }
471    }
472}
473
474/// Format for stay-within mode commands.
475#[non_exhaustive]
476#[derive(Debug, Clone)]
477pub struct StayWithinFormat {
478    pub lower_bound: Resolution,
479    pub upper_bound: Resolution,
480    pub feedforward_torque: Resolution,
481    pub kp_scale: Resolution,
482    pub kd_scale: Resolution,
483    pub maximum_torque: Resolution,
484    pub watchdog_timeout: Resolution,
485    pub ilimit_scale: Resolution,
486    pub ignore_position_bounds: Resolution,
487}
488
489impl Default for StayWithinFormat {
490    fn default() -> Self {
491        StayWithinFormat {
492            lower_bound: Resolution::Float,
493            upper_bound: Resolution::Float,
494            feedforward_torque: Resolution::Float,
495            kp_scale: Resolution::Float,
496            kd_scale: Resolution::Float,
497            maximum_torque: Resolution::Float,
498            watchdog_timeout: Resolution::Float,
499            ilimit_scale: Resolution::Float,
500            ignore_position_bounds: Resolution::Float,
501        }
502    }
503}
504
505/// Stay-within mode command.
506///
507/// Unlike [`PositionCommand`] which drives to a target, this mode keeps the
508/// servo within position bounds while applying minimal control effort.
509///
510/// # Examples
511///
512/// ```
513/// use moteus_protocol::CanFdFrame;
514/// use moteus_protocol::command::{StayWithinCommand, StayWithinFormat};
515///
516/// let mut frame = CanFdFrame::new();
517/// let cmd = StayWithinCommand::new()
518///     .lower_bound(-0.5)
519///     .upper_bound(0.5);
520/// cmd.serialize(&mut frame, &StayWithinFormat::default());
521/// ```
522#[non_exhaustive]
523#[derive(Debug, Clone, Default, Setters)]
524pub struct StayWithinCommand {
525    /// Lower position bound in revolutions
526    pub lower_bound: Option<f32>,
527    /// Upper position bound in revolutions
528    pub upper_bound: Option<f32>,
529    /// Feedforward torque in Nm
530    pub feedforward_torque: Option<f32>,
531    /// Kp scale factor
532    pub kp_scale: Option<f32>,
533    /// Kd scale factor
534    pub kd_scale: Option<f32>,
535    /// Maximum torque in Nm
536    pub maximum_torque: Option<f32>,
537    /// Watchdog timeout in seconds
538    pub watchdog_timeout: Option<f32>,
539    /// Current limit scale
540    pub ilimit_scale: Option<f32>,
541    /// Ignore position bounds flag
542    pub ignore_position_bounds: Option<f32>,
543}
544
545impl StayWithinCommand {
546    /// Creates a new stay-within command with all fields set to None.
547    pub fn new() -> Self {
548        Self::default()
549    }
550
551    /// Serializes this command to a CAN frame.
552    pub fn serialize(&self, frame: &mut CanFdFrame, format: &StayWithinFormat) {
553        let mut writer = WriteCanData::new(frame);
554
555        // Write mode
556        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
557        writer.write_u8(Register::Mode.address() as u8);
558        writer.write_i8(Mode::StayWithin as i8);
559
560        let resolutions = [
561            if self.lower_bound.is_some() {
562                format.lower_bound
563            } else {
564                Resolution::Ignore
565            },
566            if self.upper_bound.is_some() {
567                format.upper_bound
568            } else {
569                Resolution::Ignore
570            },
571            if self.feedforward_torque.is_some() {
572                format.feedforward_torque
573            } else {
574                Resolution::Ignore
575            },
576            if self.kp_scale.is_some() {
577                format.kp_scale
578            } else {
579                Resolution::Ignore
580            },
581            if self.kd_scale.is_some() {
582                format.kd_scale
583            } else {
584                Resolution::Ignore
585            },
586            if self.maximum_torque.is_some() {
587                format.maximum_torque
588            } else {
589                Resolution::Ignore
590            },
591            if self.watchdog_timeout.is_some() {
592                format.watchdog_timeout
593            } else {
594                Resolution::Ignore
595            },
596            if self.ilimit_scale.is_some() {
597                format.ilimit_scale
598            } else {
599                Resolution::Ignore
600            },
601            if self.ignore_position_bounds.is_some() {
602                format.ignore_position_bounds
603            } else {
604                Resolution::Ignore
605            },
606        ];
607
608        let mut combiner = WriteCombiner::new(
609            0x00,
610            Register::CommandStayWithinLowerBound.address(),
611            &resolutions,
612        );
613
614        if combiner.maybe_write(&mut writer) {
615            writer.write_position(self.lower_bound.unwrap_or(f32::NAN), format.lower_bound);
616        }
617        if combiner.maybe_write(&mut writer) {
618            writer.write_position(self.upper_bound.unwrap_or(f32::NAN), format.upper_bound);
619        }
620        if combiner.maybe_write(&mut writer) {
621            writer.write_torque(
622                self.feedforward_torque.unwrap_or(0.0),
623                format.feedforward_torque,
624            );
625        }
626        if combiner.maybe_write(&mut writer) {
627            writer.write_pwm(self.kp_scale.unwrap_or(1.0), format.kp_scale);
628        }
629        if combiner.maybe_write(&mut writer) {
630            writer.write_pwm(self.kd_scale.unwrap_or(1.0), format.kd_scale);
631        }
632        if combiner.maybe_write(&mut writer) {
633            writer.write_torque(self.maximum_torque.unwrap_or(0.0), format.maximum_torque);
634        }
635        if combiner.maybe_write(&mut writer) {
636            writer.write_time(
637                self.watchdog_timeout.unwrap_or(f32::NAN),
638                format.watchdog_timeout,
639            );
640        }
641        if combiner.maybe_write(&mut writer) {
642            writer.write_pwm(self.ilimit_scale.unwrap_or(1.0), format.ilimit_scale);
643        }
644        if combiner.maybe_write(&mut writer) {
645            writer.write_int(
646                self.ignore_position_bounds.unwrap_or(0.0) as i32,
647                format.ignore_position_bounds,
648            );
649        }
650    }
651}
652
653/// Format for zero-velocity mode commands.
654#[non_exhaustive]
655#[derive(Debug, Clone)]
656pub struct ZeroVelocityFormat {
657    pub kd_scale: Resolution,
658}
659
660impl Default for ZeroVelocityFormat {
661    fn default() -> Self {
662        ZeroVelocityFormat {
663            kd_scale: Resolution::Float,
664        }
665    }
666}
667
668/// Zero-velocity mode command.
669///
670/// See [`PositionCommand`] for an example of building and serializing commands.
671#[non_exhaustive]
672#[derive(Debug, Clone, Default, Setters)]
673pub struct ZeroVelocityCommand {
674    /// Kd scale factor for damping
675    pub kd_scale: Option<f32>,
676}
677
678impl ZeroVelocityCommand {
679    /// Creates a new zero-velocity command with all fields set to None.
680    pub fn new() -> Self {
681        Self::default()
682    }
683
684    /// Serializes this command to a CAN frame.
685    pub fn serialize(&self, frame: &mut CanFdFrame, format: &ZeroVelocityFormat) {
686        let mut writer = WriteCanData::new(frame);
687
688        // Write mode
689        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
690        writer.write_u8(Register::Mode.address() as u8);
691        writer.write_i8(Mode::ZeroVelocity as i8);
692
693        // Only write kd_scale if provided
694        if let Some(kd) = self.kd_scale {
695            let resolutions = [format.kd_scale];
696            let mut combiner =
697                WriteCombiner::new(0x00, Register::CommandKdScale.address(), &resolutions);
698
699            if combiner.maybe_write(&mut writer) {
700                writer.write_pwm(kd, format.kd_scale);
701            }
702        }
703    }
704}
705
706/// Stop mode command - disables motor output.
707pub struct StopCommand;
708
709impl StopCommand {
710    /// Serializes this command to a CAN frame.
711    pub fn serialize(frame: &mut CanFdFrame) {
712        let mut writer = WriteCanData::new(frame);
713
714        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
715        writer.write_u8(Register::Mode.address() as u8);
716        writer.write_i8(Mode::Stopped as i8);
717    }
718}
719
720/// Brake mode command - shorts motor windings for braking.
721pub struct BrakeCommand;
722
723impl BrakeCommand {
724    /// Serializes this command to a CAN frame.
725    pub fn serialize(frame: &mut CanFdFrame) {
726        let mut writer = WriteCanData::new(frame);
727
728        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
729        writer.write_u8(Register::Mode.address() as u8);
730        writer.write_i8(Mode::Brake as i8);
731    }
732}
733
734/// Output position setting command.
735pub struct OutputNearestCommand {
736    /// Position to set
737    pub position: f32,
738}
739
740impl OutputNearestCommand {
741    /// Creates a new output nearest command.
742    pub fn new(position: f32) -> Self {
743        OutputNearestCommand { position }
744    }
745
746    /// Serializes this command to a CAN frame.
747    pub fn serialize(&self, frame: &mut CanFdFrame) {
748        let mut writer = WriteCanData::new(frame);
749
750        writer.write_u8(multiplex::WRITE_FLOAT | 0x01);
751        writer.write_varuint(Register::SetOutputNearest.address() as u32);
752        writer.write_f32(self.position);
753    }
754}
755
756/// Set output position to exact value.
757pub struct OutputExactCommand {
758    /// Position to set
759    pub position: f32,
760}
761
762impl OutputExactCommand {
763    /// Creates a new output exact command.
764    pub fn new(position: f32) -> Self {
765        OutputExactCommand { position }
766    }
767
768    /// Serializes this command to a CAN frame.
769    pub fn serialize(&self, frame: &mut CanFdFrame) {
770        let mut writer = WriteCanData::new(frame);
771
772        writer.write_u8(multiplex::WRITE_FLOAT | 0x01);
773        writer.write_varuint(Register::SetOutputExact.address() as u32);
774        writer.write_f32(self.position);
775    }
776}
777
778/// Require reindex command.
779pub struct RequireReindexCommand;
780
781impl RequireReindexCommand {
782    /// Serializes this command to a CAN frame.
783    pub fn serialize(frame: &mut CanFdFrame) {
784        let mut writer = WriteCanData::new(frame);
785
786        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
787        writer.write_varuint(Register::RequireReindex.address() as u32);
788        writer.write_i8(1);
789    }
790}
791
792/// Recapture position and velocity command.
793pub struct RecapturePositionVelocityCommand;
794
795impl RecapturePositionVelocityCommand {
796    /// Serializes this command to a CAN frame.
797    pub fn serialize(frame: &mut CanFdFrame) {
798        let mut writer = WriteCanData::new(frame);
799
800        writer.write_u8(multiplex::WRITE_INT8 | 0x01);
801        writer.write_varuint(Register::RecapturePositionVelocity.address() as u32);
802        writer.write_i8(1);
803    }
804}
805
806/// Format for AUX PWM commands.
807#[non_exhaustive]
808#[derive(Debug, Clone)]
809pub struct AuxPwmFormat {
810    pub aux1_pwm1: Resolution,
811    pub aux1_pwm2: Resolution,
812    pub aux1_pwm3: Resolution,
813    pub aux1_pwm4: Resolution,
814    pub aux1_pwm5: Resolution,
815    pub aux2_pwm1: Resolution,
816    pub aux2_pwm2: Resolution,
817    pub aux2_pwm3: Resolution,
818    pub aux2_pwm4: Resolution,
819    pub aux2_pwm5: Resolution,
820}
821
822impl Default for AuxPwmFormat {
823    fn default() -> Self {
824        AuxPwmFormat {
825            aux1_pwm1: Resolution::Float,
826            aux1_pwm2: Resolution::Float,
827            aux1_pwm3: Resolution::Float,
828            aux1_pwm4: Resolution::Float,
829            aux1_pwm5: Resolution::Float,
830            aux2_pwm1: Resolution::Float,
831            aux2_pwm2: Resolution::Float,
832            aux2_pwm3: Resolution::Float,
833            aux2_pwm4: Resolution::Float,
834            aux2_pwm5: Resolution::Float,
835        }
836    }
837}
838
839/// AUX PWM output command.
840///
841/// Sets PWM duty cycles on the AUX1 and AUX2 ports.
842/// Each port supports up to 5 PWM channels.
843/// Values are 0.0-1.0 duty cycle.
844#[non_exhaustive]
845#[derive(Debug, Clone, Default, Setters)]
846pub struct AuxPwmCommand {
847    /// AUX1 PWM channel 1 duty cycle (0.0-1.0)
848    pub aux1_pwm1: Option<f32>,
849    /// AUX1 PWM channel 2 duty cycle (0.0-1.0)
850    pub aux1_pwm2: Option<f32>,
851    /// AUX1 PWM channel 3 duty cycle (0.0-1.0)
852    pub aux1_pwm3: Option<f32>,
853    /// AUX1 PWM channel 4 duty cycle (0.0-1.0)
854    pub aux1_pwm4: Option<f32>,
855    /// AUX1 PWM channel 5 duty cycle (0.0-1.0)
856    pub aux1_pwm5: Option<f32>,
857    /// AUX2 PWM channel 1 duty cycle (0.0-1.0)
858    pub aux2_pwm1: Option<f32>,
859    /// AUX2 PWM channel 2 duty cycle (0.0-1.0)
860    pub aux2_pwm2: Option<f32>,
861    /// AUX2 PWM channel 3 duty cycle (0.0-1.0)
862    pub aux2_pwm3: Option<f32>,
863    /// AUX2 PWM channel 4 duty cycle (0.0-1.0)
864    pub aux2_pwm4: Option<f32>,
865    /// AUX2 PWM channel 5 duty cycle (0.0-1.0)
866    pub aux2_pwm5: Option<f32>,
867}
868
869impl AuxPwmCommand {
870    /// Creates a new AUX PWM command with all channels unset.
871    pub fn new() -> Self {
872        Self::default()
873    }
874
875    /// Serializes this command to a CAN frame.
876    pub fn serialize(&self, frame: &mut CanFdFrame, format: &AuxPwmFormat) {
877        let mut writer = WriteCanData::new(frame);
878
879        let resolutions = [
880            if self.aux1_pwm1.is_some() {
881                format.aux1_pwm1
882            } else {
883                Resolution::Ignore
884            },
885            if self.aux1_pwm2.is_some() {
886                format.aux1_pwm2
887            } else {
888                Resolution::Ignore
889            },
890            if self.aux1_pwm3.is_some() {
891                format.aux1_pwm3
892            } else {
893                Resolution::Ignore
894            },
895            if self.aux1_pwm4.is_some() {
896                format.aux1_pwm4
897            } else {
898                Resolution::Ignore
899            },
900            if self.aux1_pwm5.is_some() {
901                format.aux1_pwm5
902            } else {
903                Resolution::Ignore
904            },
905            if self.aux2_pwm1.is_some() {
906                format.aux2_pwm1
907            } else {
908                Resolution::Ignore
909            },
910            if self.aux2_pwm2.is_some() {
911                format.aux2_pwm2
912            } else {
913                Resolution::Ignore
914            },
915            if self.aux2_pwm3.is_some() {
916                format.aux2_pwm3
917            } else {
918                Resolution::Ignore
919            },
920            if self.aux2_pwm4.is_some() {
921                format.aux2_pwm4
922            } else {
923                Resolution::Ignore
924            },
925            if self.aux2_pwm5.is_some() {
926                format.aux2_pwm5
927            } else {
928                Resolution::Ignore
929            },
930        ];
931
932        let mut combiner = WriteCombiner::new(0x00, Register::Aux1Pwm1.address(), &resolutions);
933
934        if combiner.maybe_write(&mut writer) {
935            writer.write_pwm(self.aux1_pwm1.unwrap_or(0.0), format.aux1_pwm1);
936        }
937        if combiner.maybe_write(&mut writer) {
938            writer.write_pwm(self.aux1_pwm2.unwrap_or(0.0), format.aux1_pwm2);
939        }
940        if combiner.maybe_write(&mut writer) {
941            writer.write_pwm(self.aux1_pwm3.unwrap_or(0.0), format.aux1_pwm3);
942        }
943        if combiner.maybe_write(&mut writer) {
944            writer.write_pwm(self.aux1_pwm4.unwrap_or(0.0), format.aux1_pwm4);
945        }
946        if combiner.maybe_write(&mut writer) {
947            writer.write_pwm(self.aux1_pwm5.unwrap_or(0.0), format.aux1_pwm5);
948        }
949        if combiner.maybe_write(&mut writer) {
950            writer.write_pwm(self.aux2_pwm1.unwrap_or(0.0), format.aux2_pwm1);
951        }
952        if combiner.maybe_write(&mut writer) {
953            writer.write_pwm(self.aux2_pwm2.unwrap_or(0.0), format.aux2_pwm2);
954        }
955        if combiner.maybe_write(&mut writer) {
956            writer.write_pwm(self.aux2_pwm3.unwrap_or(0.0), format.aux2_pwm3);
957        }
958        if combiner.maybe_write(&mut writer) {
959            writer.write_pwm(self.aux2_pwm4.unwrap_or(0.0), format.aux2_pwm4);
960        }
961        if combiner.maybe_write(&mut writer) {
962            writer.write_pwm(self.aux2_pwm5.unwrap_or(0.0), format.aux2_pwm5);
963        }
964    }
965}
966
967/// Set clock trim command.
968pub struct SetTrimCommand {
969    /// Clock trim value
970    pub trim: i32,
971}
972
973impl SetTrimCommand {
974    /// Creates a new set trim command.
975    pub fn new(trim: i32) -> Self {
976        SetTrimCommand { trim }
977    }
978
979    /// Serializes this command to a CAN frame.
980    pub fn serialize(&self, frame: &mut CanFdFrame) {
981        let mut writer = WriteCanData::new(frame);
982
983        writer.write_u8(multiplex::WRITE_INT32 | 0x01);
984        writer.write_varuint(Register::ClockTrim.address() as u32);
985        writer.write_i32(self.trim);
986    }
987}
988
989#[cfg(test)]
990mod tests {
991    use super::*;
992
993    fn bytes(frame: &CanFdFrame) -> &[u8] {
994        &frame.data[..frame.size as usize]
995    }
996
997    #[test]
998    fn test_stop_command() {
999        let mut f = CanFdFrame::new();
1000        StopCommand::serialize(&mut f);
1001        assert_eq!(bytes(&f), [1, 0, 0]);
1002    }
1003
1004    #[test]
1005    fn test_brake_command() {
1006        let mut f = CanFdFrame::new();
1007        BrakeCommand::serialize(&mut f);
1008        assert_eq!(bytes(&f), [1, 0, 15]);
1009    }
1010
1011    #[test]
1012    fn test_position_command() {
1013        let mut f = CanFdFrame::new();
1014        PositionCommand::new()
1015            .position(0.5)
1016            .velocity(1.0)
1017            .serialize(&mut f, &PositionFormat::default());
1018        assert_eq!(bytes(&f), [1, 0, 10, 14, 32, 0, 0, 0, 63, 0, 0, 128, 63]);
1019    }
1020
1021    #[test]
1022    fn test_vfoc_command() {
1023        let mut f = CanFdFrame::new();
1024        VFOCCommand::new()
1025            .theta(1.0)
1026            .voltage(2.0)
1027            .serialize(&mut f, &VFOCFormat::default());
1028        assert_eq!(bytes(&f), [1, 0, 7, 14, 24, 131, 249, 162, 62, 0, 0, 0, 64]);
1029    }
1030
1031    #[test]
1032    fn test_current_command() {
1033        let mut f = CanFdFrame::new();
1034        CurrentCommand::new()
1035            .q_current(1.5)
1036            .d_current(0.5)
1037            .serialize(&mut f, &CurrentFormat::default());
1038        assert_eq!(bytes(&f), [1, 0, 9, 14, 28, 0, 0, 192, 63, 0, 0, 0, 63]);
1039    }
1040
1041    #[test]
1042    fn test_stay_within_command() {
1043        let mut f = CanFdFrame::new();
1044        StayWithinCommand::new()
1045            .lower_bound(-1.0)
1046            .upper_bound(1.0)
1047            .maximum_torque(0.3)
1048            .serialize(&mut f, &StayWithinFormat::default());
1049        assert_eq!(
1050            bytes(&f),
1051            [1, 0, 13, 14, 64, 0, 0, 128, 191, 0, 0, 128, 63, 13, 69, 154, 153, 153, 62]
1052        );
1053    }
1054
1055    #[test]
1056    fn test_zero_velocity_command() {
1057        let mut f = CanFdFrame::new();
1058        ZeroVelocityCommand::new()
1059            .kd_scale(0.5)
1060            .serialize(&mut f, &ZeroVelocityFormat::default());
1061        assert_eq!(bytes(&f), [1, 0, 12, 13, 36, 0, 0, 0, 63]);
1062    }
1063
1064    #[test]
1065    fn test_output_nearest_command() {
1066        let mut f = CanFdFrame::new();
1067        OutputNearestCommand::new(2.5).serialize(&mut f);
1068        assert_eq!(bytes(&f), [13, 176, 2, 0, 0, 32, 64]);
1069    }
1070
1071    #[test]
1072    fn test_output_exact_command() {
1073        let mut f = CanFdFrame::new();
1074        OutputExactCommand::new(3.0).serialize(&mut f);
1075        assert_eq!(bytes(&f), [13, 177, 2, 0, 0, 64, 64]);
1076    }
1077
1078    #[test]
1079    fn test_require_reindex_command() {
1080        let mut f = CanFdFrame::new();
1081        RequireReindexCommand::serialize(&mut f);
1082        assert_eq!(bytes(&f), [1, 178, 2, 1]);
1083    }
1084
1085    #[test]
1086    fn test_recapture_position_velocity_command() {
1087        let mut f = CanFdFrame::new();
1088        RecapturePositionVelocityCommand::serialize(&mut f);
1089        assert_eq!(bytes(&f), [1, 179, 2, 1]);
1090    }
1091
1092    #[test]
1093    fn test_aux_pwm_command() {
1094        let mut f = CanFdFrame::new();
1095        AuxPwmCommand::new()
1096            .aux1_pwm1(0.75)
1097            .aux2_pwm3(0.25)
1098            .serialize(&mut f, &AuxPwmFormat::default());
1099        assert_eq!(bytes(&f), [13, 118, 0, 0, 64, 63, 13, 125, 0, 0, 128, 62]);
1100    }
1101
1102    #[test]
1103    fn test_set_trim_command() {
1104        let mut f = CanFdFrame::new();
1105        SetTrimCommand::new(42).serialize(&mut f);
1106        assert_eq!(bytes(&f), [9, 113, 42, 0, 0, 0]);
1107    }
1108}
moteus_protocol/command.rs

moteus_protocol/
command.rs
