diff --git a/examples/servo/Makefile b/examples/servo/Makefile new file mode 100644 index 000000000..cfe30ad8e --- /dev/null +++ b/examples/servo/Makefile @@ -0,0 +1,11 @@ +# Makefile for user application + +# Specify this directory relative to the current application. +TOCK_USERLAND_BASE_DIR = ../.. + +# Which files to compile. +C_SRCS := $(wildcard *.c) + +# Include userland master makefile. Contains rules and flags for actually +# building the application. +include $(TOCK_USERLAND_BASE_DIR)/AppMakefile.mk diff --git a/examples/servo/main.c b/examples/servo/main.c new file mode 100644 index 000000000..80d3ab2ce --- /dev/null +++ b/examples/servo/main.c @@ -0,0 +1,45 @@ +#include "../../libtock/interface/syscalls/servo_syscalls.h" +#include +#include +#include +#include + +int main(void) { + // Checks if the driver exists and, if not, returns -1. + if (!libtock_servo_exists()) { + printf("There is no available servo\n"); + return -1; + } + returncode_t result = RETURNCODE_EOFF; + uint16_t servo_count = 0; + libtock_servo_count(&servo_count); + printf("The number of available servomotors is: %d", servo_count); + uint16_t angle = 0; + uint16_t index = 0; // the first index available. + + if (libtock_servo_read_angle(index, &angle) == RETURNCODE_ENODEVICE) { + printf("\n The index number is bigger than the available servomotors\n"); + return -1; + } + + // Changes the angle of the servomotor from 0 to 180 degrees (waiting 0.1 ms between every change). + for (int i = 0; i <= 180; i++) { + // `result` stores the returned code received from the driver. + result = libtock_servo_set_angle(index, i); + if (result == RETURNCODE_SUCCESS) { + libtocksync_alarm_delay_ms(100); + // Verifies if the function successfully returned the current angle. + if (libtock_servo_read_angle(index, &angle) == RETURNCODE_SUCCESS) { + printf("\nThe current angle is: %d", angle); + } + } else if (result == RETURNCODE_EINVAL) { + printf("\nThe angle you provided exceeds 360 degrees\n"); + return -1; + } else if (result == RETURNCODE_FAIL) { + printf("\nAngle %d exceeds the servo's limit angle.\n", i); + } + } + if (libtock_servo_read_angle(index, &angle) != RETURNCODE_SUCCESS) { + printf("\n This servo cannot return its angle.\n"); + } +} diff --git a/libtock/interface/syscalls/servo_syscalls.c b/libtock/interface/syscalls/servo_syscalls.c new file mode 100644 index 000000000..cec370657 --- /dev/null +++ b/libtock/interface/syscalls/servo_syscalls.c @@ -0,0 +1,32 @@ +#include "servo_syscalls.h" +#include + +bool libtock_servo_exists(void) { + return driver_exists(DRIVER_NUM_SERVO); +} + +returncode_t libtock_servo_count(uint16_t* servo_count) { + uint32_t value = 0; + syscall_return_t r = command(DRIVER_NUM_SERVO, 1, 0, 0); + // Converts the returned value stored in `r` + // and assigns it to `ret`. + returncode_t ret = tock_command_return_u32_to_returncode(r, &value); + *servo_count = (uint16_t)value; + return ret; + +} + +returncode_t libtock_servo_set_angle(uint16_t index, uint16_t angle) { + syscall_return_t cval = command(DRIVER_NUM_SERVO, 2, index, angle); + return tock_command_return_novalue_to_returncode(cval); +} + +returncode_t libtock_servo_read_angle(uint16_t index, uint16_t* angle) { + uint32_t value = 0; + syscall_return_t r = command(DRIVER_NUM_SERVO, 3, index, 0); + // Converts the value returned by the servo, stored in `r` + // and assigns it to `ret`. + returncode_t ret = tock_command_return_u32_to_returncode(r, &value); + *angle = (uint16_t)value; + return ret; +} diff --git a/libtock/interface/syscalls/servo_syscalls.h b/libtock/interface/syscalls/servo_syscalls.h new file mode 100644 index 000000000..1099265fd --- /dev/null +++ b/libtock/interface/syscalls/servo_syscalls.h @@ -0,0 +1,22 @@ +#pragma once + +#include "../../tock.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define DRIVER_NUM_SERVO 0x90009 + +// Check if the servo system call driver is available on this board. +bool libtock_servo_exists(void); +// Returns the number of available servomotors. +returncode_t libtock_servo_count(uint16_t* servo_count); +// Change the angle. +returncode_t libtock_servo_set_angle(uint16_t index, uint16_t angle); +// Requests the current angle from the servo. +returncode_t libtock_servo_read_angle(uint16_t index, uint16_t* angle); + +#ifdef __cplusplus +} +#endif