Close

Turnigy servos have a continuous rotation mode?

A project log for Browser-Controlled Tracked Robot

A tracked vehicle for playing around with. Controlled from laptop for ubiqutousness, educational value and ... uh... data.

OldCrowOldCrow 10/18/2014 at 14:570 Comments

Things I learned today:

-If nothing seems to work, check that you're toggling the right pin.

-Always double-check your timers' clock source and -rate.

-If the servo PWM signal is outside the 1-2 ms area, Turnigy servos will rotate continuously. Or at least the TGY-1800A model did. And seemed unharmed afterwards.

Dear diary,

I sent the last 4 hours scratching my head because one of the sercvo control pins on the MCU didn't seem to change states. After I finally realized that I could just add to the debugger's watch window the whole class, I realized that both pin number variables (Clk and Rst) had the same number. Traced that to a wrong assignment in the initializer.

After that was sorted out, I realized that the servo I'd connected to the board was rotating continuously at times. I'd set my test code to move all servos approximately 30 degrees every 2 seconds, for 12 seconds total, and then start over from the other extreme. Since this is servos we're talking about, with a PWM control signal, the pulse length was the obvious culprit. And so I checked and double-checked the stated values written to the timer's registers. But those were all in the correct range. Then I observed what the (presumed) pulse width was when it entered continuous rotation. It was around 1.25ms. This had me checking for an off-by-half mistakes in the clock source.

The AVR32 MCU I have here does not sport a prescaler as such. Instead, it has 5 clock inputs that are f32KHzRC, fFB/2 .. fFB/128. The fFB is the speed of the high-speed bus, the same as mu fCPU, which is 10MHz. I'd of course picked the fastest clock signal available. And promptly forgot, a week later when I wrote the code to use it, that it wasn't the 10Mhz of the fCPU, but fFB/2 = fCPU/2 = 10MHz/2 = 5MHz.

Facepalm time. I was producing servo pulses between 2.0ms and 4.0ms. Yes, that would produce unspecified behavior.

I still couldn't be bothered to put my code anywhere online, so here's the servo class:

#include "Servo4017.h"

#include <tc.h>
#include <gpio.h>

