diff --git a/Cargo.toml b/Cargo.toml index b1d956d8..77c2c030 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -36,6 +36,7 @@ libtock_i2c_master = { path = "apis/peripherals/i2c_master" } libtock_i2c_master_slave = { path = "apis/peripherals/i2c_master_slave" } libtock_key_value = { path = "apis/storage/key_value" } libtock_leds = { path = "apis/interface/leds" } +libtock_servo = { path = "apis/interface/servo" } libtock_low_level_debug = { path = "apis/kernel/low_level_debug" } libtock_ninedof = { path = "apis/sensors/ninedof" } libtock_platform = { path = "platform" } @@ -69,6 +70,7 @@ exclude = ["tock"] members = [ "apis/interface/buttons", "apis/interface/buzzer", + "apis/interface/servo", "apis/interface/console", "apis/interface/leds", "apis/kernel/low_level_debug", diff --git a/apis/interface/servo/Cargo.toml b/apis/interface/servo/Cargo.toml new file mode 100644 index 00000000..e9323033 --- /dev/null +++ b/apis/interface/servo/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "libtock_servo" +version = "0.1.0" +authors = ["Tock Project Developers "] +license = "Apache-2.0 OR MIT" +edition = "2021" +repository = "https://www.github.com/tock/libtock-rs" +rust-version.workspace = true +description = "libtock servomotor driver" + +[dependencies] +libtock_platform = { path = "../../../platform" } + +[dev-dependencies] +libtock_unittest = { path = "../../../unittest" } diff --git a/apis/interface/servo/src/lib.rs b/apis/interface/servo/src/lib.rs new file mode 100644 index 00000000..abc65f4e --- /dev/null +++ b/apis/interface/servo/src/lib.rs @@ -0,0 +1,70 @@ +#![no_std] + +use libtock_platform::{ErrorCode, Syscalls}; + +pub struct Servo(S); + +impl Servo { + /// Check whether the driver exists. + pub fn exists() -> Result<(), ErrorCode> { + let val = S::command(DRIVER_NUM, EXISTS, 0, 0).is_success(); + if val { + Ok(()) + } else { + Err(ErrorCode::Fail) + } + } + /// Returns the number of the servomotors available. + pub fn count() -> Result { + S::command(DRIVER_NUM, SERVO_COUNT, 0, 0).to_result() + } + + /// Changes the angle of the servo. + /// + /// # Arguments + /// - `angle` - the variable that receives the angle + /// (in degrees from 0 to 180) from the servo driver. + /// - `index` - the variable that receives the index of the servomotor. + /// + /// # Return values: + /// - `Ok(())`: The attempt at changing the angle was successful. + /// + /// # Errors: + /// - `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. + pub fn set_angle(index: u32, angle: u32) -> Result<(), ErrorCode> { + S::command(DRIVER_NUM, SET_ANGLE, index, angle).to_result() + } + + /// Returns the angle of the servo. + /// + /// # Arguments + /// - `index` - the variable that receives the index of the servomotor. + /// + /// # Return values: + /// - `angle`: The value, in angles from 0 to 360, of the servo. + /// + /// # Errors: + /// - `NOSUPPORT`: The servo cannot return its angle. + /// - `NODEVICE`: The index exceeds the number of servomotors provided. + pub fn get_angle(index: u32) -> Result { + S::command(DRIVER_NUM, GET_ANGLE, index, 0).to_result() + } +} + +#[cfg(test)] +mod tests; + +// ----------------------------------------------------------------------------- +// Driver number and command IDs +// ----------------------------------------------------------------------------- + +const DRIVER_NUM: u32 = 0x90009; + +// Command IDs +const EXISTS: u32 = 0; +const SERVO_COUNT: u32 = 1; +const SET_ANGLE: u32 = 2; +const GET_ANGLE: u32 = 3; diff --git a/apis/interface/servo/src/tests.rs b/apis/interface/servo/src/tests.rs new file mode 100644 index 00000000..79a256ed --- /dev/null +++ b/apis/interface/servo/src/tests.rs @@ -0,0 +1,39 @@ +use libtock_platform::ErrorCode; +use libtock_unittest::fake; + +type Servo = super::Servo; + +#[test] +fn no_driver() { + let _kernel = fake::Kernel::new(); + assert_eq!(Servo::exists(), Err(ErrorCode::Fail)) +} +#[test] +fn exists() { + let kernel = fake::Kernel::new(); + let driver = fake::Servo::<2>::new(); + kernel.add_driver(&driver); + assert_eq!(Servo::exists(), Ok(())); +} +#[test] +fn count() { + let kernel = fake::Kernel::new(); + let driver = fake::Servo::<2>::new(); + kernel.add_driver(&driver); + assert_eq!(Servo::count(), Ok(2)); +} +#[test] +fn set_angle() { + let kernel = fake::Kernel::new(); + let driver = fake::Servo::<2>::new(); + kernel.add_driver(&driver); + assert_eq!(Servo::set_angle(1, 90), Ok(())); +} +#[test] +fn get_angle() { + let kernel = fake::Kernel::new(); + let driver = fake::Servo::<2>::new(); + kernel.add_driver(&driver); + assert_eq!(Servo::set_angle(1, 45), Ok(())); + assert_eq!(Servo::get_angle(1), Ok(45)); +} diff --git a/examples/servo.rs b/examples/servo.rs new file mode 100644 index 00000000..83c44ab1 --- /dev/null +++ b/examples/servo.rs @@ -0,0 +1,59 @@ +#![no_main] +#![no_std] +use core::fmt::Write; +use libtock::alarm::{Alarm, Milliseconds}; +use libtock::console::Console; +use libtock::runtime::{set_main, stack_size}; +use libtock::servo::Servo; +use libtock_platform::ErrorCode; + +set_main! {main} +stack_size! {0x300} + +fn main() { + //Checks if the driver exists. + if Err(ErrorCode::Fail) == Servo::exists() { + writeln!(Console::writer(), "The driver could not be found").unwrap(); + return; + } + let count = Servo::count().unwrap(); + + writeln!( + Console::writer(), + "The number of available servomotors is {:?}", + count + ) + .unwrap(); + + let index: u32 = 0; // the first index available. + + // Changes the angle of the servomotor from 0 to 180 degrees (waiting 0.1 ms between every change). + // "i" represents the angle we set the servomotor at. + for i in 0..180 { + let val1 = Servo::set_angle(index, i); // stores the value returned by set_angle + let val2 = Servo::get_angle(index); // stores the value returned by get_angle + + if val1 == Err(ErrorCode::Fail) { + writeln!( + Console::writer(), + "The provided angle exceeds the servo's limit" + ) + .unwrap(); + } else if val2 == Err(ErrorCode::NoSupport) { + writeln!(Console::writer(), "The servo cannot return its angle").unwrap(); + } else if val1 == Err(ErrorCode::NoDevice) { + writeln!( + Console::writer(), + "The index exceeds the number of provided servomotors" + ) + .unwrap(); + } else if val2 == Err(ErrorCode::NoDevice) { + writeln!( + Console::writer(), + "The index exceeds the number of provided servomotors" + ) + .unwrap(); + } + Alarm::sleep_for(Milliseconds(100)).unwrap(); + } +} diff --git a/src/lib.rs b/src/lib.rs index 0e4acd36..853de44d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -66,6 +66,10 @@ pub mod leds { use libtock_leds as leds; pub type Leds = leds::Leds; } +pub mod servo { + use libtock_servo as servo; + pub type Servo = servo::Servo; +} pub mod low_level_debug { use libtock_low_level_debug as lldb; pub type LowLevelDebug = lldb::LowLevelDebug; diff --git a/unittest/src/fake/mod.rs b/unittest/src/fake/mod.rs index ba042ab0..0da24264 100644 --- a/unittest/src/fake/mod.rs +++ b/unittest/src/fake/mod.rs @@ -23,6 +23,7 @@ mod leds; mod low_level_debug; mod ninedof; mod proximity; +mod servo; mod sound_pressure; mod syscall_driver; mod syscalls; @@ -42,6 +43,7 @@ pub use leds::Leds; pub use low_level_debug::{LowLevelDebug, Message}; pub use ninedof::{NineDof, NineDofData}; pub use proximity::Proximity; +pub use servo::Servo; pub use sound_pressure::SoundPressure; pub use syscall_driver::SyscallDriver; pub use syscalls::Syscalls; diff --git a/unittest/src/fake/servo/mod.rs b/unittest/src/fake/servo/mod.rs new file mode 100644 index 00000000..aa9c1562 --- /dev/null +++ b/unittest/src/fake/servo/mod.rs @@ -0,0 +1,66 @@ +use std::cell::Cell; + +use crate::DriverInfo; +use libtock_platform::{CommandReturn, ErrorCode}; + +pub struct Servo { + servo: [Cell; NUM_SERVO], +} + +impl Servo { + pub fn new() -> std::rc::Rc> { + #[allow(clippy::declare_interior_mutable_const)] + const ANGLE: Cell = Cell::new(0); + std::rc::Rc::new(Servo { + servo: [ANGLE; NUM_SERVO], + }) + } +} + +impl crate::fake::SyscallDriver for Servo { + fn info(&self) -> DriverInfo { + DriverInfo::new(DRIVER_NUM) + } + + fn command(&self, command_num: u32, servo_index: u32, angle: u32) -> CommandReturn { + match command_num { + EXISTS => crate::command_return::success(), + SERVO_COUNT => crate::command_return::success_u32(NUM_SERVO as u32), + SET_ANGLE => { + if servo_index >= NUM_SERVO as u32 { + crate::command_return::failure(ErrorCode::NoDevice) + } else if angle <= 180 { + self.servo[servo_index as usize].set(angle as u16); + crate::command_return::success() + } else { + crate::command_return::failure(ErrorCode::Fail) + } + } + // Return the current angle. + GET_ANGLE => { + if servo_index >= NUM_SERVO as u32 { + crate::command_return::failure(ErrorCode::NoDevice) + } else { + let angle = self.servo[servo_index as usize].get(); + crate::command_return::success_u32(angle as u32) + } + } + _ => crate::command_return::failure(ErrorCode::NoSupport), + } + } +} + +#[cfg(test)] +mod tests; + +// ----------------------------------------------------------------------------- +// Implementation details below +// ----------------------------------------------------------------------------- + +const DRIVER_NUM: u32 = 0x90009; + +// Command numbers +const EXISTS: u32 = 0; +const SERVO_COUNT: u32 = 1; +const SET_ANGLE: u32 = 2; +const GET_ANGLE: u32 = 3; diff --git a/unittest/src/fake/servo/tests.rs b/unittest/src/fake/servo/tests.rs new file mode 100644 index 00000000..b05f6a00 --- /dev/null +++ b/unittest/src/fake/servo/tests.rs @@ -0,0 +1,52 @@ +use crate::fake; +use fake::servo::*; +use libtock_platform::CommandReturn; + +// Tests the command implementation. +#[test] +fn command() { + use fake::SyscallDriver; + let servo = Servo::<1>::new(); + let value = servo.command(EXISTS, 0, 0); + assert!(CommandReturn::is_success(&value)); + assert_eq!( + CommandReturn::get_success_u32(&servo.command(SERVO_COUNT, 0, 0)), + Some(1) + ); + assert!(CommandReturn::is_success(&servo.command(SET_ANGLE, 0, 90)),); + assert_eq!( + CommandReturn::get_success_u32(&servo.command(GET_ANGLE, 0, 0)), + Some(90) + ); +} + +#[test] +fn kernel_integration() { + use libtock_platform::Syscalls; + let kernel = fake::Kernel::new(); + let servo = Servo::<1>::new(); + kernel.add_driver(&servo); + let value = fake::Syscalls::command(DRIVER_NUM, EXISTS, 0, 0); + assert!(CommandReturn::is_success(&value)); + assert_eq!( + CommandReturn::get_success_u32(&fake::Syscalls::command(DRIVER_NUM, SERVO_COUNT, 0, 0)), + Some(1) + ); + assert_eq!( + fake::Syscalls::command(DRIVER_NUM, SET_ANGLE, 1, 90).get_failure(), + Some(ErrorCode::NoDevice) + ); + assert_eq!( + fake::Syscalls::command(DRIVER_NUM, SET_ANGLE, 0, 181).get_failure(), + Some(ErrorCode::Fail) + ); + assert!(fake::Syscalls::command(DRIVER_NUM, SET_ANGLE, 0, 90).is_success()); + assert_eq!( + fake::Syscalls::command(DRIVER_NUM, GET_ANGLE, 0, 0).get_success_u32(), + Some(90) + ); + assert_eq!( + fake::Syscalls::command(DRIVER_NUM, GET_ANGLE, 2, 0).get_failure(), + Some(ErrorCode::NoDevice) + ); +}