-
Notifications
You must be signed in to change notification settings - Fork 118
Userland app for servomotor #558
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Closed
Closed
Changes from all commits
Commits
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,15 @@ | ||
[package] | ||
name = "libtock_servo" | ||
version = "0.1.0" | ||
authors = ["Tock Project Developers <[email protected]>"] | ||
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" } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,70 @@ | ||
#![no_std] | ||
|
||
use libtock_platform::{ErrorCode, Syscalls}; | ||
|
||
pub struct Servo<S: Syscalls>(S); | ||
|
||
impl<S: Syscalls> Servo<S> { | ||
/// 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<u32, ErrorCode> { | ||
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<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 EXISTS: u32 = 0; | ||
const SERVO_COUNT: u32 = 1; | ||
const SET_ANGLE: u32 = 2; | ||
const GET_ANGLE: u32 = 3; |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,39 @@ | ||
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::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)); | ||
} |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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(); | ||
} | ||
} |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,66 @@ | ||
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 { | ||
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; |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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) | ||
); | ||
} |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.