Merge pull request tock#4126 from inesmaria08/servo_motor_driver

Servomotor driver

boards/components/src/lib.rs

@@ -74,6 +74,7 @@ pub mod rng;
 pub mod sched;
 pub mod screen;
 pub mod segger_rtt;
+pub mod servo;
 pub mod sh1106;
 pub mod sha;
 pub mod sht3x;

boards/components/src/servo.rs

@@ -0,0 +1,60 @@
+// Licensed under the Apache License, Version 2.0 or the MIT License.
+// SPDX-License-Identifier: Apache-2.0 OR MIT
+// Copyright Tock Contributors 2024.
+
+//! Components for collections of servomotors.
+//!
+//! Usage
+//! -----
+//! ```rust
+//! let servo = components::servo::ServosComponent::new().finalize(components::servo_component_static!(
+//!     servo1, servo2,
+//! ));
+//! ```
+use capsules_extra::servo::Servo as ServoDriver;
+use core::mem::MaybeUninit;
+use kernel::component::Component;
+
+#[macro_export]
+macro_rules! servo_component_static {
+    ($($S:expr),+ $(,)?) => {{
+        use kernel::count_expressions;
+        use kernel::static_init;
+        const SERVO_COUNT: usize = count_expressions!($($S),+);
+        let arr = static_init!(
+            [&'static dyn Servo; SERVO_COUNT],
+            [
+                $(
+
+                        $S
+
+                ),+
+            ]
+        );
+
+        let servo = kernel::static_buf!( capsules_extra::servo::Servo<'static, SERVO_COUNT>);
+        (servo, arr)
+    };};
+}
+
+pub type ServosComponentType<const SERVO_COUNT: usize> = ServoDriver<'static, SERVO_COUNT>;
+
+pub struct ServosComponent<const SERVO_COUNT: usize> {}
+
+impl<const SERVO_COUNT: usize> ServosComponent<SERVO_COUNT> {
+    pub fn new() -> Self {
+        Self {}
+    }
+}
+
+impl<const SERVO_COUNT: usize> Component for ServosComponent<SERVO_COUNT> {
+    type StaticInput = (
+        &'static mut MaybeUninit<ServoDriver<'static, SERVO_COUNT>>,
+        &'static mut [&'static dyn kernel::hil::servo::Servo<'static>; SERVO_COUNT],
+    );
+    type Output = &'static ServoDriver<'static, SERVO_COUNT>;
+
+    fn finalize(self, static_buffer: Self::StaticInput) -> Self::Output {
+        static_buffer.0.write(ServoDriver::new(static_buffer.1))
+    }
+}

capsules/core/src/driver.rs

@@ -93,5 +93,6 @@ pub enum NUM {
     KeyboardHid           = 0x90005,
     DateTime              = 0x90007,
     CycleCount            = 0x90008,
+    Servo                 = 0x90009,
 }
 }

capsules/extra/README.md

@@ -116,6 +116,7 @@ These provide common and better abstractions for userspace.
 - **[App Flash](src/app_flash_driver.rs)**: Allow applications to write their
   own flash.
 - **[Buzzer](src/buzzer_driver.rs)**: Simple buzzer.
+- **[Servo](src/servo.rs)**: Servo motor.
 - **[Date-Time](src/date_time.rs)**: Real time clock date/time support.
 - **[EUI64](src/eui64.rs)**: Query device's extended unique ID.
 - **[HMAC](src/hmac.rs)**: Hash-based Message Authentication Code support.
@@ -156,6 +157,7 @@ Other capsules that implement reusable logic.
 
 - **[Bus Adapters](src/bus.rs)**: Generic abstraction for SPI/I2C/8080.
 - **[Buzzer PWM](src/buzzer_pwm.rs)**: Buzzer with a PWM pin.
+- **[SG90 PWM](src/sg90.rs)**: SG90 servomotor.
 - **[HMAC-SHA256](src/hmac_sha256.rs)**: HMAC using SHA-256.
 - **[Key-Value Store with Permissions](src/kv_store_permissions.rs)**: Key-value
   interface that requires read/write permissions.

capsules/extra/src/lib.rs

@@ -80,7 +80,9 @@ pub mod rf233_const;
 pub mod screen;
 pub mod screen_shared;
 pub mod sdcard;
+pub mod servo;
 pub mod seven_segment;
+pub mod sg90;
 pub mod sh1106;
 pub mod sha;
 pub mod sha256;

capsules/extra/src/servo.rs

@@ -0,0 +1,110 @@
+// Licensed under the Apache License, Version 2.0 or the MIT License.
+// SPDX-License-Identifier: Apache-2.0 OR MIT
+// Copyright Tock Contributors 2024.
+
+//! This provides virtualized userspace access to a servomotor.
+//!
+//! Usage
+//! -----
+//!
+//! use kernel::static_init;
+//! let mux_pwm = components::pwm::PwmMuxComponent::new(&peripherals.pwm)
+//! .finalize(components::pwm_mux_component_static!(rp2040::pwm::Pwm));
+//!
+//! let virtual_pwm_servo: &PwmPinUser<'static, rp2040::pwm::Pwm<'static>> =
+//! components::pwm::PwmPinUserComponent::new(mux_pwm, rp2040::gpio::RPGpio::GPIO4)
+//!     .finalize(components::pwm_pin_user_component_static!(rp2040::pwm::Pwm));
+//!
+//! let sg90_servo = static_init!(
+//! capsules_extra::sg90::Sg90<
+//!    'static,
+//!    capsules_core::virtualizers::virtual_pwm::PwmPinUser<'static, rp2040::pwm::Pwm>,
+//! >,
+//! capsules_extra::sg90::Sg90::new(virtual_pwm_servo)
+//! );
+//!
+//! // Here, we initialize an array of two SG90 servomotors as an example.
+//! let multi_servo = static_init!(
+//! [&'static dyn hil::servo::Servo<'static>; 2],
+//! [sg90_servo, sg90_servo]
+//! );
+//! let servo = static_init!(
+//! capsules_extra::servo::Servo<'static, 2>,
+//! capsules_extra::servo::Servo::new(multi_servo)
+//! );
+
+use kernel::hil;
+use kernel::syscall::{CommandReturn, SyscallDriver};
+use kernel::{ErrorCode, ProcessId};
+
+/// Syscall driver number.
+use capsules_core::driver;
+pub const DRIVER_NUM: usize = driver::NUM::Servo as usize;
+
+pub struct Servo<'a, const SERVO_COUNT: usize> {
+    /// The service capsule servo.
+    servo: &'a [&'a dyn hil::servo::Servo<'a>; SERVO_COUNT],
+}
+
+impl<'a, const SERVO_COUNT: usize> Servo<'a, SERVO_COUNT> {
+    pub fn new(servo: &'a [&'a dyn hil::servo::Servo<'a>; SERVO_COUNT]) -> Self {
+        Self { servo }
+    }
+}
+/// Provide an interface for userland.
+impl<'a, const SERVO_COUNT: usize> SyscallDriver for Servo<'a, SERVO_COUNT> {
+    /// Command interface.
+    ///
+    /// ### `command_num`
+    ///
+    /// - `0`: Return Ok(()) if this driver is included on the platform.
+    /// - `1`: Returns an u32 representing the number of available servomotors.
+    /// - `2`: Changing the angle immediatelly.`servo_index` receives the index
+    /// corresponding to the servo whose angle we want to adjust
+    /// `angle` is used to receive a value between 0 and 180.
+    /// - `3`: Returning the current angle for a specific index.
+    fn command(
+        &self,
+        command_num: usize,
+        servo_index: usize,
+        angle: usize,
+        _processid: ProcessId,
+    ) -> CommandReturn {
+        match command_num {
+            // Check whether the driver exists.
+            0 => CommandReturn::success(),
+            // Returns the number of available servomotors.
+            1 => CommandReturn::success_u32(SERVO_COUNT as u32),
+            // Change the angle immediately.
+            2 => {
+                if servo_index >= SERVO_COUNT {
+                    CommandReturn::failure(ErrorCode::NODEVICE)
+                } else {
+                    match angle.try_into() {
+                        Ok(angle) => match self.servo[servo_index].set_angle(angle) {
+                            Ok(()) => CommandReturn::success(),
+                            Err(_) => CommandReturn::failure(ErrorCode::FAIL),
+                        },
+                        Err(_) => CommandReturn::failure(ErrorCode::INVAL),
+                    }
+                }
+            }
+            // Return the current angle.
+            3 => {
+                if servo_index >= SERVO_COUNT {
+                    CommandReturn::failure(ErrorCode::NODEVICE)
+                } else {
+                    match self.servo[servo_index].get_angle() {
+                        Ok(angle) => CommandReturn::success_u32(angle as u32),
+                        Err(err) => CommandReturn::failure(err),
+                    }
+                }
+            }
+            _ => CommandReturn::failure(ErrorCode::NOSUPPORT),
+        }
+    }
+
+    fn allocate_grant(&self, _process_id: ProcessId) -> Result<(), kernel::process::Error> {
+        Ok(())
+    }
+}

