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
Become a Hackaday.io Member
Create an account to leave a comment. Already have an account? Log In.