void Servo4017::init(uint16_t gpioPinRst, uint16_t gpioPinClk, volatile avr32_tc_t *timer, uint16_t timerChannel, uint8_t pinsInverted)
{
    // Set the initial position to be at 50% of range.
    // That is, start centered.
    for(uint16_t i = 0; i < numServoChannels; i++)
    {
        mServoPositions[i] = 32768;
    }
    
    mServoNum = 0;
    
    mClkPin = gpioPinClk;
    mRstPin = gpioPinRst;
    mPinsInverted = pinsInverted;
    
    pmTimer = timer;
    mTimerChannel = timerChannel;
    
    // Initialize to the end of the cycle with reset high and clock low.
    mCycleTimeLeft = 1000;
    mPhase = 3;
    
    gpio_enable_gpio_pin(mClkPin);
    gpio_enable_gpio_pin(mRstPin);
    
    // Take into account the possibly reversed state.
    if(mPinsInverted == 0)
    {
        gpio_configure_pin(mRstPin, (GPIO_DIR_OUTPUT | GPIO_INIT_HIGH));
        gpio_configure_pin(mClkPin, (GPIO_DIR_OUTPUT | GPIO_INIT_LOW));
    }
    else
    {
        gpio_configure_pin(mRstPin, (GPIO_DIR_OUTPUT | GPIO_INIT_LOW));
        gpio_configure_pin(mClkPin, (GPIO_DIR_OUTPUT | GPIO_INIT_HIGH));
    }
    
    gpio_local_enable_pin_output_driver(mRstPin);
    gpio_local_enable_pin_output_driver(mClkPin);
    
    
    // TODO: Hardware init
    tc_waveform_opt_t tcOpts;
    tcOpts.channel = mTimerChannel;
    tcOpts.bswtrg = TC_EVT_EFFECT_NOOP;
    tcOpts.beevt = TC_EVT_EFFECT_NOOP;
    tcOpts.bcpc = TC_EVT_EFFECT_NOOP;
    tcOpts.bcpb = TC_EVT_EFFECT_NOOP;
    tcOpts.aswtrg = TC_EVT_EFFECT_NOOP;
    tcOpts.aeevt = TC_EVT_EFFECT_NOOP;
    tcOpts.acpc = TC_EVT_EFFECT_NOOP;
    tcOpts.acpa = TC_EVT_EFFECT_NOOP;
    tcOpts.wavsel = TC_WAVEFORM_SEL_UP_MODE_RC_TRIGGER;
    tcOpts.enetrg = false;
    tcOpts.eevt = TC_EXT_EVENT_SEL_XC0_OUTPUT;
    tcOpts.eevtedg = TC_SEL_NO_EDGE;
    tcOpts.cpcdis = false;  // Don't disable on RC compare
    tcOpts.cpcstop = false; // Don't stop on RC compare
    tcOpts.burst = TC_BURST_NOT_GATED;
    tcOpts.clki = TC_CLOCK_RISING_EDGE;
    tcOpts.tcclks = TC_CLOCK_SOURCE_TC2; // = PBAclock / 2

    // Init timer 1 for servo and second-counter duty.
    int resp = tc_init_waveform (pmTimer, &tcOpts);
    
    tc_interrupt_t tcInts;
    tcInts.covfs = false;
    tcInts.cpas = false;
    tcInts.cpbs = false;
    tcInts.cpcs = true;
    tcInts.etrgs = false;
    tcInts.ldras = false;
    tcInts.ldrbs = false;
    tcInts.lovrs = false;
    
    tc_configure_interrupts(pmTimer, mTimerChannel, &tcInts);
    
    // Start the timer. 1.0ms is given for the assertion of reset to take effect.
    tc_write_rc(pmTimer, mTimerChannel, 1000);
    tc_start(pmTimer, mTimerChannel);
}

void Servo4017::setPos(uint16_t servo, uint16_t pos)
{
    if(servo >= numServoChannels)
    {
        return;
    }
    
    // Fit the desired servo position to the timing range.
    uint32_t tmp = pos;
    tmp *= 10000UL;
    tmp /= 65535UL;
    
    // Add the minimum interrupt protection part.
    // (It's deducted elsewhere, so don't worry about that.)
    tmp += 2500;
    
    mServoPositions[servo] = tmp;
}

void Servo4017::interruptCallback(void)
{
    tc_read_sr(pmTimer, mTimerChannel);
    
    // Operate as non-volatile in the outermost if-structure
    uint16_t tmp = mPhase;
    
    // This was the 0.75ms high phase for Clk pin
    if(tmp == 0)
    {
        // Set Clk low for Phase 1.
        if(mPinsInverted == 0)
        {
            gpio_local_clr_gpio_pin(mClkPin);
        }
        else
        {
            gpio_local_set_gpio_pin(mClkPin);
        }
        
        // Set the timer and calculate the remaining time
        tc_write_rc(pmTimer, mTimerChannel, mServoPositions[mServoNum] / 2);
        mCycleTimeLeft -= mServoPositions[mServoNum];
        mServoNum++;
        
        mPhase = 1;
    }
    // This was the 0.25ms + (servo position) low time for the Clk pin
    else if(tmp == 1)
    {
        // Not the last servo yet.
        if(mServoNum < numServoChannels)
        {
            // Set Clk high and calculate and set the time
            if(mPinsInverted == 0)
            {
                gpio_local_set_gpio_pin(mClkPin);
            }
            else
            {
                gpio_local_clr_gpio_pin(mClkPin);
            }
            
            mPhase = 0;
            
            tc_write_rc(pmTimer, mTimerChannel, 7500 / 2);
        }
        // Was last servo
        else
        {
            mPhase = 3;
            
            // Set Rst high
            if(mPinsInverted == 0)
            {
                gpio_local_set_gpio_pin(mRstPin);
            }
            else
            {
                gpio_local_clr_gpio_pin(mRstPin);
            }
            
            // The remaining cycle time is 4-12ms. Half that is 2 to 6 ms.
            //  The maximum 60,000 ticks just fits to the counter.
            tc_write_rc(pmTimer, mTimerChannel, mCycleTimeLeft/4);
        }
        
    }
    else if(tmp == 3)
    {
        mPhase = 4;
        
        // Set Rst low
        if(mPinsInverted == 0)
        {
            gpio_local_clr_gpio_pin(mRstPin);
        }
        else
        {
            gpio_local_set_gpio_pin(mRstPin);
        }
        
        // The remaining cycle time, 2 to 6 ms.
        tc_write_rc(pmTimer, mTimerChannel, mCycleTimeLeft/4);
    }
    else // tmp == 4, hopefully...
    {
        // Phase 4 is always followed by phase 0.
        mPhase = 0;
        
        // Full cycle time is 20ms = 200,000 ticks.
        mCycleTimeLeft = 200000;
        
        // Start from the first servo again.
        mServoNum = 0;
        
        if(mPinsInverted == 0)
        {
            gpio_local_set_gpio_pin(mClkPin);
        }
        else
        {
            gpio_local_clr_gpio_pin(mClkPin);
        }
        
        // Will clock out the first channel's 0.75ms again
        tc_write_rc(pmTimer, mTimerChannel, 7500 / 2);
        
    }
}

