esp_hal_servo/
lib.rs

1//! A library for controlling servo motors using LEDC from [`esp-hal`](https://docs.rs/esp-hal/1.0.0/esp_hal/).
2//!
3//! This library provides two approaches for controlling servo motors:
4//!
5//! ## 1. Direct Angle Control
6//! Simply specify the desired angle from the servo's range and wait for it to reach the position.
7//! This is the simplest approach when you know the exact angle you want.
8//!
9//! ```no_run
10//! # use esp_hal_servo::*;
11//! # let mut servo = todo!();
12//! // Set servo to 42 degrees and wait for it to reach the position
13//! servo.set_angle(42.0);
14//! ```
15//!
16//! ## 2. Step-by-Step Control with Direction
17//! Specify the direction of movement and make a step. This approach gives you fine-grained
18//! control over the servo movement, allowing you to move it incrementally.
19//!
20//! ```no_run
21//! # use esp_hal_servo::*;
22//! # let mut servo = todo!();
23//! // Set direction to clockwise
24//! servo.set_dir(Dir::CW);
25//! // Make a step of 10 duty units
26//! servo.step(10.0)?;
27//! // Or make a step as a percentage of the total range
28//! servo.step_pct(5)?; // 5% of the range
29//! ```
30
31#![no_std]
32
33pub mod utils;
34
35use core::{
36    marker::PhantomData,
37    ops::{Neg, Range},
38};
39use esp_hal::{
40    gpio::DriveMode,
41    ledc::{
42        LSGlobalClkSource, Ledc,
43        channel::{self, Channel, ChannelHW, ChannelIFace},
44        timer::{self, Timer, TimerHW, TimerIFace, TimerSpeed, config::Duty},
45    },
46    time::Rate,
47};
48use log::{info, trace};
49
50#[derive(Debug, Clone)]
51pub struct ServoConfig {
52    /// Max angle that servo can be turned, mostly 180, 360.
53    pub max_angle: f32,
54    /// What frequency expect servo (ex. 50Hz for SG90).
55    pub frequency: Rate,
56    /// What pulse width in nanos servo supports (ex. 500000-2400000ns for SG90).
57    pub pulse_width_ns: Range<u32>,
58    /// PWM resolution in bits. Higher bits means more precise control.
59    pub duty: Duty,
60}
61
62impl ServoConfig {
63    /// Default servo configuration with 50Hz frequency and
64    /// pulse width range of 500000-2500000 ns (0.5-2.5ms).
65    pub fn default_servo(duty: Duty, max_angle: f32) -> Self {
66        ServoConfig {
67            max_angle,
68            frequency: Rate::from_hz(50),
69            // Standard servo pulse width range: 500-2500 us
70            pulse_width_ns: 500_000..2_500_000,
71            duty,
72        }
73    }
74
75    /// Config for [SG90](https://www.friendlywire.com/projects/ne555-servo-safe/SG90-datasheet.pdf).
76    /// Can be used for SG90s as well.
77    pub fn sg90(duty: Duty) -> Self {
78        Self {
79            pulse_width_ns: 500_000..2_400_000,
80            ..Self::default_servo(duty, 180.0)
81        }
82    }
83
84    /// Config for [MG995](https://www.electronicoscaldas.com/datasheet/MG995_Tower-Pro.pdf).
85    /// High-torque servo motor with metal gears.
86    /// Can be used for MG996, MG996R as well.
87    pub fn mg995(duty: Duty) -> Self {
88        Self::default_servo(duty, 180.0)
89    }
90
91    /// Helper function to configure a timer with this servo's configuration.
92    pub fn configure_timer<'a, S: TimerSpeed>(
93        &self,
94        ledc: &mut Ledc<'a>,
95        timer_num: timer::Number,
96        clock_source: S::ClockSourceType,
97    ) -> Result<Timer<'a, S>, timer::Error>
98    where
99        Timer<'a, S>: TimerHW<S>,
100    {
101        ledc.set_global_slow_clock(LSGlobalClkSource::APBClk);
102        let mut timer = ledc.timer::<S>(timer_num);
103        timer.configure(timer::config::Config {
104            duty: self.duty,
105            clock_source,
106            frequency: self.frequency,
107        })?;
108        Ok(timer)
109    }
110
111    /// Calculates duty range in absolute values for this servo configuration.
112    /// Returns absolute duty values (0..max_duty), not percentages.
113    pub fn calc_duty_range(&self, max_duty: f32) -> Range<f32> {
114        utils::calc_duty_range(
115            self.pulse_width_ns.clone(),
116            self.frequency.as_hz() as f32,
117            max_duty,
118        )
119    }
120
121    /// Transforms absolute duty value to angle in degrees.
122    /// Returns angle in degrees (0.0..max_angle).
123    pub fn duty_to_angle(&self, duty: f32, max_duty: f32, duty_range: &Range<f32>) -> f32 {
124        utils::duty_to_angle(duty, max_duty, duty_range)
125    }
126
127    /// Transforms angle in degrees to absolute duty value.
128    pub fn angle_to_duty(&self, angle: f32, duty_range: &Range<f32>) -> f32 {
129        utils::angle_to_duty(angle, self.max_angle, duty_range)
130    }
131}
132
133pub struct Servo<'a, S: TimerSpeed> {
134    name: &'static str,
135    channel: Channel<'a, S>,
136    /// Valid duty cycle range in absolute values (e.g., 102..491 for SG90 with 12-bit).
137    /// This corresponds to the pulse width range of the servo.
138    pub duty_range: Range<f32>,
139    config: ServoConfig,
140    /// Cached max duty value for further calculations.
141    max_duty: f32,
142    /// Current direction. Clockwise or counter-clockwise.
143    direction: Dir,
144    /// Current duty in absolute value (0..max_duty).
145    current_duty: f32,
146    _p: PhantomData<&'a mut ()>,
147}
148
149impl<'d, S: TimerSpeed> Servo<'d, S> {
150    /// Creates new servo driver instance for LEDC channel.
151    ///
152    /// # Arguments
153    ///
154    /// * `name` - Name identifier for the servo (for logging)
155    /// * `config` - Servo configuration
156    /// * `ledc` - LEDC peripheral instance
157    /// * `timer` - Configured timer instance (use `ServoConfig::configure_timer` to create it)
158    /// * `channel_num` - Channel number (e.g., `channel::Number::Channel0`)
159    /// * `pin` - GPIO pin to use for PWM output
160    pub fn new<'a>(
161        name: &'static str,
162        config: ServoConfig,
163        ledc: &mut Ledc<'a>,
164        timer: &'a Timer<'a, S>,
165        channel_num: channel::Number,
166        pin: impl esp_hal::gpio::OutputPin + 'a,
167    ) -> Result<Servo<'a, S>, channel::Error>
168    where
169        Timer<'a, S>: TimerHW<S>,
170    {
171        // Calculate max duty before configuring channel
172        let max_duty = match timer.duty() {
173            Some(duty) => (1u32 << duty as u32) - 1,
174            None => 4095, // Default to 12-bit if not configured
175        } as f32;
176
177        // Calculate duty range in absolute values
178        let duty_range = config.calc_duty_range(max_duty);
179
180        let mut channel = ledc.channel(channel_num, pin);
181        channel.configure(channel::config::Config {
182            timer,
183            duty_pct: 0,
184            drive_mode: DriveMode::PushPull,
185        })?;
186
187        let center_duty = duty_range.start + (duty_range.end - duty_range.start) / 2.0;
188        channel.set_duty_hw(center_duty as u32);
189
190        info!(
191            "{name} servo: duty_range={duty_range:?}, center_duty={center_duty}",
192            name = name,
193            duty_range = duty_range,
194            center_duty = center_duty,
195        );
196
197        Ok(Servo::<'a> {
198            name,
199            channel,
200            duty_range,
201            config,
202            direction: Dir::CW,
203            current_duty: center_duty,
204            max_duty,
205            _p: PhantomData,
206        })
207    }
208
209    /// Makes step in absolute duty units should be lesser than [`duty_range()`](Self::duty_range).
210    /// Return false if servo reaches min or max position.
211    /// See also [`step_pct()`](Self::step_pct) for percentage-based stepping.
212    /// Note: Step takes some time depending on servo speed.
213    pub fn step(&mut self, step_size: f32) -> Result<bool, channel::Error> {
214        let new_duty = self.calc_duty(step_size);
215
216        // Compare with epsilon to avoid floating point precision issues
217        if utils::approx_eq(new_duty, self.current_duty) {
218            return Ok(false);
219        }
220
221        // hardware method has better resolution
222        self.channel.set_duty_hw(new_duty as u32);
223        self.current_duty = new_duty;
224        trace!(
225            "{} servo step({}) to duty={}/{}",
226            &self.name, step_size, new_duty, self.max_duty
227        );
228        Ok(true)
229    }
230
231    /// Makes step in percentage of total range.
232    /// Returns false if servo reaches min or max position.
233    /// See also [`step()`](Self::step) for absolute duty-based stepping.
234    /// Note: Step takes some time depending on servo speed.
235    pub fn step_pct(&mut self, step_pct: u8) -> Result<bool, channel::Error> {
236        let step = (step_pct as f32 / 100.0) * self.duty_range();
237        self.step(step)
238    }
239
240    /// Set servo to move new direction.
241    /// Returns old direction if direction was actually changes.
242    pub fn set_dir(&mut self, dir: Dir) -> Option<Dir> {
243        if self.direction != dir {
244            let old = self.direction;
245            self.direction = dir;
246            Some(old)
247        } else {
248            None
249        }
250    }
251
252    /// Returns current direction value.
253    pub fn get_dir(&self) -> Dir {
254        self.direction
255    }
256
257    /// Returns current angle value in degrees.
258    pub fn get_angle(&self) -> f32 {
259        self.config
260            .duty_to_angle(self.current_duty, self.max_duty, &self.duty_range)
261    }
262
263    /// Sets servo to specified angle in degrees.
264    /// Note: turn to angle takes some time depending on servo speed.
265    pub fn set_angle(&mut self, angle: f32) -> bool {
266        let new_duty = self.config.angle_to_duty(angle, &self.duty_range);
267
268        let delta = new_duty - self.current_duty;
269        if delta > utils::EPSILON {
270            self.direction = Dir::CW;
271        } else if delta < utils::EPSILON.neg() {
272            self.direction = Dir::CCW;
273        } else {
274            return false;
275        }
276
277        self.channel.set_duty_hw(new_duty as u32);
278        self.current_duty = new_duty;
279        true
280    }
281
282    /// Returns the size of the duty range (difference between max and min duty values).
283    /// This is not the number of steps, but the range size in duty units.
284    pub fn duty_range(&self) -> f32 {
285        self.duty_range.end - self.duty_range.start
286    }
287
288    /// Calculates new duty based on current direction and step size.
289    /// Returns clamped duty value within valid range.
290    fn calc_duty(&self, step: f32) -> f32 {
291        let new_duty = match self.direction {
292            Dir::CW => {
293                // Move clockwise (increase duty)
294                self.current_duty + step
295            }
296            Dir::CCW => {
297                // Move counter-clockwise (decrease duty)
298                self.current_duty - step
299            }
300        };
301
302        let min_duty = self.duty_range.start;
303        let max_duty = self.duty_range.end - utils::EPSILON;
304        new_duty.clamp(min_duty, max_duty)
305    }
306}
307
308#[derive(PartialEq, Eq, Clone, Copy, Debug)]
309pub enum Dir {
310    /// Clockwise, increases angle.
311    CW,
312    /// Counter-clockwise, decreases angle.
313    CCW,
314}
esp_hal_servo/lib.rs

esp_hal_servo/
lib.rs
