Userland app for servomotor

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",

apis/interface/servo/Cargo.toml

@@ -0,0 +1,16 @@
+cargo-features = ["edition2024"]
+[package]
+name = "libtock_servo"
+version = "0.1.0"
+authors = ["Tock Project Developers <[email protected]>"]
+license = "Apache-2.0 OR MIT"
+edition = "2024"
+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" }

apis/interface/servo/src/lib.rs

@@ -0,0 +1,37 @@
+#![no_std]
+
+use libtock_platform::{ErrorCode, Syscalls};
+
+pub struct Servo<S: Syscalls>(S);
+
+impl<S: Syscalls> Servo<S> {
+    pub fn servo_exists() -> Result<(), ErrorCode> {
+        let val = S::command(DRIVER_NUM, SERVO_EXISTS, 0, 0).is_success();
+        if val == true {
+            Ok(())
+        } else {
+            //println!("the driver could not be found");
+            Err(ErrorCode::Fail)
+        }
+    }
+    pub fn set_angle(index: u32, angle: u32) -> Result<(), ErrorCode> {
+        S::command(DRIVER_NUM, SET_ANGLE, index, angle).to_result()
+    }
+    pub fn get_angle(index: u32) -> Result<u32, ErrorCode> {
+        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 SERVO_EXISTS: u32 = 0;
+const SET_ANGLE: u32 = 1;
+const GET_ANGLE: u32 = 2;

apis/interface/servo/src/tests.rs

@@ -0,0 +1,32 @@
+use libtock_platform::ErrorCode;
+use libtock_unittest::fake;
+
+type Servo = super::Servo<fake::Syscalls>;
+
+#[test]
+fn no_driver() {
+    let _kernel = fake::Kernel::new();
+    assert_eq!(Servo::servo_exists(), Err(ErrorCode::Fail))
+}
+#[test]
+fn servo_exists() {
+    let kernel = fake::Kernel::new();
+    let driver = fake::Servo::<2>::new();
+    kernel.add_driver(&driver);
+    assert_eq!(Servo::servo_exists(), Ok(()));
+}
+#[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));
+}

examples/servo.rs

@@ -0,0 +1,51 @@
+#![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! {0x200}
+
+fn main() {
+    //Checks if the driver exists.
+    if Err(ErrorCode::Fail) == Servo::servo_exists() {
+        writeln!(Console::writer(), "The driver could not be found").unwrap();
+        return;
+    }
+
+    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();
+    }
+}

src/lib.rs

@@ -66,6 +66,10 @@ pub mod leds {
     use libtock_leds as leds;
     pub type Leds = leds::Leds<super::runtime::TockSyscalls>;
 }
+pub mod servo {
+    use libtock_servo as servo;
+    pub type Servo = servo::Servo<super::runtime::TockSyscalls>;
+}
 pub mod low_level_debug {
     use libtock_low_level_debug as lldb;
     pub type LowLevelDebug = lldb::LowLevelDebug<super::runtime::TockSyscalls>;

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;

unittest/src/fake/servo/mod.rs

@@ -0,0 +1,64 @@
+use std::cell::Cell;
+
+use crate::DriverInfo;
+use libtock_platform::{CommandReturn, ErrorCode};
+
+pub struct Servo<const NUM_SERVO: usize> {
+    servo: [Cell<u16>; NUM_SERVO],
+}
+
+impl<const NUM_SERVO: usize> Servo<NUM_SERVO> {
+    pub fn new() -> std::rc::Rc<Servo<NUM_SERVO>> {
+        #[allow(clippy::declare_interior_mutable_const)]
+        const ANGLE: Cell<u16> = Cell::new(0);
+        std::rc::Rc::new(Servo {
+            servo: [ANGLE; NUM_SERVO],
+        })
+    }
+}
+
+impl<const NUM_SERVO: usize> crate::fake::SyscallDriver for Servo<NUM_SERVO> {
+    fn info(&self) -> DriverInfo {
+        DriverInfo::new(DRIVER_NUM)
+    }
+
+    fn command(&self, command_num: u32, servo_index: u32, angle: u32) -> CommandReturn {
+        match command_num {
+            0 => crate::command_return::success(),
+            1 => {
+                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.
+            2 => {
+                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 SERVO_EXISTS: u32 = 0;
+const SET_ANGLE: u32 = 1;
+const GET_ANGLE: u32 = 2;

unittest/src/fake/servo/tests.rs

@@ -0,0 +1,47 @@
+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(SERVO_EXISTS, 0, 0);
+    assert_eq!(CommandReturn::is_success(&value), true);
+    assert_eq!(
+        CommandReturn::is_success(&servo.command(SET_ANGLE, 0, 90)),
+        true
+    );
+    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, SERVO_EXISTS, 0, 0);
+    assert_eq!(CommandReturn::is_success(&value), true);
+    assert_eq!(
+        fake::Syscalls::command(DRIVER_NUM, SET_ANGLE, 2, 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)
+    );
+}