servo_libs_simulator/
lib.rs

1use serde::{Serialize, Deserialize};
2
3/// Servo motor specification
4#[derive(Debug, Clone, Serialize, Deserialize)]
5pub struct ServoSpec {
6    pub min_pulse: u32,   // e.g. 100 (for 0 degrees)
7    pub max_pulse: u32,   // e.g. 600 (for 180 degrees)
8    pub min_angle: f32,   // usually 0.0
9    pub max_angle: f32,   // usually 180.0
10}
11
12impl ServoSpec {
13    pub fn mg996r() -> Self {
14        Self {
15            min_pulse: 100,
16            max_pulse: 600,
17            min_angle: 0.0,
18            max_angle: 180.0,
19        }
20    }
21
22    /// Convert angle -> pulse
23    pub fn angle_to_pulse(&self, angle: f32) -> u32 {
24        let clamped = angle.clamp(self.min_angle, self.max_angle);
25        let ratio = (clamped - self.min_angle) / (self.max_angle - self.min_angle);
26        self.min_pulse + ((self.max_pulse - self.min_pulse) as f32 * ratio) as u32
27    }
28
29    /// Convert pulse -> angle
30    pub fn pulse_to_angle(&self, pulse: u32) -> f32 {
31        let clamped = pulse.clamp(self.min_pulse, self.max_pulse);
32        let ratio = (clamped - self.min_pulse) as f32 / (self.max_pulse - self.min_pulse) as f32;
33        self.min_angle + (self.max_angle - self.min_angle) * ratio
34    }
35}
36
37/// Represents a single servo channel
38#[derive(Debug, Clone, Serialize, Deserialize)]
39pub struct ServoChannel {
40    pub id: usize,
41    pub name: String,
42    pub angle: f32,
43    pub spec: ServoSpec,
44}
45
46impl ServoChannel {
47    pub fn new(id: usize, name: &str, spec: ServoSpec) -> Self {
48        Self {
49            id,
50            name: name.to_string(),
51            angle: 0.0,
52            spec,
53        }
54    }
55
56    pub fn set_angle(&mut self, angle: f32) {
57        self.angle = angle.clamp(self.spec.min_angle, self.spec.max_angle);
58    }
59
60    pub fn increment(&mut self, delta: f32) {
61        self.set_angle(self.angle + delta);
62    }
63
64    pub fn decrement(&mut self, delta: f32) {
65        self.set_angle(self.angle - delta);
66    }
67
68    pub fn get_pulse(&self) -> u32 {
69        self.spec.angle_to_pulse(self.angle)
70    }
71
72    pub fn set_pulse(&mut self, pulse: u32) {
73        self.angle = self.spec.pulse_to_angle(pulse);
74    }
75
76    pub fn smooth_move(current: f32, target: f32, speed: f32, delta: f32) -> f32 {
77        let diff = target - current;
78        if diff.abs() < 0.01 {
79            return target;
80        }
81        let step = speed * delta;
82        if diff > 0.0 {
83            (current + step).min(target)
84        } else {
85            (current - step).max(target)
86        }
87    }
88}
89
90/// System of multiple servo channels
91#[derive(Debug, Clone, Serialize, Deserialize)]
92pub struct ServoSystem {
93    pub channels: Vec<ServoChannel>,
94}
95
96impl ServoSystem {
97    pub fn new() -> Self {
98        Self { channels: Vec::new() }
99    }
100
101    pub fn add_channel(&mut self, channel: ServoChannel) {
102        self.channels.push(channel);
103    }
104
105    pub fn get_channel_mut(&mut self, id: usize) -> Option<&mut ServoChannel> {
106        self.channels.iter_mut().find(|c| c.id == id)
107    }
108}
servo_libs_simulator/lib.rs

servo_libs_simulator/
lib.rs
