capsules_extra/servo.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
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}