orbita3d_controller/io/
poulpe_ethercat.rs

1use motor_toolbox_rs::{Limit, MotorsController, RawMotorsIO, Result, PID};
2use poulpe_ethercat_grpc::client::PoulpeRemoteClient;
3use serde::{Deserialize, Serialize};
4use std::{f64::consts::PI, f64::consts::TAU, thread, time::Duration};
5
6use log::{error, info};
7
8use crate::ZeroType;
9
10#[derive(Debug, Deserialize, Serialize)]
11/// EthercatPoulpeController Config
12pub struct PoulpeEthercatConfig {
13    /// url of the ethercat master grpc serverS
14    pub url: String,
15    /// Actuator id
16    pub id: Option<u8>,
17    /// Actuator name
18    pub name: Option<String>,
19}
20
21#[derive(Debug)]
22/// EthercatPoulpeController - wrapper around the three disks motors
23pub struct EthercatPoulpeController {
24    io: PoulpeRemoteClient,
25    id: u16,
26
27    offsets: [Option<f64>; 3],
28    reduction: [Option<f64>; 3],
29    // motor_reduction: [Option<f64>; 3],
30    limits: [Option<Limit>; 3],
31    inverted_axes: [Option<bool>; 3],
32    axis_sensor_zeros: [Option<f64>; 3],
33}
34
35impl EthercatPoulpeController {
36    /// Creates a new EthercatPoulpeController
37    pub fn new(
38        url: &str,
39        id: Option<u8>,
40        name: Option<String>,
41        zero: ZeroType,
42        reductions: f64,
43        inverted_axes: [Option<bool>; 3],
44    ) -> Result<Self> {
45        let update_time = Duration::from_secs_f32(0.002);
46
47        let mut io = match (id, name) {
48            (_, Some(name)) => {
49                log::info!("Connecting to the slave with name: {}", name);
50
51                match PoulpeRemoteClient::connect_with_name(url.parse()?, vec![name], update_time) {
52                    Ok(client) => client,
53                    Err(e) => {
54                        error!(
55                            "Error while connecting to EthercatPoulpeController: {:?}",
56                            e
57                        );
58                        return Err("Error while connecting to EthercatPoulpeController".into());
59                    }
60                }
61            }
62            (Some(id), None) => {
63                log::info!("Connecting to the slave with id: {}", id);
64
65                match PoulpeRemoteClient::connect(url.parse()?, vec![id as u16], update_time) {
66                    Ok(client) => client,
67                    Err(e) => {
68                        error!(
69                            "Error while connecting to EthercatPoulpeController: {:?}",
70                            e
71                        );
72                        return Err("Error while connecting to EthercatPoulpeController".into());
73                    }
74                }
75            }
76            _ => {
77                log::error!("Invalid config file, make sure to provide either the id or the name!");
78                return Err("Invalid config file".into());
79            }
80        };
81        let id = io.ids[0];
82        let name = io.names[0].clone();
83
84        // wait for the connection to be established
85        let mut trials = 20; // 2s
86        while io.get_state(id as u16).is_err() {
87            thread::sleep(Duration::from_millis(100));
88            log::warn!(
89                "Waiting for connection to Orbita3d PoulpeRemoteClient with id {}",
90                id
91            );
92            if trials == 0 {
93                log::error!(
94                    "Error: Timeout while connecting to the Orbita3d PoulpeRemoteClient with id {}",
95                    id
96                );
97                return Err(
98                    "Error: Timeout while connecting to the Orbita3d  PoulpeRemoteClient".into(),
99                );
100            }
101            trials -= 1;
102        }
103        log::info!(
104            "Connected Orbita3d Client created for Slave {} (id: {}), sampling time: {:}ms",
105            name,
106            id,
107            update_time.as_millis()
108        );
109
110        // set the initial velocity and torque limit to 100%
111        io.set_velocity_limit(id as u16, [1.0; 3].to_vec());
112        io.set_torque_limit(id as u16, [1.0; 3].to_vec());
113
114        //We can only change the mode if torque=off, then we ensure we are ProfilePositionMode
115        io.set_mode_of_operation(id as u16, 1); //0=NoMode, 1=ProfilePositionMode, 3=ProfileVelocityMode, 4=ProfileTorqueMode
116
117        let mut poulpe_controller = EthercatPoulpeController {
118            io,
119            id: id as u16,
120            offsets: [None; 3],
121            reduction: [Some(reductions); 3],
122            limits: [None; 3],
123            inverted_axes,
124            axis_sensor_zeros: [None; 3],
125        };
126
127        info!(
128            "Orbita3d EthercatPoulpeController:\n\t - url: {:?}\n\t - id: {:?}",
129            url, id
130        );
131
132        log::info!("Creacting controller");
133
134        match zero {
135            // deprecated
136            ZeroType::ApproximateHardwareZero(zero) => {
137                log::info!("ApproximateHardwarezero");
138
139                let current_pos = MotorsController::get_current_position(&mut poulpe_controller)?;
140                log::info!("Current position: {:?}", current_pos);
141
142                zero.hardware_zero
143                    .iter()
144                    .zip(current_pos.iter())
145                    .enumerate()
146                    .for_each(|(i, (&hardware_zero, &current_pos))| {
147                        poulpe_controller.offsets[i] = Some(find_closest_offset_to_zero(
148                            current_pos,
149                            hardware_zero,
150                            reductions,
151                        ));
152                    });
153            }
154            ZeroType::FirmwareZero(_) => {
155                log::info!(
156                    "FirmwareZero => zero has been done in firmware, no need to do it here."
157                );
158                //we only need to get these once, they are stored in the firmware
159                //beware offsets in config file are also used
160                let axis_sensor_zeros =
161                    MotorsController::get_axis_sensor_zeros(&mut poulpe_controller)?;
162                log::info!("Axis sensor zeros: {:?}", axis_sensor_zeros);
163                poulpe_controller.axis_sensor_zeros = [
164                    Some(axis_sensor_zeros[0]),
165                    Some(axis_sensor_zeros[1]),
166                    Some(axis_sensor_zeros[2]),
167                ];
168            }
169            //deprecated
170            ZeroType::ZeroStartup(_) => {
171                log::info!("ZeroStartup");
172
173                let current_pos = MotorsController::get_current_position(&mut poulpe_controller)?;
174
175                log::info!("Current position: {:?}", current_pos);
176
177                current_pos
178                    .iter()
179                    .enumerate()
180                    .for_each(|(i, &current_pos)| {
181                        poulpe_controller.offsets[i] = Some(current_pos);
182                    });
183            }
184            ZeroType::HallZero(_zero) => {
185                log::error!("HallZero Not supported with Ethercat!");
186            }
187        }
188
189        Ok(poulpe_controller)
190    }
191
192    pub fn get_rpy_inverted_axes(&self) -> [Option<bool>; 3] {
193        self.inverted_axes
194    }
195
196    pub fn id(&self) -> u8 {
197        self.id as u8
198    }
199}
200
201impl MotorsController<3> for EthercatPoulpeController {
202    fn io(&mut self) -> &mut dyn RawMotorsIO<3> {
203        self
204    }
205
206    fn offsets(&self) -> [Option<f64>; 3] {
207        self.offsets
208    }
209
210    fn reduction(&self) -> [Option<f64>; 3] {
211        let mut reduction = [None; 3];
212        reduction.iter_mut().enumerate().for_each(|(i, r)| {
213            *r = Some(self.reduction[i].unwrap());
214        });
215        reduction
216    }
217
218    fn limits(&self) -> [Option<motor_toolbox_rs::Limit>; 3] {
219        self.limits
220    }
221
222    // fn inverted_axes(&self) -> [Option<bool>; 3] {
223    //     // self.inverted_axes
224    //     [None, None, None] //For Orbita3d, we need to inverse axes in the roll/pitch/yaw space...
225    // }
226    fn output_inverted_axes(&self) -> [Option<bool>; 3] {
227        self.inverted_axes
228    }
229}
230
231impl RawMotorsIO<3> for EthercatPoulpeController {
232    fn name(&self) -> String {
233        "EthercatPoulpeController".to_string()
234    }
235
236    fn is_torque_on(&mut self) -> Result<[bool; 3]> {
237        match self.io.get_torque_state(self.id) {
238            Ok(state) => Ok([state, state, state]),
239            Err(_) => Err("Error while getting torque state".into()),
240        }
241    }
242
243    fn set_torque(&mut self, on: [bool; 3]) -> Result<()> {
244        assert!(on.iter().all(|&t| t == on[0]));
245        if on.iter().any(|&x| x) {
246            self.io.turn_on(self.id);
247        } else {
248            self.io.turn_off(self.id);
249        }
250        Ok(())
251    }
252
253    fn get_current_position(&mut self) -> Result<[f64; 3]> {
254        match self.io.get_position_actual_value(self.id) {
255            Ok(position) => Ok([position[0] as f64, position[1] as f64, position[2] as f64]),
256            Err(_) => Err("Error while getting position".into()),
257        }
258    }
259
260    fn get_current_velocity(&mut self) -> Result<[f64; 3]> {
261        match self.io.get_velocity_actual_value(self.id) {
262            Ok(velocity) => Ok([velocity[0] as f64, velocity[1] as f64, velocity[2] as f64]),
263            Err(_) => Err("Error while getting velocity".into()),
264        }
265    }
266
267    fn get_current_torque(&mut self) -> Result<[f64; 3]> {
268        match self.io.get_torque_actual_value(self.id) {
269            Ok(torque) => Ok([torque[0] as f64, torque[1] as f64, torque[2] as f64]),
270            Err(_) => Err("Error while getting torque".into()),
271        }
272    }
273
274    fn get_target_position(&mut self) -> Result<[f64; 3]> {
275        match self.io.get_target_position(self.id) {
276            Ok(position) => Ok([position[0] as f64, position[1] as f64, position[2] as f64]),
277            Err(_) => Err("Error while getting target position".into()),
278        }
279    }
280
281    fn set_target_position(&mut self, position: [f64; 3]) -> Result<()> {
282        let target_position = position.iter().map(|&x| x as f32).collect::<Vec<f32>>();
283        self.io.set_target_position(self.id, target_position);
284        Ok(())
285    }
286
287    // fn get_target_velocity(&mut self) -> Result<[f64; 3]> {
288    //     match self.io.get_target_velocity(self.id) {
289    //         Ok(vel) => Ok([vel[0] as f64, vel[1] as f64, vel[2] as f64]),
290    //         Err(_) => Err("Error while getting target velocity".into()),
291    //     }
292    // }
293
294    fn set_target_velocity(&mut self, vel: [f64; 3]) -> Result<()> {
295        let target_velocity = vel.iter().map(|&x| x as f32).collect::<Vec<f32>>();
296        self.io.set_target_velocity(self.id, target_velocity);
297        Ok(())
298    }
299
300    // fn get_target_torque(&mut self) -> Result<[f64; 3]> {
301    //     match self.io.get_target_torque(self.id) {
302    //         Ok(vel) => Ok([vel[0] as f64, vel[1] as f64, vel[2] as f64]),
303    //         Err(_) => Err("Error while getting target torque".into()),
304    //     }
305    // }
306
307    fn set_target_torque(&mut self, vel: [f64; 3]) -> Result<()> {
308        let target_torque = vel.iter().map(|&x| x as f32).collect::<Vec<f32>>();
309        self.io.set_target_torque(self.id, target_torque);
310        Ok(())
311    }
312
313    fn set_target_position_fb(&mut self, position: [f64; 3]) -> Result<[f64; 3]> {
314        let target_position = position.iter().map(|&x| x as f32).collect::<Vec<f32>>();
315        self.io.set_target_position(self.id, target_position);
316
317        match self.io.get_position_actual_value(self.id) {
318            Ok(position) => Ok([position[0] as f64, position[1] as f64, position[2] as f64]),
319            Err(_) => Err("Error while getting position".into()),
320        }
321    }
322
323    fn get_velocity_limit(&mut self) -> Result<[f64; 3]> {
324        match self.io.get_velocity_limit(self.id) {
325            Ok(limit) => Ok([limit[0] as f64, limit[1] as f64, limit[2] as f64]),
326            Err(_) => Err("Error while getting velocity limit".into()),
327        }
328    }
329
330    fn set_velocity_limit(&mut self, velocity: [f64; 3]) -> Result<()> {
331        let velocity_limit = velocity.iter().map(|&x| x as f32).collect::<Vec<f32>>();
332        self.io.set_velocity_limit(self.id, velocity_limit);
333        Ok(())
334    }
335
336    fn get_torque_limit(&mut self) -> Result<[f64; 3]> {
337        match self.io.get_torque_limit(self.id) {
338            Ok(limit) => Ok([limit[0] as f64, limit[1] as f64, limit[2] as f64]),
339            Err(_) => Err("Error while getting torque limit".into()),
340        }
341    }
342
343    fn set_torque_limit(&mut self, torque: [f64; 3]) -> Result<()> {
344        let torque_limit = torque.iter().map(|&x| x as f32).collect::<Vec<f32>>();
345        self.io.set_torque_limit(self.id, torque_limit);
346        Ok(())
347    }
348
349    //TODO
350    fn get_pid_gains(&mut self) -> Result<[PID; 3]> {
351        Ok([PID {
352            p: 0.0,
353            i: 0.0,
354            d: 0.0,
355        }; 3])
356    }
357
358    fn set_pid_gains(&mut self, _pid: [PID; 3]) -> Result<()> {
359        Ok(())
360    }
361    fn get_axis_sensors(&mut self) -> Result<[f64; 3]> {
362        match self.io.get_axis_sensors(self.id) {
363            Ok(mut sensor) => {
364                // substract the sensor zero and the axis offset
365                // FIXME: these are not the "output" angles. We need to compute kinematics and handle multiturn...
366                for (i, s) in sensor.iter_mut().enumerate() {
367                    // apply the gearing ratio first
368                    *s *= 1.0 / self.reduction[i].unwrap() as f32;
369                    // substract the zero and the offset
370                    if self.axis_sensor_zeros[i].is_some() {
371                        *s -= self.axis_sensor_zeros[i].unwrap() as f32;
372                    }
373                    // remove any offset
374                    if self.offsets[i].is_some() {
375                        *s -= self.offsets[i].unwrap() as f32;
376                    }
377                    // wrap to pi
378                    *s = wrap_to_pi(*s as f64) as f32;
379                }
380                Ok([sensor[0] as f64, sensor[1] as f64, sensor[2] as f64])
381            }
382
383            Err(_) => Err("Error while getting axis sensors".into()),
384        }
385    }
386
387    fn get_axis_sensor_zeros(&mut self) -> Result<[f64; 3]> {
388        match self.io.get_axis_sensor_zeros(self.id) {
389            Ok(sensor) => Ok([sensor[0] as f64, sensor[1] as f64, sensor[2] as f64]),
390            Err(_) => Err("Error while getting axis sensor zeros".into()),
391        }
392    }
393
394    fn get_board_state(&mut self) -> Result<u8> {
395        match self.io.get_state(self.id) {
396            Ok(state) => Ok(state as u8),
397            Err(_) => Err("Error while getting board state".into()),
398        }
399    }
400
401    fn get_error_codes(&mut self) -> Result<[i32; 3]> {
402        match self.io.get_error_codes(self.id) {
403            Ok(codes) => Ok([codes[0], codes[1], codes[2]]),
404            Err(_) => Err("Error while getting error codes".into()),
405        }
406    }
407
408    fn get_motor_temperatures(&mut self) -> Result<[f64; 3]> {
409        match self.io.get_motor_temperatures(self.id) {
410            Ok(temp) => Ok([temp[0] as f64, temp[1] as f64, temp[2] as f64]),
411            Err(_) => Err("Error while getting motor temperatures".into()),
412        }
413    }
414
415    fn get_board_temperatures(&mut self) -> Result<[f64; 3]> {
416        match self.io.get_board_temperatures(self.id) {
417            Ok(temp) => Ok([temp[0] as f64, temp[1] as f64, temp[2] as f64]),
418            Err(_) => Err("Error while getting board temperatures".into()),
419        }
420    }
421
422    fn set_board_state(&mut self, _state: u8) -> Result<()> {
423        Ok(())
424    }
425
426    fn get_control_mode(&mut self) -> Result<[u8; 3]> {
427        match self.io.get_mode_of_operation(self.id) {
428            Ok(mode) => Ok([mode as u8, mode as u8, mode as u8]), //It is in fact the same for each axis (TODO make it board level?)
429            Err(_) => Err("Error while getting mode of operation".into()),
430        }
431    }
432
433    fn set_control_mode(&mut self, _mode: [u8; 3]) -> Result<()> {
434        if !(_mode[0] == _mode[1] && _mode[1] == _mode[2]) {
435            return Err("Error, invalid control mode".into());
436        }
437        self.io.set_mode_of_operation(self.id, _mode[0] as u32);
438        Ok(())
439    }
440
441    fn emergency_stop(&mut self) {
442        self.io.emergency_stop(self.id);
443        error!("EMERCENCY STOP!");
444    }
445}
446
447fn find_closest_offset_to_zero(current_position: f64, hardware_zero: f64, reduction: f64) -> f64 {
448    //! Find the closest offset to zero
449    //!
450    //! The absolute encoder is placed before orbita reduction.
451    //! Thus, we do not have the real orbita disk absolute position.
452    //! But we only know where we are in a local dial of arc (2pi / reduction).
453    //!
454    //! To find the closest offset to our zero hardware, we assume that the current position is at maximum one dial from the hardware zero.
455    log::info!(
456        "find_closest_offset_to_zero: current_position: {}, hardware_zero: {}, reduction: {}",
457        current_position,
458        hardware_zero,
459        reduction
460    );
461
462    let dial_arc = 2.0 * PI / reduction;
463
464    let possibilities = [
465        hardware_zero - dial_arc,
466        hardware_zero,
467        hardware_zero + dial_arc,
468    ];
469    log::debug!("possibilities: {:?}", possibilities);
470
471    let best = possibilities
472        .iter()
473        .map(|&p| (p - current_position).abs())
474        .enumerate()
475        .min_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap())
476        .map(|(i, _)| possibilities[i])
477        .unwrap();
478
479    log::info!("best: {}", best);
480    best
481}
482
483#[allow(dead_code)]
484fn find_position_with_hall(
485    current_position: f64,
486    hardware_zero: f64,
487    hall_zero: f64,
488    hall_index: u8,
489    reduction: f64,
490) -> (f64, i16) {
491    //! Find the current position corrected by the hall sensor
492    //! There is 16 Hall sensors on the disk output and a ratio 'reduction' between the disk and the motor gearbox output
493    //! We knwow the 'hall_zero' index which correspond to the index of the Hall sensor closest to the disk zero position
494    //! We also know the 'hall_index' which is the index of the Hall sensor closest to the current position
495    //! Finally we know the 'hardware_zero' which is the position of the disk zero
496
497    const MAX_TURN: usize = 3;
498    let mut offset: [f64; MAX_TURN] = [0.0; MAX_TURN];
499    let mut offset_search: [f64; MAX_TURN] = [0.0; MAX_TURN];
500    let turn_offset = 2.0 * PI * reduction;
501    let hall_offset = 2.0 * PI / 16.0 * reduction; //22.5° disk for each Hall sensor
502
503    // let hall_diff = hall_diff(hall_index, hall_zero);
504
505    let diff_gear = current_position * reduction - hardware_zero * reduction;
506    let shortest_diff_gear = angle_diff(current_position * reduction, hardware_zero * reduction); //nul FIXME
507    let shortest_to_zero = angle_diff(0.0, hardware_zero * reduction);
508
509    let pos = (current_position * reduction) % TAU; //this should be the raw gearbox position
510    let _shortest_to_current = angle_diff(0.0, pos);
511    let _gearbox_turn = 0.0;
512
513    log::debug!(
514        "Diff: {:?} shortest diff: {:?} shortest_to_zero {:?} hall_zero_angle: {:?}",
515        diff_gear,
516        shortest_diff_gear,
517        shortest_to_zero,
518        hall_zero
519    );
520
521    for i in 0..offset.len() {
522        // theoretical position of the gearbox starting from the zero and moving toward detected hall
523
524        offset_search[i] = (hardware_zero * reduction) % TAU
525            + (hall_zero * reduction) % TAU
526            + ((i as f64 - (offset.len() / 2) as f64) * turn_offset) % TAU;
527        offset_search[i] %= TAU;
528
529        let residual = angle_diff(
530            pos,
531            (hardware_zero * reduction) % TAU + (hall_zero * reduction) % TAU,
532        ) / reduction;
533
534        // Offset to apply
535        offset[i] = current_position
536            - hall_zero
537            - residual
538            - (i as f64 - (offset.len() / 2) as f64)
539                * (turn_offset / reduction - TAU * (reduction - reduction.floor()) / reduction);
540
541        //in orbita ref
542    }
543
544    log::debug!(
545        "Residual (gearbox) {:?} (orbita) {:?}",
546        angle_diff(pos, (hall_zero * reduction) % TAU),
547        angle_diff(pos, (hall_zero * reduction) % TAU) / reduction
548    );
549    log::debug!("possible offset (orbita domain): {:?}", offset);
550    log::debug!("searching offset (gearbox domain): {:?}", offset_search);
551
552    log::debug!(
553        "current pos (gearbox): {:?} hardware_zero (gearbox): {:?} hall_idx: {:?} hall_zero: {:?} hall_offset: {:?} turn_offset: {:?}",
554        pos,
555        hardware_zero * reduction,
556        hall_index as f64,
557        hall_zero,
558        hall_offset,
559	turn_offset
560    );
561
562    let best = offset_search
563        .iter()
564        .map(|&p| {
565            let d = angle_diff(p, pos).abs();
566            log::debug!("Diff search: {:?}", d);
567            d
568        })
569        .enumerate()
570        .min_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap())
571        .map(|(i, _)| offset[i])
572        .unwrap();
573
574    let best_idx = offset.iter().position(|&x| x == best).unwrap();
575    log::debug!(
576        "best offset (orbita domain): {} gearbox domain: {:?}",
577        best,
578        offset_search[best_idx]
579    );
580    log::debug!(
581        "It corresponds to {} turn (orbita domain)",
582        best_idx as i16 - (offset.len() / 2) as i16
583    );
584
585    (best, best_idx as i16 - (offset.len() / 2) as i16)
586}
587
588#[allow(dead_code)]
589pub fn angle_diff(angle_a: f64, angle_b: f64) -> f64 {
590    let mut angle = angle_a - angle_b;
591    angle = (angle + PI) % TAU - PI;
592    if angle < -PI {
593        angle + TAU
594    } else {
595        angle
596    }
597}
598
599#[allow(dead_code)]
600pub fn hall_diff(hall_a: u8, hall_b: u8) -> f64 {
601    // shortest hall difference (16 discrete Hall)
602    let d: f64 = hall_a as f64 - hall_b as f64;
603    if d >= 0.0 {
604        if d >= 8.0 {
605            d - 16.0
606        } else {
607            d
608        }
609    } else if d >= -8.0 {
610        d
611    } else {
612        d + 16.0
613    }
614}
615
616// function wrapping an angle in radians to
617// the range [-pi, pi]
618fn wrap_to_pi(angle: f64) -> f64 {
619    (((angle + PI) % (2.0 * PI)) + (2.0 * PI)) % (2.0 * PI) - PI
620}
621
622#[cfg(test)]
623mod tests {
624    use rand::Rng;
625    use std::f64::consts::PI;
626
627    use crate::{
628        io::{poulpe_ethercat::find_closest_offset_to_zero, Orbita3dIOConfig},
629        Orbita3dConfig,
630    };
631
632    #[test]
633    fn parse_config_file() {
634        let f = std::fs::File::open("./config/ethercat_poulpe.yaml").unwrap();
635
636        let config: Result<Orbita3dConfig, _> = serde_yaml::from_reader(f);
637        assert!(config.is_ok());
638
639        let config = config.unwrap();
640
641        if let Orbita3dIOConfig::PoulpeEthercat(dxl_config) = config.io {
642            assert_eq!(config.kinematics_model.alpha, 54.0_f64.to_radians());
643            assert_eq!(config.kinematics_model.gamma_min, 40.0_f64.to_radians());
644            assert_eq!(config.kinematics_model.offset, 0.0);
645            assert_eq!(config.kinematics_model.beta, PI / 2.0);
646            assert_eq!(config.kinematics_model.gamma_max, PI);
647            assert!(config.kinematics_model.passiv_arms_direct);
648
649            if let crate::ZeroType::FirmwareZero(_) = config.disks.zeros {
650            } else {
651                panic!("Wrong config type");
652            }
653            assert_eq!(config.disks.reduction, 5.333_333_333_333_333);
654            // assert_eq!(config.disks.reduction, 4.2666667); //Old Orbita
655
656            assert_eq!(dxl_config.url, "http://127.0.0.1:50098");
657            assert_eq!(dxl_config.id, Some(0));
658        } else {
659            panic!("Wrong config type");
660        }
661    }
662
663    #[test]
664    fn test_approx() {
665        let mut rng = rand::thread_rng();
666
667        let pos = rng.gen_range(-PI..PI);
668        let reduction = rng.gen_range(0.5..2.0);
669        assert_eq!(find_closest_offset_to_zero(pos, pos, reduction), pos);
670
671        let pos = 0.0;
672        let hardware_zero = 0.25;
673        let reduction = 1.0;
674
675        assert_eq!(
676            find_closest_offset_to_zero(pos, hardware_zero, reduction),
677            0.25
678        );
679
680        let reduction = 100.0;
681        assert_eq!(
682            find_closest_offset_to_zero(pos, hardware_zero, reduction),
683            0.25 - 2.0 * PI / reduction
684        );
685    }
686}
orbita3d_controller/io/poulpe_ethercat.rs

orbita3d_controller/io/
poulpe_ethercat.rs
