#![doc = include_str!("../docs/how_servos_work.md")]
use crate::Result;
use core::cell::RefCell;
pub use device_envoy_core::servo::{Direction, Servo};
use esp_hal::gpio::{DriveMode, interconnect::PeripheralOutput};
use esp_hal::ledc::{LowSpeed, channel, timer};
use esp_hal::ledc::{channel::ChannelHW, channel::ChannelIFace, timer::TimerIFace};
use esp_hal::time::Rate;
use static_cell::StaticCell;
#[doc(inline)]
pub use crate::combine;
#[doc(inline)]
pub use crate::servo_player::servo_player;
#[doc(hidden)]
pub use device_envoy_core::servo::{
__servo_player_animate, __servo_player_hold, __servo_player_relax, __servo_player_set_degrees,
device_loop,
};
pub use device_envoy_core::servo::{
AtEnd, ServoPlayer, ServoPlayerHandle, ServoPlayerStatic, combine, linear,
};
pub mod servo_player_generated {
#[cfg(doc)]
pub use crate::servo_player::servo_player_generated::*;
}
const SERVO_PERIOD_US: u32 = 20_000;
const SERVO_TIMER_DUTY_BITS: u32 = 14;
pub const SERVO_MIN_US_DEFAULT: u32 = 500;
pub const SERVO_MAX_US_DEFAULT: u32 = 2_500;
pub struct ServoStatic {
timer: StaticCell<timer::Timer<'static, LowSpeed>>,
channel: StaticCell<channel::Channel<'static, LowSpeed>>,
timer_number: timer::Number,
channel_number: channel::Number,
min_us: u32,
max_us: u32,
max_degrees: u16,
direction: Direction,
}
impl ServoStatic {
#[must_use]
pub const fn new_static(
timer_number: timer::Number,
channel_number: channel::Number,
min_us: u32,
max_us: u32,
max_degrees: u16,
direction: Direction,
) -> Self {
assert!(min_us < max_us, "min_us must be less than max_us");
assert!(max_degrees > 0, "max_degrees must be positive");
Self {
timer: StaticCell::new(),
channel: StaticCell::new(),
timer_number,
channel_number,
min_us,
max_us,
max_degrees,
direction,
}
}
}
#[doc(hidden)]
pub struct ServoEsp {
channel: RefCell<&'static mut channel::Channel<'static, LowSpeed>>,
min_us: u32,
max_us: u32,
max_degrees: u16,
direction: Direction,
}
impl ServoEsp {
pub const DEFAULT_MAX_DEGREES: u16 = <Self as Servo>::DEFAULT_MAX_DEGREES;
pub fn new(
servo_static: &'static ServoStatic,
ledc: &esp_hal::ledc::Ledc<'static>,
pin: impl PeripheralOutput<'static>,
) -> Result<Self> {
let timer = servo_static
.timer
.init(ledc.timer::<LowSpeed>(servo_static.timer_number));
timer.configure(timer::config::Config {
duty: timer::config::Duty::Duty14Bit,
clock_source: timer::LSClockSource::APBClk,
frequency: Rate::from_hz(50),
})?;
let channel = servo_static
.channel
.init(ledc.channel(servo_static.channel_number, pin));
channel.configure(channel::config::Config {
timer,
duty_pct: 0,
drive_mode: DriveMode::PushPull,
})?;
Ok(Self {
channel: RefCell::new(channel),
min_us: servo_static.min_us,
max_us: servo_static.max_us,
max_degrees: servo_static.max_degrees,
direction: servo_static.direction,
})
}
fn pulse_for_degrees(&self, degrees: u16) -> u32 {
let pulse_span = self.max_us - self.min_us;
self.min_us
+ (u32::from(degrees) * pulse_span + u32::from(self.max_degrees / 2))
/ u32::from(self.max_degrees)
}
fn degrees_to_duty(&self, degrees: u16) -> u32 {
let pulse_us = self.pulse_for_degrees(degrees);
let duty_range = 1u32 << SERVO_TIMER_DUTY_BITS;
((pulse_us * duty_range) + (SERVO_PERIOD_US / 2)) / SERVO_PERIOD_US
}
}
impl Servo for ServoEsp {
const DEFAULT_MAX_DEGREES: u16 = 180;
fn set_degrees(&self, degrees: u16) {
assert!(degrees <= self.max_degrees);
let physical_degrees = match self.direction {
Direction::Forward => degrees,
Direction::Reverse => self.max_degrees - degrees,
};
let duty = self.degrees_to_duty(physical_degrees);
self.channel.borrow_mut().set_duty_hw(duty);
}
fn hold(&self) {}
fn relax(&self) {
self.channel
.borrow_mut()
.set_duty(0)
.expect("LEDC set_duty failed in Servo::relax");
}
}
#[doc(hidden)]
pub use paste;
#[macro_export]
#[doc(hidden)]
macro_rules! servo {
($($tt:tt)*) => { $crate::__servo_impl! { $($tt)* } };
}
#[doc(inline)]
pub use servo;
#[doc(hidden)]
#[macro_export]
macro_rules! __servo_impl {
(
$name:ident {
$($fields:tt)*
}
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [],
timer: [],
channel: [],
min_us: [],
max_us: [],
max_degrees: [],
direction: [],
fields: [ $($fields)* ]
}
};
(@__parse
name: $name:ident,
pin: [],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ pin: $pin:ident $(, $($rest:tt)*)? ]
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [$pin],
timer: [$($timer)?],
channel: [$($channel)?],
min_us: [$($min_us)?],
max_us: [$($max_us)?],
max_degrees: [$($max_degrees)?],
direction: [$($direction)?],
fields: [ $($($rest)*)? ]
}
};
(@__parse
name: $name:ident,
pin: [$_pin_seen:ident],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ pin: $pin:ident $(, $($rest:tt)*)? ]
) => {
compile_error!("servo! duplicate `pin` field");
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ timer: $timer:ident $(, $($rest:tt)*)? ]
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [$($pin)?],
timer: [$timer],
channel: [$($channel)?],
min_us: [$($min_us)?],
max_us: [$($max_us)?],
max_degrees: [$($max_degrees)?],
direction: [$($direction)?],
fields: [ $($($rest)*)? ]
}
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$_timer_seen:ident],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ timer: $timer:ident $(, $($rest:tt)*)? ]
) => {
compile_error!("servo! duplicate `timer` field");
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ channel: $channel:ident $(, $($rest:tt)*)? ]
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [$($pin)?],
timer: [$($timer)?],
channel: [$channel],
min_us: [$($min_us)?],
max_us: [$($max_us)?],
max_degrees: [$($max_degrees)?],
direction: [$($direction)?],
fields: [ $($($rest)*)? ]
}
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$_channel_seen:ident],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ channel: $channel:ident $(, $($rest:tt)*)? ]
) => {
compile_error!("servo! duplicate `channel` field");
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ min_us: $min_us:expr $(, $($rest:tt)*)? ]
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [$($pin)?],
timer: [$($timer)?],
channel: [$($channel)?],
min_us: [$min_us],
max_us: [$($max_us)?],
max_degrees: [$($max_degrees)?],
direction: [$($direction)?],
fields: [ $($($rest)*)? ]
}
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$_min_us_seen:expr],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ min_us: $min_us:expr $(, $($rest:tt)*)? ]
) => {
compile_error!("servo! duplicate `min_us` field");
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ max_us: $max_us:expr $(, $($rest:tt)*)? ]
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [$($pin)?],
timer: [$($timer)?],
channel: [$($channel)?],
min_us: [$($min_us)?],
max_us: [$max_us],
max_degrees: [$($max_degrees)?],
direction: [$($direction)?],
fields: [ $($($rest)*)? ]
}
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$_max_us_seen:expr],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ max_us: $max_us:expr $(, $($rest:tt)*)? ]
) => {
compile_error!("servo! duplicate `max_us` field");
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [],
direction: [$($direction:expr)?],
fields: [ max_degrees: $max_degrees:expr $(, $($rest:tt)*)? ]
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [$($pin)?],
timer: [$($timer)?],
channel: [$($channel)?],
min_us: [$($min_us)?],
max_us: [$($max_us)?],
max_degrees: [$max_degrees],
direction: [$($direction)?],
fields: [ $($($rest)*)? ]
}
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$_max_degrees_seen:expr],
direction: [$($direction:expr)?],
fields: [ max_degrees: $max_degrees:expr $(, $($rest:tt)*)? ]
) => {
compile_error!("servo! duplicate `max_degrees` field");
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ direction: $new_direction:expr $(, $($rest:tt)*)? ]
) => {
$crate::__servo_impl! {
@__parse
name: $name,
pin: [$($pin)?],
timer: [$($timer)?],
channel: [$($channel)?],
min_us: [$($min_us)?],
max_us: [$($max_us)?],
max_degrees: [$($max_degrees)?],
direction: [$new_direction],
fields: [ $($($rest)*)? ]
}
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$_direction_seen:expr],
fields: [ direction: $direction:expr $(, $($rest:tt)*)? ]
) => {
compile_error!("servo! duplicate `direction` field");
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ ]
) => {
$crate::__servo_impl! {
@__finish
name: $name,
pin: [$($pin)?],
timer: [$($timer)?],
channel: [$($channel)?],
min_us: [$($min_us)?],
max_us: [$($max_us)?],
max_degrees: [$($max_degrees)?],
direction: [$($direction)?]
}
};
(@__parse
name: $name:ident,
pin: [$($pin:ident)?],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?],
fields: [ $field:ident : $($value:tt)+ ]
) => {
compile_error!(
"servo! unknown field; expected `pin`, `timer`, `channel`, `min_us`, `max_us`, `max_degrees`, or `direction`"
);
};
(@__finish
name: $name:ident,
pin: [],
timer: [$($timer:ident)?],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?]
) => {
compile_error!("servo! missing required `pin` field");
};
(@__finish
name: $name:ident,
pin: [$pin:ident],
timer: [],
channel: [$($channel:ident)?],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?]
) => {
compile_error!("servo! missing required `timer` field");
};
(@__finish
name: $name:ident,
pin: [$pin:ident],
timer: [$timer:ident],
channel: [],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?]
) => {
compile_error!("servo! missing required `channel` field");
};
(@__finish
name: $name:ident,
pin: [$pin:ident],
timer: [$timer:ident],
channel: [$channel:ident],
min_us: [$($min_us:expr)?],
max_us: [$($max_us:expr)?],
max_degrees: [$($max_degrees:expr)?],
direction: [$($direction:expr)?]
) => {
$crate::servo::paste::paste! {
pub struct $name;
// Link-time ownership claims: duplicate timer or channel selection across the
#[used]
#[unsafe(no_mangle)]
static [<__device_envoy_esp_ledc_timer_claim_ $timer:lower>]: u8 = 0;
#[used]
#[unsafe(no_mangle)]
static [<__device_envoy_esp_ledc_channel_claim_ $channel:lower>]: u8 = 0;
static [<$name:upper _SERVO_STATIC>]: [<$name Static>] = $name::new_static();
pub struct [<$name Static>] {
servo_static: $crate::servo::ServoStatic,
}
impl $name {
#[must_use]
pub const fn new_static() -> [<$name Static>] {
[<$name Static>] {
servo_static: $crate::servo::ServoStatic::new_static(
::esp_hal::ledc::timer::Number::$timer,
::esp_hal::ledc::channel::Number::$channel,
$crate::__servo_impl!(@min_us $($min_us)?),
$crate::__servo_impl!(@max_us $($max_us)?),
$crate::__servo_impl!(@max_degrees $($max_degrees)?),
$crate::__servo_impl!(@direction $($direction)?),
),
}
}
pub fn new(
ledc: &::esp_hal::ledc::Ledc<'static>,
pin: ::esp_hal::peripherals::$pin<'static>,
) -> $crate::Result<$crate::servo::ServoEsp> {
$crate::servo::ServoEsp::new(&[<$name:upper _SERVO_STATIC>].servo_static, ledc, pin)
}
}
}
};
(@min_us $min_us:expr) => { $min_us };
(@min_us) => { $crate::servo::SERVO_MIN_US_DEFAULT };
(@max_us $max_us:expr) => { $max_us };
(@max_us) => { $crate::servo::SERVO_MAX_US_DEFAULT };
(@max_degrees $max_degrees:expr) => { $max_degrees };
(@max_degrees) => { $crate::servo::ServoEsp::DEFAULT_MAX_DEGREES };
(@direction $direction:expr) => { $direction };
(@direction) => { $crate::servo::Direction::Forward };
}