robstride/actuators/
robstride03.rs

1use crate::actuator_types::{
2    ActuatorParameter, EnableCommand, ParameterMetadata, ParameterType, TxCommand,
3};
4use crate::{
5    actuator::{denormalize_value, normalize_value, TypedCommandData, TypedFeedbackData},
6    Actuator, ActuatorMeasurementLimits, ActuatorType, Command, CommandData, CommunicationType,
7    ControlCommand, FeedbackFrame, ObtainIDCommand, ParaStrInfo, ReadCommand, SetIDCommand,
8    SetZeroCommand, StopCommand, WriteCommand,
9};
10use async_trait::async_trait;
11use eyre::{Result, WrapErr};
12use std::f32::consts::PI;
13use tokio::sync::mpsc;
14
15const LIMITS: ActuatorMeasurementLimits = ActuatorMeasurementLimits {
16    min_angle: -4.0 * PI,
17    max_angle: 4.0 * PI,
18    min_velocity: -20.0,
19    max_velocity: 20.0,
20    min_torque: -60.0,
21    max_torque: 60.0,
22    min_kp: 0.0,
23    max_kp: 5000.0,
24    min_kd: 0.0,
25    max_kd: 100.0,
26};
27
28#[derive(Debug, Clone, Default)]
29pub struct RobStride03Command {
30    pub target_angle_rad: f32,     // Radians
31    pub target_velocity_rads: f32, // Radians per second
32    pub kp: f32,                   // Position gain
33    pub kd: f32,                   // Velocity gain
34    pub torque_nm: f32,            // Newton-meters
35}
36
37#[derive(Debug, Clone, Copy)]
38pub struct RobStride03Feedback {
39    pub angle_rad: f32,
40    pub velocity_rads: f32,
41    pub torque_nm: f32,
42}
43
44impl TypedCommandData for RobStride03Command {
45    fn to_control_command(&self) -> ControlCommand {
46        ControlCommand {
47            target_angle: normalize_value(
48                self.target_angle_rad,
49                LIMITS.min_angle,
50                LIMITS.max_angle,
51                -100.0,
52                100.0,
53            ),
54            target_velocity: normalize_value(
55                self.target_velocity_rads,
56                LIMITS.min_velocity,
57                LIMITS.max_velocity,
58                -100.0,
59                100.0,
60            ),
61            kp: normalize_value(self.kp, LIMITS.min_kp, LIMITS.max_kp, 0.0, 100.0),
62            kd: normalize_value(self.kd, LIMITS.min_kd, LIMITS.max_kd, 0.0, 100.0),
63            torque: normalize_value(
64                self.torque_nm,
65                LIMITS.min_torque,
66                LIMITS.max_torque,
67                -100.0,
68                100.0,
69            ),
70        }
71    }
72
73    fn from_control_command(cmd: ControlCommand) -> Self {
74        Self {
75            target_angle_rad: denormalize_value(
76                cmd.target_angle,
77                -100.0,
78                100.0,
79                LIMITS.min_angle,
80                LIMITS.max_angle,
81            ),
82            target_velocity_rads: denormalize_value(
83                cmd.target_velocity,
84                -100.0,
85                100.0,
86                LIMITS.min_velocity,
87                LIMITS.max_velocity,
88            ),
89            kp: denormalize_value(cmd.kp, 0.0, 100.0, LIMITS.min_kp, LIMITS.max_kp),
90            kd: denormalize_value(cmd.kd, 0.0, 100.0, LIMITS.min_kd, LIMITS.max_kd),
91            torque_nm: denormalize_value(
92                cmd.torque,
93                -100.0,
94                100.0,
95                LIMITS.min_torque,
96                LIMITS.max_torque,
97            ),
98        }
99    }
100}
101
102impl TypedFeedbackData for RobStride03Feedback {
103    fn from_feedback_frame(frame: FeedbackFrame) -> Self {
104        Self {
105            angle_rad: normalize_value(
106                frame.angle,
107                -100.0,
108                100.0,
109                LIMITS.min_angle,
110                LIMITS.max_angle,
111            ),
112            velocity_rads: normalize_value(
113                frame.velocity,
114                -100.0,
115                100.0,
116                LIMITS.min_velocity,
117                LIMITS.max_velocity,
118            ),
119            torque_nm: normalize_value(
120                frame.torque,
121                -100.0,
122                100.0,
123                LIMITS.min_torque,
124                LIMITS.max_torque,
125            ),
126        }
127    }
128
129    fn angle_rad(&self) -> f32 {
130        self.angle_rad
131    }
132
133    fn velocity_rads(&self) -> f32 {
134        self.velocity_rads
135    }
136
137    fn torque_nm(&self) -> f32 {
138        self.torque_nm
139    }
140}
141
142#[derive(Debug, Clone)]
143pub struct RobStride03 {
144    pub id: u8,
145    pub host_id: u8,
146    pub tx: mpsc::Sender<TxCommand>,
147}
148
149impl RobStride03 {
150    pub fn new(id: u8, host_id: u8, tx: mpsc::Sender<TxCommand>) -> Self {
151        Self { id, host_id, tx }
152    }
153}
154
155#[async_trait]
156impl Actuator for RobStride03 {
157    fn id(&self) -> u8 {
158        self.id
159    }
160
161    fn actuator_type(&self) -> ActuatorType {
162        ActuatorType::RobStride03
163    }
164
165    async fn enable(&self) -> Result<()> {
166        let cmd = EnableCommand {
167            host_id: self.host_id,
168        }
169        .to_can_packet(self.id);
170        self.tx
171            .send(TxCommand::Send {
172                id: cmd.0,
173                data: cmd.1,
174            })
175            .await
176            .wrap_err("failed to send enable command")?;
177        Ok(())
178    }
179
180    async fn disable(&self, clear_fault: bool) -> Result<()> {
181        let cmd = StopCommand {
182            host_id: self.host_id,
183            clear_fault,
184        }
185        .to_can_packet(self.id);
186        self.tx
187            .send(TxCommand::Send {
188                id: cmd.0,
189                data: cmd.1,
190            })
191            .await
192            .wrap_err("failed to send disable command")?;
193        Ok(())
194    }
195
196    async fn set_id(&mut self, id: u8) -> Result<()> {
197        let cmd = SetIDCommand {
198            host_id: self.host_id,
199            new_id: id,
200        }
201        .to_can_packet(self.id);
202        self.tx
203            .send(TxCommand::Send {
204                id: cmd.0,
205                data: cmd.1,
206            })
207            .await
208            .wrap_err("failed to send set_id command")?;
209        self.id = id;
210        Ok(())
211    }
212
213    async fn get_uuid(&self) -> Result<()> {
214        let cmd = ObtainIDCommand {
215            host_id: self.host_id,
216        }
217        .to_can_packet(self.id);
218        self.tx
219            .send(TxCommand::Send {
220                id: cmd.0,
221                data: cmd.1,
222            })
223            .await
224            .wrap_err("failed to send get_uuid command")?;
225        Ok(())
226    }
227
228    async fn control(&self, cmd: ControlCommand) -> Result<()> {
229        let cmd = cmd.to_can_packet(self.id);
230        self.tx
231            .send(TxCommand::Send {
232                id: cmd.0,
233                data: cmd.1,
234            })
235            .await
236            .wrap_err("failed to send control command")?;
237        Ok(())
238    }
239
240    async fn get_feedback(&self) -> Result<()> {
241        let cmd = Command {
242            data: [0; 8],
243            can_id: self.id,
244            data_2: 0,
245            communication_type: CommunicationType::Feedback,
246        }
247        .to_can_packet();
248
249        self.tx
250            .send(TxCommand::Send {
251                id: cmd.0,
252                data: cmd.1,
253            })
254            .await
255            .wrap_err("failed to send get_feedback command")?;
256        Ok(())
257    }
258
259    async fn write_parameter(&self, cmd: WriteCommand) -> Result<()> {
260        let cmd = cmd.to_can_packet(self.id);
261        self.tx
262            .send(TxCommand::Send {
263                id: cmd.0,
264                data: cmd.1,
265            })
266            .await
267            .wrap_err("failed to send write_parameter command")?;
268        Ok(())
269    }
270
271    async fn read_parameter(&self, param_index: u16) -> Result<()> {
272        let cmd = ReadCommand {
273            host_id: self.host_id,
274            parameter_index: param_index,
275            data: 0,
276            read_status: false,
277        }
278        .to_can_packet(self.id);
279        self.tx
280            .send(TxCommand::Send {
281                id: cmd.0,
282                data: cmd.1,
283            })
284            .await
285            .wrap_err("failed to send read_parameter command")?;
286        Ok(())
287    }
288
289    async fn get_parameter_string_info(&self) -> Result<()> {
290        let cmd = ParaStrInfo {
291            host_id: self.host_id,
292        }
293        .to_can_packet(self.id);
294        self.tx
295            .send(TxCommand::Send {
296                id: cmd.0,
297                data: cmd.1,
298            })
299            .await
300            .wrap_err("failed to send get_parameter_string_info command")?;
301        Ok(())
302    }
303
304    async fn set_zero(&self) -> Result<()> {
305        let cmd = SetZeroCommand {
306            host_id: self.host_id,
307        }
308        .to_can_packet(self.id);
309        self.tx
310            .send(TxCommand::Send {
311                id: cmd.0,
312                data: cmd.1,
313            })
314            .await
315            .wrap_err("failed to send set_zero command")?;
316        Ok(())
317    }
318
319    async fn set_max_torque(&self, torque: f32) -> Result<()> {
320        let param = RobStride03Parameter::LimitTorque;
321        let cmd = WriteCommand {
322            host_id: self.host_id,
323            parameter_index: param.metadata().index,
324            data: torque,
325        };
326        self.write_parameter(cmd).await
327    }
328
329    async fn set_max_velocity(&self, velocity: f32) -> Result<()> {
330        let param = RobStride03Parameter::LimitSpd;
331        let cmd = WriteCommand {
332            host_id: self.host_id,
333            parameter_index: param.metadata().index,
334            data: velocity,
335        };
336        self.write_parameter(cmd).await
337    }
338
339    async fn set_max_current(&self, current: f32) -> Result<()> {
340        let param = RobStride03Parameter::LimitCur;
341        let cmd = WriteCommand {
342            host_id: self.host_id,
343            parameter_index: param.metadata().index,
344            data: current,
345        };
346        self.write_parameter(cmd).await
347    }
348}
349
350#[derive(Debug, Clone, Copy, PartialEq)]
351pub enum RobStride03Parameter {
352    RunMode,     // 0x7005 - Operation control mode (0-3)
353    IqRef,       // 0x7006 - Current mode Iq command (-90A to 90A)
354    SpdRef,      // 0x700A - Speed mode command (-15 to 15 rad/s)
355    LimitTorque, // 0x700B - Torque limitation (0-120 Nm)
356    CurKp,       // 0x7010 - Current Kp (default 0.05)
357    CurKi,       // 0x7011 - Current Ki (default 0.05)
358    CurFitGain,  // 0x7014 - Current filter coefficient (0-1.0, default 0.06)
359    Ref,         // 0x7016 - Position mode angle command (rad)
360    LimitSpd,    // 0x7017 - Position mode speed limit (0-15 rad/s)
361    LimitCur,    // 0x7018 - Speed/position mode current limit (0-90A)
362    MechPos,     // 0x7019 - Load end mechanical angle (rad)
363    Iqf,         // 0x701A - Iq filter values (-90A to 90A)
364    MechVel,     // 0x701B - Load end speed (-15 to 15 rad/s)
365    VBus,        // 0x701C - Bus voltage (V)
366    LocKp,       // 0x701E - Position Kp (default 30)
367    SpdKp,       // 0x701F - Speed Kp (default 5)
368    SpdKi,       // 0x7020 - Speed Ki (default 0.005)
369    SpdFiltGain, // 0x7021 - Speed filter gain (default 0.1)
370    Unknown,
371}
372
373impl ActuatorParameter for RobStride03Parameter {
374    fn metadata(&self) -> ParameterMetadata {
375        match self {
376            Self::RunMode => ParameterMetadata {
377                index: 0x7005,
378                name: String::from("Run Mode"),
379                param_type: ParameterType::Uint8,
380                units: String::from("mode"),
381                min_value: Some(0.0),
382                max_value: Some(3.0),
383            },
384            Self::IqRef => ParameterMetadata {
385                index: 0x7006,
386                name: String::from("Iq Reference"),
387                param_type: ParameterType::Float,
388                units: String::from("A"),
389                min_value: Some(-43.0),
390                max_value: Some(43.0),
391            },
392            Self::SpdRef => ParameterMetadata {
393                index: 0x700A,
394                name: String::from("Speed Reference"),
395                param_type: ParameterType::Float,
396                units: String::from("rad/s"),
397                min_value: Some(-20.0),
398                max_value: Some(20.0),
399            },
400            Self::LimitTorque => ParameterMetadata {
401                index: 0x700B,
402                name: String::from("Torque Limit"),
403                param_type: ParameterType::Float,
404                units: String::from("Nm"),
405                min_value: Some(0.0),
406                max_value: Some(60.0),
407            },
408            Self::CurKp => ParameterMetadata {
409                index: 0x7010,
410                name: String::from("Current Kp"),
411                param_type: ParameterType::Float,
412                units: String::from(""),
413                min_value: Some(0.0),
414                max_value: None,
415            },
416            Self::CurKi => ParameterMetadata {
417                index: 0x7011,
418                name: String::from("Current Ki"),
419                param_type: ParameterType::Float,
420                units: String::from(""),
421                min_value: Some(0.0),
422                max_value: None,
423            },
424            Self::CurFitGain => ParameterMetadata {
425                index: 0x7014,
426                name: String::from("Current Filter Gain"),
427                param_type: ParameterType::Float,
428                units: String::from(""),
429                min_value: Some(0.0),
430                max_value: Some(1.0),
431            },
432            Self::Ref => ParameterMetadata {
433                index: 0x7016,
434                name: String::from("Position Reference"),
435                param_type: ParameterType::Float,
436                units: String::from("rad"),
437                min_value: None,
438                max_value: None,
439            },
440            Self::LimitSpd => ParameterMetadata {
441                index: 0x7017,
442                name: String::from("Speed Limit"),
443                param_type: ParameterType::Float,
444                units: String::from("rad/s"),
445                min_value: Some(0.0),
446                max_value: Some(20.0),
447            },
448            Self::LimitCur => ParameterMetadata {
449                index: 0x7018,
450                name: String::from("Current Limit"),
451                param_type: ParameterType::Float,
452                units: String::from("A"),
453                min_value: Some(0.0),
454                max_value: Some(43.0),
455            },
456            Self::MechPos => ParameterMetadata {
457                index: 0x7019,
458                name: String::from("Mechanical Position"),
459                param_type: ParameterType::Float,
460                units: String::from("rad"),
461                min_value: None,
462                max_value: None,
463            },
464            Self::Iqf => ParameterMetadata {
465                index: 0x701A,
466                name: String::from("Iq Filter Value"),
467                param_type: ParameterType::Float,
468                units: String::from("A"),
469                min_value: Some(-43.0),
470                max_value: Some(43.0),
471            },
472            Self::MechVel => ParameterMetadata {
473                index: 0x701B,
474                name: String::from("Mechanical Velocity"),
475                param_type: ParameterType::Float,
476                units: String::from("rad/s"),
477                min_value: Some(-20.0),
478                max_value: Some(20.0),
479            },
480            Self::VBus => ParameterMetadata {
481                index: 0x701C,
482                name: String::from("Bus Voltage"),
483                param_type: ParameterType::Float,
484                units: String::from("V"),
485                min_value: None,
486                max_value: None,
487            },
488            Self::LocKp => ParameterMetadata {
489                index: 0x701E,
490                name: String::from("Position Kp"),
491                param_type: ParameterType::Float,
492                units: String::from(""),
493                min_value: Some(0.0),
494                max_value: None,
495            },
496            Self::SpdKp => ParameterMetadata {
497                index: 0x701F,
498                name: String::from("Speed Kp"),
499                param_type: ParameterType::Float,
500                units: String::from(""),
501                min_value: Some(0.0),
502                max_value: None,
503            },
504            Self::SpdKi => ParameterMetadata {
505                index: 0x7020,
506                name: String::from("Speed Ki"),
507                param_type: ParameterType::Float,
508                units: String::from(""),
509                min_value: Some(0.0),
510                max_value: None,
511            },
512            Self::SpdFiltGain => ParameterMetadata {
513                index: 0x7021,
514                name: String::from("Speed Filter Gain"),
515                param_type: ParameterType::Float,
516                units: String::from(""),
517                min_value: Some(0.0),
518                max_value: Some(1.0),
519            },
520            Self::Unknown => ParameterMetadata {
521                index: 0x0000,
522                name: String::from("Unknown"),
523                param_type: ParameterType::Uint16,
524                units: String::from(""),
525                min_value: None,
526                max_value: None,
527            },
528        }
529    }
530}
531
532impl RobStride03Parameter {
533    pub fn iter() -> impl Iterator<Item = RobStride03Parameter> {
534        use RobStride03Parameter::*;
535        [
536            RunMode,
537            IqRef,
538            SpdRef,
539            LimitTorque,
540            CurKp,
541            CurKi,
542            CurFitGain,
543            Ref,
544            LimitSpd,
545            LimitCur,
546            MechPos,
547            Iqf,
548            MechVel,
549            VBus,
550            LocKp,
551            SpdKp,
552            SpdKi,
553            SpdFiltGain,
554        ]
555        .iter()
556        .cloned()
557    }
558
559    pub fn from_index(index: u16) -> Option<Self> {
560        match index {
561            0x7005 => Some(Self::RunMode),
562            0x7006 => Some(Self::IqRef),
563            0x700A => Some(Self::SpdRef),
564            0x700B => Some(Self::LimitTorque),
565            0x7010 => Some(Self::CurKp),
566            0x7011 => Some(Self::CurKi),
567            0x7014 => Some(Self::CurFitGain),
568            0x7016 => Some(Self::Ref),
569            0x7017 => Some(Self::LimitSpd),
570            0x7018 => Some(Self::LimitCur),
571            0x7019 => Some(Self::MechPos),
572            0x701A => Some(Self::Iqf),
573            0x701B => Some(Self::MechVel),
574            0x701C => Some(Self::VBus),
575            0x701E => Some(Self::LocKp),
576            0x701F => Some(Self::SpdKp),
577            0x7020 => Some(Self::SpdKi),
578            0x7021 => Some(Self::SpdFiltGain),
579
580            _ => None,
581        }
582    }
583}
robstride/actuators/robstride03.rs

robstride/actuators/
robstride03.rs
