diff --git a/drivers/include/servo.h b/drivers/include/servo.h index 3ab000786f..a019a7e5eb 100644 --- a/drivers/include/servo.h +++ b/drivers/include/servo.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2014 Freie Universität Berlin + * Copyright (C) 2015 Eistec AB * * This file is subject to the terms and conditions of the GNU Lesser General * Public License v2.1. See the file LICENSE in the top level directory for more @@ -16,6 +17,7 @@ * @brief High-level driver for easy handling of servo motors * * @author Hauke Petersen + * @author Joakim Gebart */ #ifndef SERVO_H @@ -31,10 +33,12 @@ extern "C" { * @brief Descriptor struct for a servo */ typedef struct { - pwm_t device; /**< the PWM device driving the servo */ - int channel; /**< the channel the servo is connected to */ - unsigned int min; /**< minimum pulse width, in us */ - unsigned int max; /**< maximum pulse width, in us */ + pwm_t device; /**< the PWM device driving the servo */ + int channel; /**< the channel the servo is connected to */ + unsigned int min; /**< minimum pulse width, in us */ + unsigned int max; /**< maximum pulse width, in us */ + unsigned int scale_nom; /**< timing scale factor, to adjust for an inexact PWM frequency, nominator */ + unsigned int scale_den; /**< timing scale factor, to adjust for an inexact PWM frequency, denominator */ } servo_t; /** diff --git a/drivers/servo/servo.c b/drivers/servo/servo.c index 8d473cd43b..5fc9f456f0 100644 --- a/drivers/servo/servo.c +++ b/drivers/servo/servo.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2014 Freie Universität Berlin + * Copyright (C) 2015 Eistec AB * * This file is subject to the terms and conditions of the GNU Lesser General * Public License v2.1. See the file LICENSE in the top level directory for more @@ -14,33 +15,88 @@ * @brief Servo motor driver implementation * * @author Hauke Petersen + * @author Joakim Gebart * * @} */ #include "servo.h" #include "periph/pwm.h" +#include "timex.h" /* for SEC_IN_USEC */ + +#define ENABLE_DEBUG (0) +#include "debug.h" #define FREQUENCY (100U) -#define RESOLUTION (10000U) +#define RESOLUTION (SEC_IN_USEC / FREQUENCY) int servo_init(servo_t *dev, pwm_t pwm, int pwm_channel, unsigned int min, unsigned int max) { + int actual_frequency; + + actual_frequency = pwm_init(dev->device, PWM_LEFT, FREQUENCY, RESOLUTION); + + DEBUG("servo: requested %d hz, got %d hz\n", FREQUENCY, actual_frequency); + + if (actual_frequency < 0) { + /* PWM error */ + return -1; + } dev->device = pwm; dev->channel = pwm_channel; dev->min = min; dev->max = max; - return pwm_init(dev->device, PWM_LEFT, FREQUENCY, RESOLUTION); + /* Compute scaling fractional */ + /* + * The PWM pulse width can be written as: + * + * t = k / (f * r) + * + * where t is the pulse high time, k is the value set in the PWM peripheral, + * f is the frequency, and r is the resolution of the PWM module. + * + * define t0 as the desired pulse width: + * + * t0 = k0 / (f0 * r) + * + * where f0 is the requested frequency, k0 is the requested number of ticks. + * Introducing f1 as the closest achievable frequency and k1 as the set tick + * value yields: + * + * t1 = k1 / (f1 * r) + * + * setting t1 = t0 and substituting k1 = k0 * s yields: + * + * k0 / (f0 * r) = k0 * s / (f1 * r) + * + * solve for s: + * + * s = f1 / f0 + * + * where s is the optimal scale factor to translate from requested position + * to actual hardware ticks. + */ + dev->scale_nom = actual_frequency; + dev->scale_den = FREQUENCY; + + return 0; } int servo_set(servo_t *dev, unsigned int pos) { + unsigned int raw_value; if (pos > dev->max) { pos = dev->max; } else if (pos < dev->min) { pos = dev->min; } - return pwm_set(dev->device, dev->channel, pos); + + /* rescale value to match PWM peripheral configuration */ + raw_value = (pos * dev->scale_nom) / dev->scale_den; + + DEBUG("servo_set: pos %d -> raw %d\n", pos, raw_value); + + return pwm_set(dev->device, dev->channel, raw_value); } diff --git a/tests/driver_servo/Makefile b/tests/driver_servo/Makefile new file mode 100644 index 0000000000..043d5ce43f --- /dev/null +++ b/tests/driver_servo/Makefile @@ -0,0 +1,9 @@ +export APPLICATION = driver_servo +include ../Makefile.tests_common + +FEATURES_REQUIRED = periph_pwm + +USEMODULE += vtimer +USEMODULE += servo + +include $(RIOTBASE)/Makefile.include diff --git a/tests/driver_servo/README.md b/tests/driver_servo/README.md new file mode 100644 index 0000000000..e93c8517ef --- /dev/null +++ b/tests/driver_servo/README.md @@ -0,0 +1,16 @@ +Background +========== + +Test for the high level servo driver. + +Expected result +=============== + +A servo connected to `PWM_0` channel 0 should move back and forth inside the +angle -90 degrees to +90 degrees, approximately. + +Using a scope should show a varying pulse length between 1000 us to 2000 us +long. The requested frequency is 100 Hz, but due to hardware limitations it +might not be possible to achieve the selected frequency. The pulse width +should, however, remain the same, only the frequency of pulses (and hence the +duty cycle) should differ. diff --git a/tests/driver_servo/main.c b/tests/driver_servo/main.c new file mode 100644 index 0000000000..c6ba92cd8c --- /dev/null +++ b/tests/driver_servo/main.c @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2015 Eistec AB + * + * This file is subject to the terms and conditions of the GNU Lesser General + * Public License v2.1. See the file LICENSE in the top level directory for more + * details. + */ + +/** + * @ingroup tests + * @{ + * + * @file + * @brief Test for servo driver + * + * This test initializes the given servo device and moves it between + * 1.000 -- 2.000 ms, roughly -/+ 90 degrees from the middle position if the + * connected servo is a standard RC servo. + * + * @author Joakim Gebart + * + * @} + */ + +#include + +#include "cpu.h" +#include "board.h" +#include "vtimer.h" +#include "periph/pwm.h" +#include "servo.h" + +#define DEV PWM_0 +#define CHANNEL 0 + +#define SERVO_MIN (1000U) +#define SERVO_MAX (2000U) + +/* these are defined outside the limits of the servo_init min/max parameters above */ +/* we will test the clamping functionality of the servo_set function. */ +#define STEP_LOWER_BOUND (900U) +#define STEP_UPPER_BOUND (2100U) + +/* Step size that we move per WAIT us */ +#define STEP (10U) + +/* Sleep time between updates, no need to update the servo position more than + * once per cycle */ +#define WAIT (10000U) + +static servo_t servo; + +int main(void) +{ + int res; + int pos = (STEP_LOWER_BOUND + STEP_UPPER_BOUND) / 2; + int step = STEP; + + puts("\nRIOT RC servo test"); + puts("Connect an RC servo or scope to PWM_0 channel 0 to see anything"); + + res = servo_init(&servo, DEV, CHANNEL, SERVO_MIN, SERVO_MAX); + if (res < 0) { + puts("Errors while initializing servo"); + return -1; + } + puts("Servo initialized."); + + while (1) { + servo_set(&servo, pos); + + pos += step; + if (pos <= STEP_LOWER_BOUND || pos >= STEP_UPPER_BOUND) { + step = -step; + } + + vtimer_usleep(WAIT); + } + + return 0; +}