function that returns the number of servomotors available

tock · Sep 25, 2024 · a208bcc · a208bcc
1 parent 8e26dad
commit a208bcc
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 8 deletions.
diff --git a/apis/interface/servo/src/lib.rs b/apis/interface/servo/src/lib.rs
@@ -14,6 +14,10 @@ impl<S: Syscalls> Servo<S> {
  Err(ErrorCode::Fail)
  }
  }
+ /// Returns the number of the servomotors available.
+ pub fn servo_number() -> Result<u32, ErrorCode> {
+ S::command(DRIVER_NUM, SERVO_NUMBER, 0, 0).to_result()
+ }
 
  /// Changes the angle of the servo.
  /// Return values:
@@ -55,5 +59,6 @@ const DRIVER_NUM: u32 = 0x90009;
 
 // Command IDs
 const EXISTS: u32 = 0;
-const SET_ANGLE: u32 = 1;
-const GET_ANGLE: u32 = 2;
+const SERVO_NUMBER: 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
@@ -16,6 +16,13 @@ fn exists() {
  assert_eq!(Servo::exists(), Ok(()));
 }
 #[test]
+fn servo_number() {
+ let kernel = fake::Kernel::new();
+ let driver = fake::Servo::<2>::new();
+ kernel.add_driver(&driver);
+ assert_eq!(Servo::servo_number(), Ok(2));
+}
+#[test]
 fn set_angle() {
  let kernel = fake::Kernel::new();
  let driver = fake::Servo::<2>::new();

diff --git a/examples/servo.rs b/examples/servo.rs
@@ -8,14 +8,22 @@ use libtock::servo::Servo;
 use libtock_platform::ErrorCode;
 
 set_main! {main}
-stack_size! {0x200}
+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 servo_number = Servo::servo_number().unwrap();
+
+ writeln!(
+ Console::writer(),
+ "The number of available servomotors is {:?}",
+ servo_number
+ )
+ .unwrap();
 
  let index: u32 = 0; // the first index available.
 

diff --git a/unittest/src/fake/servo/mod.rs b/unittest/src/fake/servo/mod.rs
@@ -25,7 +25,8 @@ impl<const NUM_SERVO: usize> crate::fake::SyscallDriver for Servo<NUM_SERVO> {
  fn command(&self, command_num: u32, servo_index: u32, angle: u32) -> CommandReturn {
  match command_num {
  0 => crate::command_return::success(),
- 1 => {
+ 1 => crate::command_return::success_u32(NUM_SERVO as u32),
+ 2 => {
  if servo_index >= NUM_SERVO as u32 {
  crate::command_return::failure(ErrorCode::NoDevice)
  } else if angle <= 180 {
@@ -36,7 +37,7 @@ impl<const NUM_SERVO: usize> crate::fake::SyscallDriver for Servo<NUM_SERVO> {
  }
  }
  // Return the current angle.
- 2 => {
+ 3 => {
  if servo_index >= NUM_SERVO as u32 {
  crate::command_return::failure(ErrorCode::NoDevice)
  } else {
@@ -60,5 +61,6 @@ const DRIVER_NUM: u32 = 0x90009;
 
 // Command numbers
 const EXISTS: u32 = 0;
-const SET_ANGLE: u32 = 1;
-const GET_ANGLE: u32 = 2;
+const SERVO_NUMBER: 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
@@ -9,6 +9,10 @@ fn command() {
  let servo = Servo::<1>::new();
  let value = servo.command(EXISTS, 0, 0);
  assert_eq!(CommandReturn::is_success(&value), true);
+ assert_eq!(
+ CommandReturn::get_success_u32(&servo.command(SERVO_NUMBER, 0, 0)),
+ Some(1)
+ );
  assert_eq!(
  CommandReturn::is_success(&servo.command(SET_ANGLE, 0, 90)),
  true
@@ -28,7 +32,11 @@ fn kernel_integration() {
  let value = fake::Syscalls::command(DRIVER_NUM, EXISTS, 0, 0);
  assert_eq!(CommandReturn::is_success(&value), true);
  assert_eq!(
- fake::Syscalls::command(DRIVER_NUM, SET_ANGLE, 2, 90).get_failure(),
+ CommandReturn::get_success_u32(&fake::Syscalls::command(DRIVER_NUM, SERVO_NUMBER, 0, 0)),
+ Some(1)
+ );
+ assert_eq!(
+ fake::Syscalls::command(DRIVER_NUM, SET_ANGLE, 1, 90).get_failure(),
  Some(ErrorCode::NoDevice)
  );
  assert_eq!(