poulpe_ethercat_controller/
lib.rs

1use std::{
2    collections::HashMap,
3    error::Error,
4    f32::consts::{E, PI},
5    sync::RwLock,
6    time::Duration,
7};
8
9pub mod state_machine;
10use state_machine::{
11    parse_homing_error_flags, parse_motor_error_flags, parse_state_from_status_bits,
12    parse_status_word, CiA402State, ControlWord, ErrorFlags, StatusBit,
13};
14
15extern crate num;
16#[macro_use]
17extern crate num_derive;
18
19use bitvec::{prelude::*, ptr::read};
20
21use ethercat_controller::{
22    config::{PoulpeKind, SlaveConfig},
23    Config, EtherCatController,
24};
25
26pub mod register;
27use register::PdoRegister;
28
29#[derive(Debug)]
30pub struct PoulpeController {
31    pub inner: EtherCatController,
32    pub poulpe_config: HashMap<u16, PoulpeKind>,
33}
34
35impl PoulpeController {
36    pub fn connect(filename: &str) -> Result<Self, Box<dyn Error>> {
37        let config = Config::from_yaml(filename)?;
38
39        let controller = EtherCatController::open(
40            config.ethercat.master_id,
41            Duration::from_micros(config.ethercat.cycle_time_us as u64),
42            config.ethercat.command_drop_time_us,
43            config.ethercat.watchdog_timeout_ms,
44            config.ethercat.mailbox_wait_time_ms,
45        )?
46        .wait_for_ready();
47
48        let mut poulpe_config = HashMap::new();
49
50        // get the list of connected slaves
51        let slaves = controller.get_slave_ids_and_names();
52
53        // construct the map of connected poulpe boards
54        for (slave_id, slave_name) in slaves {
55            // check if the slave is a poulpe board
56            if !slave_name.contains("Orbita") {
57                log::warn!(
58                    "Slave {} with name {:?} maybe not a poulpe board! Skipping configuration!",
59                    slave_id,
60                    slave_name
61                );
62            }
63
64            // from this point on we are sure we are dealing with a poulpe boards
65
66            // check if slave with the same name already exists in the slave list
67            // if it does, throw an error and return
68            if poulpe_config
69                .values()
70                .any(|p: &PoulpeKind| p.name == slave_name)
71            {
72                log::error!(
73                    "Slave {} with name {:?} already connected, possible duplicate names at ids: {} and {}",
74                    slave_id,
75                    slave_name,
76                    slave_id,
77                    poulpe_config.iter().find(|(_, v)| v.name == slave_name).unwrap().0
78                );
79                return Err("Duplicate slave!".into());
80            }
81
82            if slave_name.contains("Orbita2d") {
83                let poulpe = PoulpeKind {
84                    id: slave_id,
85                    name: slave_name.clone(),
86                    orbita_type: 2,
87                };
88                poulpe_config.insert(slave_id, poulpe);
89            } else if slave_name.contains("Orbita3d") {
90                let poulpe = PoulpeKind {
91                    id: slave_id,
92                    name: slave_name.clone(),
93                    orbita_type: 3,
94                };
95                poulpe_config.insert(slave_id, poulpe);
96            } else {
97                log::warn!(
98                    "Slave {} with name {:?} maybe not a poulpe board!",
99                    slave_id,
100                    slave_name
101                );
102            }
103        }
104
105        Ok(Self {
106            inner: controller,
107            poulpe_config,
108        })
109    }
110
111    // function that checks if the time is longer that dropping time
112    // returns true if its longer and false if not
113    pub fn check_if_too_old(&self, message_ellased_time: Duration) -> bool {
114        message_ellased_time.as_micros() as u32 > self.inner.command_drop_time_us
115    }
116
117    pub fn get_orbita_type(&self, id: u32) -> u32 {
118        self.poulpe_config[&(id as u16)].orbita_type
119    }
120
121    pub fn get_slave_ids(&self) -> Vec<u32> {
122        self.poulpe_config.keys().map(|x| *x as u32).collect()
123    }
124
125    pub fn get_slave_name(&self, slave_id: u16) -> Option<String> {
126        self.poulpe_config.get(&slave_id).map(|x| x.name.clone())
127    }
128
129    pub fn get_slave_id(&self, name: &str) -> Option<u32> {
130        for (id, poulpe) in &self.poulpe_config {
131            if poulpe.name == name {
132                return Some(*id as u32);
133            }
134        }
135        None
136    }
137
138    pub fn get_slave_names(&self) -> Vec<String> {
139        self.poulpe_config
140            .values()
141            .map(|x| x.name.clone())
142            .collect()
143    }
144
145    pub fn is_poulpe_setup(&self, id: u32) -> bool {
146        self.inner.get_slave_setup(id as u16)
147    }
148
149    pub fn set_poulpe_setup(&self, id: u32, value: bool) {
150        self.inner.set_slave_setup(id as u16, value);
151    }
152
153    pub fn is_slave_ready(&self, id: u16) -> bool {
154        self.inner.is_slave_ready(id)
155    }
156
157    fn get_status_bits(&self, slave_id: u16) -> Result<Vec<StatusBit>, Box<dyn Error>> {
158        let status_word = self.get_pdo_register(slave_id, PdoRegister::StatusWord, 0)?;
159        let bits = u16::from_le_bytes(status_word.try_into().unwrap());
160        Ok(parse_status_word(bits))
161    }
162
163    pub fn get_mode_of_operation(&self, slave_id: u16) -> Result<u8, Box<dyn Error>> {
164        let mode_of_opearation = self.get_pdo_register(slave_id, PdoRegister::ModeOfOperation, 0);
165        match mode_of_opearation {
166            Ok(b) => Ok(b[0]),
167            Err(_) => Err("Error reading mode of operation".into()),
168        }
169    }
170    pub fn get_mode_of_operation_display(&self, slave_id: u16) -> Result<u8, Box<dyn Error>> {
171        let mode_of_operation_display =
172            self.get_pdo_register(slave_id, PdoRegister::ModeOfOperationDisplay, 0);
173        match mode_of_operation_display {
174            Ok(b) => Ok(b[0]),
175            Err(_) => Err("Error reading mode of operation display".into()),
176        }
177    }
178
179    fn clear_fault(&self, slave_id: u16) -> Result<(), Box<dyn Error>> {
180        Ok(())
181    }
182
183    fn set_controlword(&self, slave_id: u16, value: u16) -> Result<(), Box<dyn Error>> {
184        self.set_pdo_register(slave_id, PdoRegister::ControlWord, 0, &value.to_le_bytes())
185    }
186
187    pub fn get_error_flags(&self, slave_id: u16) -> Result<ErrorFlags, Box<dyn Error>> {
188        let error_codes = self.get_pdo_registers(slave_id, PdoRegister::ErrorCode)?;
189        let homing_error_flags = parse_homing_error_flags(error_codes[0][0..2].try_into().unwrap());
190        let mut motor_error_flags = vec![Vec::new(); error_codes.len() - 1];
191        for (i, e) in error_codes.iter().skip(1).enumerate() {
192            motor_error_flags[i] = parse_motor_error_flags(e[0..2].try_into().unwrap());
193        }
194
195        Ok(ErrorFlags {
196            motor_error_flags,
197            homing_error_flags,
198        })
199    }
200
201    fn wait_for_status_bit(
202        &self,
203        slave_id: u16,
204        bit: StatusBit,
205        timeout: Duration,
206    ) -> Result<(), Box<dyn Error>> {
207        let status_bits = self.get_status_bits(slave_id);
208        log::debug!("Waiting for {:?} ({:?})", bit, status_bits);
209
210        let start = std::time::Instant::now();
211        loop {
212            let status_bits = self.get_status_bits(slave_id)?;
213
214            if status_bits.contains(&StatusBit::Fault) {
215                // log::error!(
216                //     "Slave {} in Fault state \n {:#x?}",
217                //     slave_id,
218                //     self.get_error_flags(slave_id)?,
219                // );
220                return Err("Fault status".into());
221            }
222
223            if status_bits.contains(&bit) {
224                // bit is set
225                break;
226            }
227            if start.elapsed() > timeout {
228                log::error!("Timeout waiting for {:?} on slave {:?}", bit, slave_id);
229                return Err("Timeout waiting for bit".into());
230            }
231            self.inner.wait_for_next_cycle();
232        }
233        Ok(())
234    }
235
236    fn get_pdo_register(
237        &self,
238        slave_id: u16,
239        reg: PdoRegister,
240        index: usize,
241    ) -> Result<Vec<u8>, Box<dyn Error>> {
242        Ok(self
243            .inner
244            .get_pdo_register(slave_id, &reg.name().to_string(), index)
245            .unwrap())
246    }
247    fn set_pdo_register(
248        &self,
249        slave_id: u16,
250        reg: PdoRegister,
251        index: usize,
252        value: &[u8],
253    ) -> Result<(), Box<dyn Error>> {
254        self.inner
255            .set_pdo_register(slave_id, &reg.name().to_string(), index, value.to_vec());
256        Ok(())
257    }
258
259    fn get_pdo_registers(
260        &self,
261        slave_id: u16,
262        reg: PdoRegister,
263    ) -> Result<Vec<Vec<u8>>, Box<dyn Error>> {
264        Ok(self
265            .inner
266            .get_pdo_registers(slave_id, &reg.name().to_string())
267            .unwrap())
268    }
269    fn set_pdo_registers(
270        &self,
271        slave_id: u16,
272        reg: PdoRegister,
273        values: Vec<Vec<u8>>,
274    ) -> Result<(), Box<dyn Error>> {
275        self.inner
276            .set_pdo_registers(slave_id, &reg.name().to_string(), values);
277        Ok(())
278    }
279}
280
281impl PoulpeController {
282    pub fn setup(&self, id: u32) -> Result<(), Box<dyn Error>> {
283        let slave_id = id as u16;
284
285        if self.is_poulpe_setup(id) {
286            log::info!("Slave {} already setup", id);
287            return Ok(());
288        }
289
290        #[cfg(not(feature = "verify_network_on_slave_setup"))]
291        {
292            // check if slave_id exists in the slaves list built on startup
293            // if not there
294            if !self.get_slave_ids().contains(&id) {
295                log::error!(
296                    "Slave {} with name {:?} was not found in the slaves list built on startup!",
297                    id,
298                    self.get_slave_name(slave_id).unwrap()
299                );
300                return Err("Slave was not found in the slaves list built on startup!".into());
301            }
302
303            // check if slave_id exists in etheract network
304            // also check that the slave name is correct
305            if !self.inner.get_slave_ids().contains(&slave_id) {
306                log::error!(
307                    "Slave {} with name {:?} not found in Ethercat network, check connection!",
308                    id,
309                    self.poulpe_config[&slave_id].name
310                );
311                return Err("Slave not connected!".into());
312            }
313            match self.inner.get_slave_name(slave_id) {
314                Some(name) => {
315                    if self.poulpe_config[&slave_id].name != name {
316                        log::error!(
317                            "Slave {} Name mismatch: expected {:?}, got {:?}",
318                            slave_id,
319                            self.poulpe_config[&slave_id].name,
320                            name
321                        );
322                        return Err("Name mismatch".into());
323                    }
324                }
325                _ => {
326                    log::error!("Slave {} name not found, check connection!", slave_id);
327                    return Err("Name not found, check connection!".into());
328                }
329            }
330        }
331
332        // verify that the orbita type is the same as it is indicated in the name
333        // that was read from the ethercat network (Orbita2d or Orbita3d)
334        // orbita type is the number of axes
335        // - orbita2s has 2 axes
336        // - orbita3s has 3 axes
337        #[cfg(feature = "verify_orbita_type")]
338        {
339            let no_axes = self.poulpe_config[&slave_id].orbita_type;
340            let current_time = std::time::SystemTime::now();
341            let orbita_type = loop {
342                match self.get_type(slave_id as u32) {
343                    0 => std::thread::sleep(std::time::Duration::from_millis(100)),
344                    n => break n,
345                }
346                if current_time.elapsed().unwrap().as_millis() > 1000 {
347                    log::error!(
348                        "Slave {} - Commnunication not established:  1s timout on orbita type!",
349                        id
350                    );
351                    return Err("Commnunication not established:  1s timout on orbita type!".into());
352                }
353            };
354            if orbita_type != (no_axes as u8) {
355                log::error!(
356                    "Slave {} Orbita type mismatch: expected {}, got {}",
357                    slave_id,
358                    no_axes,
359                    orbita_type
360                );
361                return Err("Orbita type mismatch".into());
362            }
363        }
364
365        let state: CiA402State = self.get_status(slave_id as u32)?;
366        log::info!("Slave {}, inital state: {:?}", slave_id, state);
367
368        // get staus bits
369        let mut status_bits = self.get_status_bits(slave_id)?;
370
371        if status_bits.contains(&StatusBit::Warning) {
372            log::warn!(
373                "Slave {} in Warning state \n {:#x?}",
374                slave_id,
375                self.get_error_flags(slave_id)?,
376            );
377        }
378
379        // verify if not in fault state
380        if status_bits.contains(&StatusBit::Fault) {
381            log::error!(
382                "Slave {} in Fault state \n {:#x?}",
383                slave_id,
384                self.get_error_flags(slave_id)?,
385            );
386
387            #[cfg(not(feature = "allow_fault_on_slave"))]
388            return Err("Fault status".into());
389            #[cfg(feature = "allow_fault_on_slave")]
390            {
391                // turn off all the slaves if one of them is in fault state
392                self.emergency_stop_all(slave_id)?;
393                return Ok(());
394            }
395        }
396
397        if state == CiA402State::NotReadyToSwitchOn {
398            log::info!(
399                "Slave {} in NotReadyToSwitchOn state, waiting to be rady for SwitchOn",
400                slave_id
401            );
402            // wait 1s
403            std::thread::sleep(std::time::Duration::from_secs(1));
404            self.wait_for_status_bit(
405                slave_id,
406                StatusBit::SwitchedOnDisabled,
407                Duration::from_secs(100),
408            )?;
409
410            // get staus bits
411            status_bits = self.get_status_bits(slave_id)?;
412        }
413
414        // if enabled (should not be possible in normal operation)
415        if status_bits.contains(&StatusBit::OperationEnabled) {
416            #[cfg(feature = "turn_off_slaves_setup")]
417            {
418                // if the operation is enabled, we need
419                // to disable it before we can set the controlword
420                self.emergency_stop(id)?;
421                log::warn!("Slave {} in OperationEnabled state, turning off", slave_id);
422                self.wait_for_status_bit(
423                    slave_id,
424                    StatusBit::SwitchedOnDisabled,
425                    Duration::from_secs(20),
426                )?; // wait 20s (quick stop can take up to 10s)
427                    // get staus bits
428                status_bits = self.get_status_bits(slave_id)?;
429            }
430            #[cfg(not(feature = "turn_off_slaves_setup"))]
431            {
432                log::info!("Slave {}, setup done! Current state: {:?}", slave_id, state);
433                return Ok(());
434            }
435        }
436
437        // if switch on disabled, we need to switch on
438        if status_bits.contains(&StatusBit::SwitchedOnDisabled) {
439            // go to the ready to switch on state
440            self.set_controlword(slave_id, ControlWord::Shutdown.to_u16())?;
441        }
442
443        // here the slave shoulde be in ready to switch on state)
444        self.wait_for_status_bit(slave_id, StatusBit::ReadyToSwitchOn, Duration::from_secs(1))?;
445        status_bits = self.get_status_bits(slave_id)?;
446
447        // if ready to switch on, we need to switch on
448        if status_bits.contains(&StatusBit::ReadyToSwitchOn) {
449            // go to the switched on state
450            self.set_controlword(slave_id, ControlWord::SwitchOn.to_u16())?;
451            self.wait_for_status_bit(slave_id, StatusBit::SwitchedOn, Duration::from_secs(1))?;
452        }
453
454        let state: CiA402State = self.get_status(slave_id as u32)?;
455        log::info!("Slave {}, setup done! Current state: {:?}", slave_id, state);
456
457        // set the slave as setup
458        self.set_poulpe_setup(id, true);
459
460        Ok(())
461    }
462
463    pub fn is_torque_on(&self, id: u32) -> Result<Option<bool>, Box<dyn std::error::Error>> {
464        let slave_id = id as u16;
465        let status = self.get_status_bits(slave_id)?;
466        Ok(Some(status.contains(&StatusBit::OperationEnabled)))
467    }
468
469    pub fn set_torque(
470        &self,
471        id: u32,
472        requested_torque: bool,
473    ) -> Result<(), Box<dyn std::error::Error>> {
474        let slave_id = id as u16;
475        match self.is_torque_on(id) {
476            Ok(Some(actual_torque)) => {
477                log::debug!(
478                    "Setting torque on slave {} to {} from {}",
479                    id,
480                    requested_torque,
481                    actual_torque
482                );
483                let status_bits = self.get_status_bits(slave_id)?;
484                if status_bits.contains(&StatusBit::Fault) {
485                    #[cfg(feature = "allow_fault_on_slave")]
486                    // return ok if the slave is in the fault state - dont try to set the torque
487                    return Ok(());
488
489                    // return error if the slave is in fault state - dont try to set the torque
490                    log::error!("Slave {} in fault state", slave_id);
491                    return Err("Slave in fault state status".into());
492                }
493                #[cfg(not(feature = "switchon_on_turnon"))]
494                if status_bits.contains(&StatusBit::SwitchedOnDisabled) && requested_torque {
495                    #[cfg(feature = "allow_fault_on_slave")]
496                    // return ok if the slave is in switch on disabled state
497                    // the board is probably been turned off by a quick stop
498                    return Ok(());
499
500                    // return error if the slave is in fault state - dont try to set the torque
501                    log::error!(
502                        "Slave {} in SwitchedOnDisabled state, cannot enable torque!",
503                        slave_id
504                    );
505                    return Err(
506                        "Slave in SwitchedOnDisabled state status, cannot enable torque!".into(),
507                    );
508                }
509
510                if actual_torque == requested_torque {
511                    return Ok(());
512                } else {
513                    // if turn on is requested, set the target position to the current position - safety feature
514                    if requested_torque {
515                        #[cfg(feature = "safe_turn_on")]
516                        {
517                            // set the target position to the current position
518                            let current_position = self.get_current_position(id).unwrap().unwrap();
519                            self.set_target_position(id, current_position.clone())
520                                .unwrap();
521
522                            // verify that the target position is set correctly and try 5 times
523                            let mut target_position =
524                                self.get_current_target_position(id).unwrap().unwrap();
525                            let mut tries = 0;
526                            // check if the target position is set correctly (small error margin)
527                            while tries < 5
528                                && (current_position
529                                    .iter()
530                                    .zip(target_position.iter())
531                                    .any(|(a, b)| (a - b).abs() > 0.001))
532                            {
533                                self.set_target_position(id, current_position.clone())
534                                    .unwrap();
535                                std::thread::sleep(std::time::Duration::from_millis(2));
536                                target_position =
537                                    self.get_current_target_position(id).unwrap().unwrap();
538                                tries += 1;
539                            }
540                            std::thread::sleep(std::time::Duration::from_millis(5));
541                            // throw error if the target position is not set correctly
542                            if tries == 5 {
543                                log::error!("Error setting target position!");
544                                return Err("Error setting target position!".into());
545                            }
546                        }
547
548                        #[cfg(feature = "switchon_on_turnon")]
549                        {
550                            // Switch on
551                            self.set_controlword(slave_id, ControlWord::SwitchOn.to_u16())?;
552                            self.wait_for_status_bit(
553                                slave_id,
554                                StatusBit::SwitchedOn,
555                                Duration::from_secs(1),
556                            )?;
557                        }
558
559                        // Enable
560                        self.set_controlword(slave_id, ControlWord::EnableOperation.to_u16())?;
561                        self.wait_for_status_bit(
562                            slave_id,
563                            StatusBit::OperationEnabled,
564                            Duration::from_millis(20),
565                        )?;
566                    } else {
567                        // Shutdown
568                        self.set_controlword(slave_id, ControlWord::DisableOperation.to_u16())?;
569                        self.wait_for_status_bit(
570                            slave_id,
571                            StatusBit::SwitchedOn,
572                            Duration::from_millis(20),
573                        )?;
574                    }
575                }
576            }
577            _ => {
578                log::error!("Error getting torque state!");
579                return Err("Error getting torque state!".into());
580            }
581        }
582        Ok(())
583    }
584
585    fn get_pid(&self, _id: u32) -> Result<Option<(f32, f32, f32)>, Box<dyn std::error::Error>> {
586        Ok(None)
587    }
588    fn set_pid(&self, _id: u32, _pid: (f32, f32, f32)) -> Result<(), Box<dyn std::error::Error>> {
589        Ok(())
590    }
591
592    pub fn get_status(&self, slave_id: u32) -> Result<CiA402State, Box<dyn std::error::Error>> {
593        let status_bits = self.get_status_bits(slave_id as u16)?;
594        parse_state_from_status_bits(status_bits)
595    }
596
597    pub fn get_type(&self, slave_id: u32) -> u8 {
598        let byte = match self.get_pdo_register(slave_id as u16, PdoRegister::ActuatorType, 0) {
599            Ok(b) => b[0],
600            Err(_) => 255,
601        };
602        byte
603    }
604
605    // make sure that the slave is disabled before changing the mode of operation
606    pub fn set_mode_of_operation(&self, slave_id: u16, value: u8) -> Result<(), Box<dyn Error>> {
607        // if the mode of operation is already set, return
608        let mode_of_operation = self.get_mode_of_operation_display(slave_id)?;
609        if mode_of_operation == value {
610            return Ok(());
611        }
612
613        // if it is not verified that the slave is disabled
614        // if not return error
615        let is_on = self.is_torque_on(slave_id as u32)?;
616        match is_on {
617            Some(is_on) => match is_on {
618                true => {
619                    log::error!(
620                        "Slave {} | Cannot change mode of operation when slave is turned on!",
621                        slave_id
622                    );
623                    return Err("Cannot change mode of operation when torque is on!".into());
624                }
625                false => self.set_pdo_register(slave_id, PdoRegister::ModeOfOperation, 0, &[value]),
626            },
627            _ => {
628                log::error!("Slave {} | Error getting torque state!", slave_id);
629                return Err("Error getting torque state!".into());
630            }
631        }
632    }
633
634    fn get_register_values(
635        &self,
636        id: u32,
637        register: PdoRegister,
638    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
639        let slave_id = id as u16;
640        match self.get_pdo_registers(slave_id, register) {
641            Ok(bytes) => {
642                let values = bytes
643                    .iter()
644                    .map(|x| f32::from_le_bytes(x[0..4].try_into().unwrap()))
645                    .collect::<Vec<f32>>();
646                Ok(Some(values))
647            }
648            Err(_) => Err("Error reading register!".into()),
649        }
650    }
651
652    pub fn get_current_position(
653        &self,
654        id: u32,
655    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
656        self.get_register_values(id, PdoRegister::PositionActualValue)
657    }
658
659    pub fn get_current_velocity(
660        &self,
661        id: u32,
662    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
663        self.get_register_values(id, PdoRegister::VelocityActualValue)
664    }
665
666    pub fn get_current_torque(
667        &self,
668        id: u32,
669    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
670        self.get_register_values(id, PdoRegister::TorqueActualValue)
671    }
672
673    pub fn get_current_axis_sensors(
674        &self,
675        id: u32,
676    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
677        self.get_register_values(id, PdoRegister::AxisSensorActualValue)
678    }
679
680    pub fn get_current_target_position(
681        &self,
682        id: u32,
683    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
684        self.get_register_values(id, PdoRegister::TargetPosition)
685    }
686
687    pub fn get_board_temperatures(
688        &self,
689        id: u32,
690    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
691        self.get_register_values(id, PdoRegister::BoardTemperature)
692    }
693
694    pub fn get_motor_temperatures(
695        &self,
696        id: u32,
697    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
698        self.get_register_values(id, PdoRegister::MotorTemperature)
699    }
700
701    pub fn get_temperatures(
702        &self,
703        id: u32,
704    ) -> Result<Option<(Vec<f32>, Vec<f32>)>, Box<dyn std::error::Error>> {
705        let board_temperatures = self.get_board_temperatures(id)?;
706        let motor_temperatures = self.get_motor_temperatures(id)?;
707        return match (board_temperatures, motor_temperatures) {
708            (Some(b), Some(m)) => Ok(Some((b, m))),
709            _ => Err("Error reading temperatures!".into()),
710        };
711    }
712
713    // axis sensor zeros in firmware
714    pub fn get_axis_sensor_zeros(
715        &self,
716        id: u32,
717    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
718        self.get_register_values(id, PdoRegister::AxisZeroPosition)
719    }
720
721    // we are not actually reading it
722    // we return the set value
723    pub fn get_current_velocity_limit(
724        &self,
725        id: u32,
726    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
727        self.get_register_values(id, PdoRegister::VelocityLimit)
728    }
729
730    // we are not actually reading it
731    // we return the set value
732    pub fn get_current_torque_limit(
733        &self,
734        id: u32,
735    ) -> Result<Option<Vec<f32>>, Box<dyn std::error::Error>> {
736        self.get_register_values(id, PdoRegister::TorqueLimit)
737    }
738
739    fn set_register_values(
740        &self,
741        id: u32,
742        register: PdoRegister,
743        values: Vec<f32>,
744    ) -> Result<(), Box<dyn std::error::Error>> {
745        let slave_id = id as u16;
746        let values_bytes: Vec<Vec<u8>> = values.iter().map(|&x| x.to_le_bytes().to_vec()).collect();
747        self.set_pdo_registers(slave_id, register, values_bytes)
748    }
749
750    pub fn set_target_position(
751        &self,
752        id: u32,
753        target_position: Vec<f32>,
754    ) -> Result<(), Box<dyn std::error::Error>> {
755        self.set_register_values(id, PdoRegister::TargetPosition, target_position)
756    }
757
758    pub fn set_velocity_limit(
759        &self,
760        id: u32,
761        velocity_limit: Vec<f32>,
762    ) -> Result<(), Box<dyn std::error::Error>> {
763        self.set_register_values(id, PdoRegister::VelocityLimit, velocity_limit)
764    }
765
766    pub fn set_torque_limit(
767        &self,
768        id: u32,
769        torque_limit: Vec<f32>,
770    ) -> Result<(), Box<dyn std::error::Error>> {
771        self.set_register_values(id, PdoRegister::TorqueLimit, torque_limit)
772    }
773
774    pub fn set_target_velocity(
775        &self,
776        id: u32,
777        target_velocity: Vec<f32>,
778    ) -> Result<(), Box<dyn std::error::Error>> {
779        self.set_register_values(id, PdoRegister::TargetVelocity, target_velocity)
780    }
781
782    pub fn set_target_torque(
783        &self,
784        id: u32,
785        target_torque: Vec<f32>,
786    ) -> Result<(), Box<dyn std::error::Error>> {
787        self.set_register_values(id, PdoRegister::TargetTorque, target_torque)
788    }
789
790    pub fn get_error_codes(&self, id: u32) -> Result<Vec<u32>, Box<dyn std::error::Error>> {
791        let slave_id = id as u16;
792        match self.get_pdo_registers(slave_id, PdoRegister::ErrorCode) {
793            Ok(bytes) => {
794                let mut error_codes = Vec::new();
795                for e in bytes.iter() {
796                    error_codes.push(u16::from_le_bytes(e[0..2].try_into().unwrap()) as u32);
797                }
798                Ok(error_codes)
799            }
800            Err(_) => Err("Error reading error codes!".into()),
801        }
802    }
803
804    pub fn emergency_stop(&self, id: u32) -> Result<(), Box<dyn std::error::Error>> {
805        let slave_id = id as u16;
806        self.set_controlword(slave_id, ControlWord::QuickStop.to_u16())
807    }
808
809    pub fn reactivate_after_emergency_stop(
810        &self,
811        id: u32,
812    ) -> Result<(), Box<dyn std::error::Error>> {
813        let slave_id = id as u16;
814        self.set_controlword(slave_id, ControlWord::SwitchOn.to_u16())
815    }
816
817    // emergency stop on all slaves connected to the ethercat network
818    pub fn emergency_stop_all(&self, slave_id: u16) -> Result<(), Box<dyn std::error::Error>> {
819        for id in self.get_slave_ids() {
820            self.emergency_stop(id as u32)?;
821        }
822        Ok(())
823    }
824}