capsules/extra/src/sg90.rs

@@ -0,0 +1,59 @@
+// Licensed under the Apache License, Version 2.0 or the MIT License.
+// SPDX-License-Identifier: Apache-2.0 OR MIT
+// Copyright Tock Contributors 2024.
+
+use core::cell::Cell;
+use core::mem::size_of;
+use kernel::hil;
+use kernel::ErrorCode;
+pub struct Sg90<'a, P: hil::pwm::PwmPin> {
+    /// The underlying PWM generator to change the angle.
+    pwm_pin: &'a P,
+    /// Stores the angle everytime it changes.
+    current_angle: Cell<Option<usize>>,
+}
+
+impl<'a, P: hil::pwm::PwmPin> Sg90<'a, P> {
+    pub fn new(pwm_pin: &'a P) -> Sg90<'a, P> {
+        Sg90 {
+            pwm_pin,
+            current_angle: Cell::new(None),
+        }
+    }
+}
+
+impl<'a, P: hil::pwm::PwmPin> kernel::hil::servo::Servo<'a> for Sg90<'a, P> {
+    fn set_angle(&self, angle: u16) -> Result<(), ErrorCode> {
+        // The assert! macro ensures that the code will not compile on platforms
+        // where `usize` is smaller than `u16`.
+        const _: () = assert!(size_of::<usize>() >= size_of::<u16>());
+        if angle <= 180 {
+            self.current_angle.set(Some(angle as usize));
+            // As specified in the datasheet:
+            // https://www.friendlywire.com/projects/ne555-servo-safe/SG90-datasheet.pdf,
+            // the frequency used for sg90 servo is always 50hz.
+            const FREQUENCY_HZ: usize = 50;
+            // This calculates the pulse width in microseconds for a specific angle.
+            // 500 and 2000 miliseconds define the range within
+            // which the angle can be set to any position.
+            let pulse_width_us = 500 + 2000 / 180 * (angle as usize);
+            // The duty_cycle formula is (pulse_width/period)*100.
+            // The period is 20 000 miliseconds (also specified in the datasheet).
+            // If we simplify we're left with pulse_width/20.
+            // We also need to scale this to the maximum duty_cycle suported by the pin.
+            // We do this by multiplying the value we get from the
+            // get_maximum_duty_cycle() function with pulse_width/20 and divide it by 100.
+            // This leaves us with the below formula:
+            let duty_cycle = pulse_width_us * self.pwm_pin.get_maximum_duty_cycle() / 20000;
+            self.pwm_pin.start(FREQUENCY_HZ, duty_cycle)?;
+            Ok(())
+        } else {
+            Err(ErrorCode::INVAL)
+        }
+    }
+
+    fn get_angle(&self) -> Result<usize, ErrorCode> {
+        //The SG90 servomotor cannot return its angle.
+        Err(ErrorCode::NOSUPPORT)
+    }
+}

