Servo speed

[Mikael Gustafsson]

First of all it's great to see that you're back at developing this library!
Would it be possible to add servo slower functionality to ServoOut?
You're doing a great job and you're a huge help to the community.

[dvdouden]

Thanks for your interest in the library!

I've added ticket [#50] for this for the next release. It shouldn't be that difficult to implement, something similar is already available in the Retracts and DAIPin classes.
I probably won't add this to ServoOut directly since that would clutter the class. I think it should be part of the Channel class so it'll sit right next to channel reverse and endpoints, which seems to be where high end transmitters also put this feature.

If you need something that can operate directly on milliseconds (in case you want to tie PPMIn to ServoOut directly) then I can arrange that as well.

[Mikael Gustafsson]

Hi!

You guessed right, between PPMIn and ServoOut would be perfect!

[dvdouden]

Here's a test implementation of the ServoSpeed class and a modified servo out example. You can specify servo speed in deciseconds (0.1 sec) to indicate how much time it should take the servo to travel from one extreme to the other, range is 0 (instant) to 100 (10 seconds). It operates in the microseconds domain, so you can supply a value in us (from PPMIn for example) and feed the output to ServoOut if you want to.
The same will be added to the Channel class in release 0.4, but that will operate on normalized values instead of microseconds.
You'll need one ServoSpeed object for each servo, since each object keeps track of time and progress individually. See the attached zip file for sources and example.
I haven't thoroughly tested it yet, so if you find any bugs, let me know :)

ServoSpeed.h:

#ifndef INC_RC_SERVOSPEED_H
#define INC_RC_SERVOSPEED_H

/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** ServoSpeed.h
** Custom class to implement servo speed in the microsecond domain
** For direct use in combination with PPMOut and ServoOut
**
** Author: Daniel van den Ouden
** Project: ArduinoRCLib
** Website: http://sourceforge.net/p/arduinorclib/
** -------------------------------------------------------------------------*/

#include <inttypes.h>

namespace rc
{

/*! 
*  \brief     Class to encapsulate Servo Speed functionality.
*  \details   This class implements servo speed in the microsecond domain.
*  \author    Daniel van den Ouden
*  \date      Nov-2012
*  \copyright Public Domain.
*/
class ServoSpeed
{
public:
    /*! \brief Constructs a ServoSpeed object
        \param p_speed Speed in deciseconds, 0 = instant, range [0 - 100].*/
    ServoSpeed(uint8_t p_speed = 0);

    /*! \brief Sets the servo speed; the time it takes to travel between servo extremes.
        \param p_speed The time it takes to travel between both extremes in deciseconds (0.1 sec),
                    range [0 - 100] (instant - 10 sec).
        \note Default is 0 (instant).
        \note To convert degrees per second to speed, use deg per sec = total throw in degrees / (speed / 10).
            The other way around: speed = (throw in deg / deg per sec) * 10.*/
    void setSpeed(uint8_t p_speed);

    /*! \brief Gets the servo speed.
        \return The time it takes to travel between two extremes in deciseconds, range [0 - 100].*/
    uint8_t getSpeed() const;

    /*! \brief Applies servo speed.
        \param p_target Input value in microseconds, range center +- travel (see util.h).
        \return Output value with speed applied in microseconds, range center +- travel (see util.h)*/
    uint16_t apply(uint16_t p_target);

private:
    uint8_t  m_speed;    //!< Servo speed
    uint8_t  m_time;     //!< Last time update was called
    uint16_t m_last;     //!< Value of last update
};

} // namespace end

#endif // INC_RC_SERVOSPEED_H

ServoSpeed.cpp

/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** ServoSpeed.cpp
** Channel functionality
**
** Author: Daniel van den Ouden
** Project: ArduinoRCLib
** Website: http://sourceforge.net/p/arduinorclib/
** -------------------------------------------------------------------------*/

#include <Arduino.h>

#include <ServoSpeed.h>
#include <util.h>


