capsules_extra/
sg90.rs

1// Licensed under the Apache License, Version 2.0 or the MIT License.
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Copyright Tock Contributors 2024.
4
5use core::cell::Cell;
6use core::mem::size_of;
7use kernel::hil;
8use kernel::ErrorCode;
9pub struct Sg90<'a, P: hil::pwm::PwmPin> {
10    /// The underlying PWM generator to change the angle.
11    pwm_pin: &'a P,
12    /// Stores the angle everytime it changes.
13    current_angle: Cell<Option<usize>>,
14}
15
16impl<'a, P: hil::pwm::PwmPin> Sg90<'a, P> {
17    pub fn new(pwm_pin: &'a P) -> Sg90<'a, P> {
18        Sg90 {
19            pwm_pin,
20            current_angle: Cell::new(None),
21        }
22    }
23}
24
25impl<'a, P: hil::pwm::PwmPin> kernel::hil::servo::Servo<'a> for Sg90<'a, P> {
26    fn set_angle(&self, angle: u16) -> Result<(), ErrorCode> {
27        // The assert! macro ensures that the code will not compile on platforms
28        // where `usize` is smaller than `u16`.
29        const _: () = assert!(size_of::<usize>() >= size_of::<u16>());
30        if angle <= 180 {
31            self.current_angle.set(Some(angle as usize));
32            // As specified in the datasheet:
33            // https://www.friendlywire.com/projects/ne555-servo-safe/SG90-datasheet.pdf,
34            // the frequency used for sg90 servo is always 50hz.
35            const FREQUENCY_HZ: usize = 50;
36            // This calculates the pulse width in microseconds for a specific angle.
37            // 500 and 2000 miliseconds define the range within
38            // which the angle can be set to any position.
39            let pulse_width_us = 500 + 2000 / 180 * (angle as usize);
40            // The duty_cycle formula is (pulse_width/period)*100.
41            // The period is 20 000 miliseconds (also specified in the datasheet).
42            // If we simplify we're left with pulse_width/20.
43            // We also need to scale this to the maximum duty_cycle suported by the pin.
44            // We do this by multiplying the value we get from the
45            // get_maximum_duty_cycle() function with pulse_width/20 and divide it by 100.
46            // This leaves us with the below formula:
47            let duty_cycle = pulse_width_us * self.pwm_pin.get_maximum_duty_cycle() / 20000;
48            self.pwm_pin.start(FREQUENCY_HZ, duty_cycle)?;
49            Ok(())
50        } else {
51            Err(ErrorCode::INVAL)
52        }
53    }
54
55    fn get_angle(&self) -> Result<usize, ErrorCode> {
56        //The SG90 servomotor cannot return its angle.
57        Err(ErrorCode::NOSUPPORT)
58    }
59}
capsules_extra/sg90.rs

capsules_extra/
sg90.rs
