1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

use std::sync::Mutex;

use crate::{
    error::Error,
    traits::{MotorDriveEffort, MotorDrivePosition, MotorDriveVelocity},
};

#[derive(Debug)]
pub struct DummyMotorDrivePosition {
    pub current_position: Mutex<f64>,
}

impl DummyMotorDrivePosition {
    pub fn new() -> Self {
        Self {
            current_position: Mutex::new(0f64),
        }
    }
}

impl Default for DummyMotorDrivePosition {
    fn default() -> Self {
        Self::new()
    }
}

impl MotorDrivePosition for DummyMotorDrivePosition {
    fn set_motor_position(&self, position: f64) -> Result<(), Error> {
        *self.current_position.lock().unwrap() = position;
        Ok(())
    }

    fn get_motor_position(&self) -> Result<f64, Error> {
        Ok(*self.current_position.lock().unwrap())
    }
}

#[derive(Debug)]
pub struct DummyMotorDriveVelocity {
    pub current_velocity: Mutex<f64>,
}

impl DummyMotorDriveVelocity {
    pub fn new() -> Self {
        Self {
            current_velocity: Mutex::new(0f64),
        }
    }
}

impl Default for DummyMotorDriveVelocity {
    fn default() -> Self {
        Self::new()
    }
}

impl MotorDriveVelocity for DummyMotorDriveVelocity {
    fn set_motor_velocity(&self, velocity: f64) -> Result<(), Error> {
        *self.current_velocity.lock().unwrap() = velocity;
        Ok(())
    }

    fn get_motor_velocity(&self) -> Result<f64, Error> {
        Ok(*self.current_velocity.lock().unwrap())
    }
}

#[derive(Debug)]
pub struct DummyMotorDriveEffort {
    pub current_effort: Mutex<f64>,
}

impl DummyMotorDriveEffort {
    pub fn new() -> Self {
        Self {
            current_effort: Mutex::new(0f64),
        }
    }
}

impl Default for DummyMotorDriveEffort {
    fn default() -> Self {
        Self::new()
    }
}

impl MotorDriveEffort for DummyMotorDriveEffort {
    fn set_motor_effort(&self, effort: f64) -> Result<(), Error> {
        *self.current_effort.lock().unwrap() = effort;
        Ok(())
    }

    fn get_motor_effort(&self) -> Result<f64, Error> {
        Ok(*self.current_effort.lock().unwrap())
    }
}