doc/syscalls/90009_servo.md

@@ -0,0 +1,55 @@
+---
+driver number: 0x90009
+---
+
+# servo
+
+## Overview
+
+The servo driver provides a simple interface for changing the angle and returning to the app the current angle of a servo motor from userland applications.
+
+## Command
+
+  * ### Command number: `0`
+
+    **Description**: Does the driver exist?
+
+    **Argument 1**: unused
+
+    **Argument 2**: unused
+
+    **Returns**: Success if it exists
+
+  * ### Command number: `1`
+
+    **Description**: Returns the number of servomotors available.
+
+    **Argument 1**: unused
+
+    **Argument 2**: unused
+
+    **Returns**: A value (u32) representing the number of servomotors available.
+
+
+  * ### Command number: `2`
+
+    **Description**: Changes the angle of the servo
+
+    **Argument 1**: receives the index (u16) for the servomotors array from the application
+
+    **Argument 2**: receives the angle (in degrees) from the application
+
+    **Returns**: "Ok" if successful, "Fail" if the angle could not be adjusted, "Inval" if the value provided exceeds 360 degrees, or "NoDevice" if the index exceeds the number of available servomotors.
+
+  * ### Command number: `3`
+
+    **Description**: Returns the current angle of the servo
+
+    **Argument 1**: receives the index (u16) for the servomotors array from the application
+
+    **Argument 2**: unused
+
+    **Returns**: A value (u32) representing the current angle if successful, "NoSupport" if the servo cannot return its angle, or "NoDevice" if the index exceeds the number of available servomotors.
+
+  * ### Any other command:
+    **Returns**: An error indicating the command is not supported

