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 let slaves = controller.get_slave_ids_and_names();
52
53 for (slave_id, slave_name) in slaves {
55 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 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 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 return Err("Fault status".into());
221 }
222
223 if status_bits.contains(&bit) {
224 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, ®.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, ®.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, ®.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, ®.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 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 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 #[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 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 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 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 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 status_bits = self.get_status_bits(slave_id)?;
412 }
413
414 if status_bits.contains(&StatusBit::OperationEnabled) {
416 #[cfg(feature = "turn_off_slaves_setup")]
417 {
418 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 )?; 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 status_bits.contains(&StatusBit::SwitchedOnDisabled) {
439 self.set_controlword(slave_id, ControlWord::Shutdown.to_u16())?;
441 }
442
443 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 status_bits.contains(&StatusBit::ReadyToSwitchOn) {
449 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 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(());
488
489 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(());
499
500 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 requested_torque {
515 #[cfg(feature = "safe_turn_on")]
516 {
517 let current_position = self.get_current_position(id).unwrap().unwrap();
519 self.set_target_position(id, current_position.clone())
520 .unwrap();
521
522 let mut target_position =
524 self.get_current_target_position(id).unwrap().unwrap();
525 let mut tries = 0;
526 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 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 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 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 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 pub fn set_mode_of_operation(&self, slave_id: u16, value: u8) -> Result<(), Box<dyn Error>> {
607 let mode_of_operation = self.get_mode_of_operation_display(slave_id)?;
609 if mode_of_operation == value {
610 return Ok(());
611 }
612
613 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 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 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 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 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}