namespace rc
{

// Public functions

ServoSpeed::ServoSpeed(uint8_t p_speed)
:
m_speed(0),
m_time(0),
m_last(0)
{
    setSpeed(p_speed);
}


void ServoSpeed::setSpeed(uint8_t p_speed)
{
    m_speed = p_speed;
    m_time  = static_cast<uint8_t>(millis());
}


uint8_t ServoSpeed::getSpeed() const
{
    return m_speed;
}


uint16_t ServoSpeed::apply(uint16_t p_target)
{
    // handle first call
    if (m_last == 0)
    {
        m_last = p_target;
    }

    if (m_speed == 0 || p_target == m_last)
    {
        return p_target;
    }
    // we might as well use 8 bit for times, just make sure to update at least 4 times per second
    // (1000 ms / 256 = 3.9 overflows per second)
    uint8_t now = static_cast<uint8_t>(millis());
    uint8_t delta = now - m_time;
    m_time = now;

    // the total amount traveled in the past delta time is:
    // (full throw / time which it takes to travel in millis) * delta time in millis
    // or
    // (2 * rc::getTravel() / (m_speed * 100)) * delta
    // that'll result in 0 for any speed above 10 or so, so let's not do that
    // but the following is the same:
    // (delta * 2 * rc::getTravel()) / (m_speed * 100)
    // but this will overflow for delta >= about 64, so we're going to use 32 bit longs here
    uint16_t travel = static_cast<uint16_t>((delta * static_cast<uint32_t>(rc::getTravel()) * 2) / (m_speed * 100));

    // now that we know how far we can travel in this update, let's see in which direction we'll need to go
    if (m_last > p_target)
    {
        if (static_cast<uint32_t>(m_last - p_target) < travel)
        {
            m_last = p_target;
        }
        else
        {
            m_last = m_last - static_cast<int16_t>(travel);
        }
    }
    else
    {
        if (static_cast<uint32_t>(p_target - m_last) < travel)
        {
            m_last = p_target;
        }
        else
        {
            m_last = m_last + static_cast<int16_t>(travel);
        }
    }
    return m_last;
}


// namespace end
}

Modified servoout_example.pde

/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** servoout_example.pde
** Demonstrate Servo Signal Output functionality
**
** Author: Daniel van den Ouden
** Project: ArduinoRCLib
** Website: http://sourceforge.net/p/arduinorclib/
** -------------------------------------------------------------------------*/

#include <ServoOut.h>
#include <ServoSpeed.h>
#include <Timer1.h>

#define SERVOS 4

uint8_t  g_pinsIn[SERVOS] = {A0, A1, A2, A3}; // Input pins
uint8_t  g_pinsOut[SERVOS] = {2, 3, 4, 5};    // Output pins
uint16_t g_input[SERVOS];                     // Input buffer for servoOut, microseconds
uint8_t  g_work[SERVOOUT_WORK_SIZE(SERVOS)];  // we need to have a work buffer for the ServoOut class

// ServoOut requires three buffers:
//     Pins buffer containing output pins
//     Input buffer containing input values in microseconds
//     Work buffer of SERVOOUT_WORK_SIZE(SERVOS) elements for internal calculations
// This setup removes any technical limit on the number of servos you want,
// and makes sure the library doesn't use more memory than it really needs,
// since the client code supplies the buffers.
rc::ServoOut g_ServoOut(g_pinsOut, g_input, g_work, SERVOS);

// define a servo speed object for each servo
rc::ServoSpeed g_ServoSpeed[SERVOS];

void setup()
{
    // Initialize timer1, this is required for all features that use Timer1
    // (PPMIn/PPMOut/ServoIn/ServoOut)
    rc::Timer1::init();

    for (uint8_t i = 0;  i < SERVOS; ++i)
    {
        // set up input pins
        pinMode(g_pinsIn[i], INPUT);

        // set up output pins
        pinMode(g_pinsOut[i], OUTPUT);

        // put them low
        digitalWrite(g_pinsOut[i], LOW);

        // fill input buffer, convert raw values to normalized ones
        g_input[i] = map(analogRead(g_pinsIn[i]), 0, 1024, 1000, 2000);

        g_ServoSpeed[i].setSpeed(i * 10); // instant for servo 0, 1 second for 1, 2 seconds for 2 etc
    }
    g_ServoOut.start();
}

void loop()
{
    // update the input buffer
    for (uint8_t i = 0;  i < SERVOS; ++i)
    {
        // fill input buffer, convert raw values to normalized ones
        g_input[i] = map(analogRead(g_pinsIn[i]), 0, 1024, 1000, 2000);
        g_input[i] = g_ServoSpeed[i].apply(g_input[i]); // apply servo speed
    }

    // tell ServoOut there are new values available in the input buffer
    g_ServoOut.update();

    // Just a quick note here to keep in mind
    // If you're having problems getting your servos to work properly, i.e. servos are not responding well
    // or seem to miss their input signal every couple of seconds, then the servos may be disturbing their control signal.
    // This happens especially when you've connected the servos directly to the Arduino and when they are of poor quality.
    // The motor of the servo will cause drops in the voltage of the Arduino which will mess with the generated signal,
    // I've had this happen to me while using some cheap Esky micro servos. If you hook the signal up to a scope you
    // can see voltage drops in the signal every few hundred microseconds while the servo's motor is working.
    // To fix this you need a couple of decoupling capacitors, google for more details ;)
}

[dvdouden]

Sourceforge is acting up, here's the attachment

[dvdouden]

I've just tested the code (both the test code posted here and the final code in Channel) and it works fine, really cool feature. Thanks for the suggestion!

[Mikael Gustafsson]

No need to thank me, but Thank you! I'll be testing this during the weekend, but this sounds absolutely amazing.

[Anonymous]

I finally had the time to test this and it works great! Tested the whole "loop". PPM data read with PPMIn and outputted to servo with ServoOut via ServoSpeed.