doc/syscalls/README.md

@@ -146,3 +146,5 @@ _Note:_ GPIO is slated for re-numbering in Tock 2.0.
 |2.0| Driver Number | Driver                                  | Description                                |
 |---|---------------|-----------------------------------------|--------------------------------------------|
 |   | 0x90000       | Buzzer                                  | Buzzer                                     |
+|   | 0x90009       | [Servo](90009_servo.md)                |                  |
+Servo

kernel/src/hil/mod.rs

@@ -32,6 +32,7 @@ pub mod radio;
 pub mod rng;
 pub mod screen;
 pub mod sensors;
+pub mod servo;
 pub mod spi;
 pub mod symmetric_encryption;
 pub mod text_screen;

kernel/src/hil/servo.rs

@@ -0,0 +1,27 @@
+// Licensed under the Apache License, Version 2.0 or the MIT License.
+// SPDX-License-Identifier: Apache-2.0 OR MIT
+// Copyright Tock Contributors 2024.
+
+use crate::ErrorCode;
+pub trait Servo<'a> {
+    /// Changes the angle of the servo.
+    /// Return values:
+    ///
+    /// - `Ok(())`: The attempt at changing the angle was successful.
+    /// - `FAIL`: Cannot change the angle.
+    /// - `INVAL`: The value exceeds u16, indicating it's incorrect
+    /// since servomotors can only have a maximum of 360 degrees.
+    /// - `NODEVICE`: The index exceeds the number of servomotors provided.
+    ///  # Arguments
+    /// - `angle` - the variable that receives the angle
+    /// (in degrees from 0 to 180) from the servo driver.
+    fn set_angle(&self, angle: u16) -> Result<(), ErrorCode>;
+
+    /// Returns the angle of the servo.
+    /// Return values:
+    ///
+    /// - `angle`: The value, in angles from 0 to 360, of the servo.
+    /// - `NOSUPPORT`:  The servo cannot return its angle.
+    /// - `NODEVICE`: The index exceeds the number of servomotors provided.
+    fn get_angle(&self) -> Result<usize, ErrorCode>;
+}