capsules_extra/
servo.rs

Help
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
5//! This provides virtualized userspace access to a servomotor.
6//!
7//! Usage
8//! -----
9//!
10//! ```ignore
11//! use kernel::static_init;
12//! let mux_pwm = components::pwm::PwmMuxComponent::new(&peripherals.pwm)
13//! .finalize(components::pwm_mux_component_static!(rp2040::pwm::Pwm));
14//!
15//! let virtual_pwm_servo: &PwmPinUser<'static, rp2040::pwm::Pwm<'static>> =
16//! components::pwm::PwmPinUserComponent::new(mux_pwm, rp2040::gpio::RPGpio::GPIO4)
17//!     .finalize(components::pwm_pin_user_component_static!(rp2040::pwm::Pwm));
18//!
19//! let sg90_servo = static_init!(
20//! capsules_extra::sg90::Sg90<
21//!    'static,
22//!    capsules_core::virtualizers::virtual_pwm::PwmPinUser<'static, rp2040::pwm::Pwm>,
23//! >,
24//! capsules_extra::sg90::Sg90::new(virtual_pwm_servo)
25//! );
26//!
27//! // Here, we initialize an array of two SG90 servomotors as an example.
28//! let multi_servo = static_init!(
29//! [&'static dyn hil::servo::Servo<'static>; 2],
30//! [sg90_servo, sg90_servo]
31//! );
32//! let servo = static_init!(
33//! capsules_extra::servo::Servo<'static, 2>,
34//! capsules_extra::servo::Servo::new(multi_servo)
35//! );
36//! ```
37
38use kernel::hil;
39use kernel::syscall::{CommandReturn, SyscallDriver};
40use kernel::{ErrorCode, ProcessId};
41
42/// Syscall driver number.
43use capsules_core::driver;
44pub const DRIVER_NUM: usize = driver::NUM::Servo as usize;
45
46pub struct Servo<'a, const SERVO_COUNT: usize> {
47    /// The service capsule servo.
48    servo: &'a [&'a dyn hil::servo::Servo<'a>; SERVO_COUNT],
49}
50
51impl<'a, const SERVO_COUNT: usize> Servo<'a, SERVO_COUNT> {
52    pub fn new(servo: &'a [&'a dyn hil::servo::Servo<'a>; SERVO_COUNT]) -> Self {
53        Self { servo }
54    }
55}
56/// Provide an interface for userland.
57impl<const SERVO_COUNT: usize> SyscallDriver for Servo<'_, SERVO_COUNT> {
58    /// Command interface.
59    ///
60    /// ### `command_num`
61    ///
62    /// - `0`: Return Ok(()) if this driver is included on the platform.
63    /// - `1`: Returns an u32 representing the number of available servomotors.
64    /// - `2`: Changing the angle immediatelly.`servo_index` receives the index
65    /// corresponding to the servo whose angle we want to adjust
66    /// `angle` is used to receive a value between 0 and 180.
67    /// - `3`: Returning the current angle for a specific index.
68    fn command(
69        &self,
70        command_num: usize,
71        servo_index: usize,
72        angle: usize,
73        _processid: ProcessId,
74    ) -> CommandReturn {
75        match command_num {
76            // Check whether the driver exists.
77            0 => CommandReturn::success(),
78            // Returns the number of available servomotors.
79            1 => CommandReturn::success_u32(SERVO_COUNT as u32),
80            // Change the angle immediately.
81            2 => {
82                if servo_index >= SERVO_COUNT {
83                    CommandReturn::failure(ErrorCode::NODEVICE)
84                } else {
85                    match angle.try_into() {
86                        Ok(angle) => match self.servo[servo_index].set_angle(angle) {
87                            Ok(()) => CommandReturn::success(),
88                            Err(_) => CommandReturn::failure(ErrorCode::FAIL),
89                        },
90                        Err(_) => CommandReturn::failure(ErrorCode::INVAL),
91                    }
92                }
93            }
94            // Return the current angle.
95            3 => {
96                if servo_index >= SERVO_COUNT {
97                    CommandReturn::failure(ErrorCode::NODEVICE)
98                } else {
99                    match self.servo[servo_index].get_angle() {
100                        Ok(angle) => CommandReturn::success_u32(angle as u32),
101                        Err(err) => CommandReturn::failure(err),
102                    }
103                }
104            }
105            _ => CommandReturn::failure(ErrorCode::NOSUPPORT),
106        }
107    }
108
109    fn allocate_grant(&self, _process_id: ProcessId) -> Result<(), kernel::process::Error> {
110        Ok(())
111    }
112}
capsules_extra/servo.rs

capsules_extra/
servo.rs
