Merge pull request #3106 from gebart/pr/servo-updates

drivers/servo: Add test program, handle inexact frequencies

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 <hauke.petersen@fu-berlin.de>
+ * @author      Joakim Gebart <joakim.gebart@eistec.se>
  */
 
 #ifndef SERVO_H
@@ -35,6 +37,8 @@ typedef struct {
     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;
 
 /**

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 <hauke.petersen@fu-berlin.de>
+ * @author      Joakim Gebart <joakim.gebart@eistec.se>
  *
  * @}
  */
 
 #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);
 }

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

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.

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 <joakim.gebart@eistec.se>
+ *
+ * @}
+ */
+
+#include <stdio.h>
+
+#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;
+}