And header:

  

#ifndef __SERVO4017_H__
#define __SERVO4017_H__

#include <stdint.h>
#include <tc.h>



class Servo4017
{
public:
    Servo4017(){}
    
    // Warning!: gpio_local_init() MUST BE DONE BEFORE calling this function.
    // Init must be called before any other action is taken.
    // It will initialize the dependencies. Namely the timer interface.
    void init(uint16_t gpioPinRst, uint16_t gpioPinClk, volatile avr32_tc_t *timer, uint16_t timerChannel, uint8_t pinsInverted);
        
    // uint16_t represents full scale of motion for servo,
    // or that of 1.0ms..2.0ms output pulse length for servo driver.
    // Halfway-point is therefore max(uint16_t) / 2
    void setPos(uint16_t servo, uint16_t pos);
    
    void interruptCallback(void);    
private:
    Servo4017( const Servo4017 &c );
    Servo4017& operator=( const Servo4017 &c );
    
    static const uint16_t numServoChannels = 8;
    
    // Timer bits
    volatile avr32_tc_t *pmTimer;
    volatile uint16_t mTimerChannel;
    
    // Pins to use for output
    volatile uint16_t mClkPin;
    volatile uint16_t mRstPin;
    
    
    // Indicates the servo channel currently being counted out
    volatile uint16_t mServoNum;
    
    // The positions of the servos, in timer ticks, plus 0.25ms added for minimum interrupt execution time
    uint16_t mServoPositions[numServoChannels];
    
    // In phase 0, the Clk pin is set high for 0.75ms. For phase 1, Clk is set low for the duration of 0.25ms + (1.0ms * servo position).
    // Phase 3 happens when the last servo has been through phase 1, and in it the Rst pin is set high for 1.0ms.
    // In phase 4 the Rst pin is set low for 20.0ms - (sum of all servos total signalling time). The 20.0ms is
    // in theory not necessary to be timed accurately. However, some rare servos and other effectors may be sensitive, so time it out.
    // Phase 4 will be divided to parts that can be counted with the timer channel's current 6.5ms maximum.
    volatile uint8_t mPhase;
    
    // Keeps count of how much longer the current timing cycle must last. All servo signalling times will be deducted
    // and the remainder waited out at the end of each cycle.
    volatile uint32_t mCycleTimeLeft;
    
    // Are the output pins inverted in functionality?
    //  Note: Documentative comments do not take into account 
    //  output polarity unless specifically stating otherwise.
    volatile uint8_t mPinsInverted;
};

#endif //__SERVO4017_H__

Discussions