Arduino UNO基于Timer2的舵机驱动库(精度比官方的高)
Arduino UNO基于Timer2的舵机驱动库(精度比官方的高)原博客格式更友好:http://www.straka.cn/blog/more-accurate-arduino-uno-timer2-servo-driver-library-than-official-one/
事情是这样的,本来有个小车,想改装下,已经有的驱动板上引脚已经限定了用途和功能,最终的结果就是,如果我想用红外发射库,就无法同时使用舵机对小车进行调速,因为他们都用了 Timer1定时器,何况我还要同时在3、5引脚使用pwm。无奈之下只能寻找别的办法。
先在网上了解了下ARDUINO的定时器、中断、PWM、舵机控制,红外收发等相关知识。尤其是仔细阅读了AVR atmega328p,也就是ARDUINO UNO的芯片手册的定时器部分,其中有两点:
AT mega328p的定时器有3个,对应Arduino UNO板子,
Timer0 对应 5、6引脚pwm, 8bit
Timer1 对应 9、10引脚pwm, 16bit
Timer2 对应 11、3引脚pwm, 8bit
舵机的pwm频率为50Hz / 20ms, 但是控制舵机需要的占空比比较小,为20ms中的5 ~ 2.5ms。
由于红外接收发射库可以选择timer2或者timer1作为38khz载波发生定时器,(之所以不用timer0,因为timer0是用于delay这种延时函数的,所以如果被征用了会导致延时异常)。考虑到38khz频率相对较高,如果我在中断中做些其他的处理,容易导致其载波频率不可靠,所以还是选择改动舵机库,毕竟舵机的频率低,对时间准确性要求相对低,而我对pwm的准确性要求就更低了,所以考虑用定时器2同时作为pwm和舵机控制的定时器。
参考了下官方的库只支持定时器1,无奈,只能自己写一个定时器2的库了。找了下网上,并没有很多相关的文章,有一篇倒是给出了源码,我试了下还是可以用的【参考资料1】,但是想着自己之前对avr单片机的定时器这块也不是特别了解,索性边学边自己也写一个吧。网上的源码并没有太多的解释,看了一遍后,发现和库的源码思路不太一致,主要在于跳变沿的中断条件设置、判断,以及时间修正逻辑。
先看官方库代码:
#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
//#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER)
static servo_t servos; // static array of servo structures
static volatile int8_t Channel; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
uint8_t ServoCount = 0; // the total number of attached servos
// convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
#define SERVO(_timer,_channel) (servos) // macro to access servo class by timer and channel
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
/************ static functions common to all instances ***********************/
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
if( Channel < 0 )
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{
if( SERVO_INDEX(timer,Channel) < ServoCount && SERVO(timer,Channel).Pin.isActive == true )
digitalWrite( SERVO(timer,Channel).Pin.nbr,LOW); // pulse this channel low if activated
}
Channel++; // increment to the next channel
if( SERVO_INDEX(timer,Channel) < ServoCount && Channel < SERVOS_PER_TIMER) {
*OCRnA = *TCNTn + SERVO(timer,Channel).ticks;
if(SERVO(timer,Channel).Pin.isActive == true) // check if activated
digitalWrite( SERVO(timer,Channel).Pin.nbr,HIGH); // its an active channel so pulse it high
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel = -1; // this will get incremented at the end of the refresh period to start again at the first channel
}
}
SIGNAL (TIMER1_COMPA_vect)
{
handle_interrupts(_timer1, &TCNT1, &OCR1A);
}
static void initISR(timer16_Sequence_t timer)
{
......
TCCR1A = 0; // normal counting mode
TCCR1B = _BV(CS11); // set prescaler of 8
TCNT1 = 0; // clear the timer count
TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
......
}
具体说来,官方库由于使用timer1, 为16bit的定时器,所以对于定时器的tick频率(这里指系统晶振fosk/prescale预除数)在16M晶振,预除数8情况下,就是2M,对应舵机的高电平最多2.5ms的情况下,每个舵机最多tick次数2.5*2k=5k < 2^16=65536,因而完全可以在单次COMPA的触发去完成时间的控制,所以相对简单很多。
而定时器2是8bit的,要完成5k次的tick,光靠COMPA的中断是不够的,还需要纪录中断的次数,因而会复杂一些。
在看参考资料1的代码:
typedef struct {
uint8_t nbr :5 ; // a pin number from 0 to 31
uint8_t isActive :1 ; // false if this channel not enabled, pin only pulsed if true
} ServoPin_t ;
typedef struct {
ServoPin_t Pin;
byte counter;
byte remainder;
} servo_t;
static volatile uint8_t Channel; // counter holding the channel being pulsed
static volatile uint8_t ISRCount; // iteration counter used in the interrupt routines;
uint8_t ChannelCount = 0; // counter holding the number of attached channels
static boolean isStarted = false; // flag to indicate if the ISR has been initialised
ISR (TIMER2_OVF_vect)
{
++ISRCount; // increment the overlflow counter
if (ISRCount == servos.counter ) // are we on the final iteration for this channel
{
TCNT2 = servos.remainder; // yes, set count for overflow after remainder ticks
}
else if(ISRCount > servos.counter)
{
// we have finished timing the channel so pulse it low and move on
if(servos.Pin.isActive == true) // check if activated
digitalWrite( servos.Pin.nbr,LOW); // pulse this channel low if active
Channel++; // increment to the next channel
ISRCount = 0; // reset the isr iteration counter
TCNT2 = 0; // reset the clock counter register
if( (Channel != FRAME_SYNC_INDEX) && (Channel <= NBR_CHANNELS) ){ // check if we need to pulse this channel
if(servos.Pin.isActive == true) // check if activated
digitalWrite( servos.Pin.nbr,HIGH); // its an active channel so pulse it high
}
else if(Channel > NBR_CHANNELS){
Channel = 0; // all done so start over
}
}
}
static void initISR()
{
for(uint8_t i=1; i <= NBR_CHANNELS; i++) { // channels start from 1
writeChan(i, DEFAULT_PULSE_WIDTH); // store default values
}
servos.counter = FRAME_SYNC_DELAY; // store the frame sync period
Channel = 0; // clear the channel index
ISRCount = 0; // clear the value of the ISR counter;
/* setup for timer 2 */
TIMSK2 = 0; // disable interrupts
TCCR2A = 0; // normal counting mode
TCCR2B = _BV(CS21); // set prescaler of 8
TCNT2 = 0; // clear the timer2 count
TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
isStarted = true; // flag to indicate this initialisation code has been executed
}
void ServoTimer2::write(int pulsewidth)
{
writeChan(this->chanIndex, pulsewidth); // call the static function to store the data for this servo
}
static void writeChan(uint8_t chan, int pulsewidth)
{
// calculate and store the values for the given channel
if( (chan > 0) && (chan <= NBR_CHANNELS) ) // ensure channel is valid
{
if( pulsewidth < MIN_PULSE_WIDTH ) // ensure pulse width is valid
pulsewidth = MIN_PULSE_WIDTH;
else if( pulsewidth > MAX_PULSE_WIDTH )
pulsewidth = MAX_PULSE_WIDTH;
pulsewidth -=DELAY_ADJUST; // subtract the time it takes to process the start and end pulses (mostly from digitalWrite)
servos.counter = pulsewidth / 128;
servos.remainder = 255 - (2 * (pulsewidth - ( servos.counter * 128))); // the number of 0.5us ticks for timer overflow
}
}
其实现方式为每个舵机对应一个对象,其成员包括该舵机在每个周期(本文中的周期会指两个概念,一个是舵机驱动要求的周期,即20ms,另一个是单片机系统的中断周期,即256ticks,也即指TIMER2_OVF_vect或TIMER2_COMPA_vect终端向量的处理周期,后文中如不加说明,特指后者,如果指前者会加上20ms以做区别)内需要触发的COMPA中断次数即counter和额外的tick次数即reminder。ISRCount始终标记了当前handle的舵机周期20ms内所经过的中断次数,当ISR等于counter,说明该舵机所需的中断周期数已经满足,那么还需要额外经过255-reminder次ticks,所以将TCNT2设置为reminder,则会在255-reminder时间后触发中断,完成高电平的脉冲,开始低电平脉冲后进入下一个舵机的控制阶段。
接下来,我们就先思索下在不考虑修正,时间精确的情况下,写一个差不多能用的舵机库。
首先要说明单个定时器是如何对多个舵机进行控制的,由于前面的第二点,舵机控制周期为20ms,但是其中高电平最多占2.5ms,那么很容易想到,当一个舵机的高电平结束后就可以开始下一个舵机的高电平控制,那么所能控制的舵机数量就是20ms/2.5ms=8个,当然这里面要注意的是:
舵机控制直接的切换是需要耗时的,所以如果要控制8个,舵机的高电平就不能达到5ms,会略少于2.5ms
此外如果加入复杂的计算,也可以实现控制更多舵机的能力,就是让舵机的高电平直接有所重合,这个有兴趣的可以去试试啦。
然后需要明确的是舵机的工作模式,最简单的做法就是中断周期不变,意味着定时器计数始终从0到top,至于top是OCRA还是0XFF,如果是用OCRA作为定时器溢出位置,那么需要在每次切换舵机的时候更改溢出值,但是这样麻烦的是,如果舵机的高电平时间不能整除OCRA,需要中途改变一次OCRA的值,综合考虑,用固定的0XFF比较简单,每次在TCNT==OCRA的时候中断,当COMPA中断第一次的时候输出高电平,然后前N-1次的时候保持高电平,当COMPA最后一次中断的时候输出低电平。然后将OCRA置为0。
例如,16M晶振,prescale为8,每us tick数为2次,如果一个舵机需要高电平1ms,那么就是2000次tick,2000/256= 7, 2000%256=208,所以当前一个舵机高电平结束,OCRA为0,第一次COMPA中断开始,将OCRA置为208,经过8次中断,时间恰好经过2000次TICK,即1ms,然后第八次中断中将OCRA置为0,再经过10次中断,在第19个中断周期中发生第11次中断,这个中断中将OCRA改成0,由于中断中对OCRA的修改在下一个中断周期中才生效,因而实际每个舵机是19个中断周期,那么实际每个舵机可以达到的最大高电平时间为19*256/2=2432us,那么经过8个舵机时间后,仍然达不到20ms,所以需要对整个周期20ms进行修正,20ms-2432us*8=544us,所以需要增加矫正544*2/256=4,544*2%256=64,即矫正4个中断周期,64个tick。
以上就是第一版粗精度的舵机驱动库的实现,原理简单,也很容易复现。
先定义servo结构体,其中pin表示对应舵机控制引脚,cycles对应舵机高电平脉冲需要的中断周期数,ticks对应舵机高电平脉冲除去cycles个中断周期后还需要的ticks数,activated表明该舵机是否启用。并定义了一个全局数组servos用来记录所有的舵机,之所以数组开的大一个,是为了放置修正20ms周期用的虚拟舵机。这个舵机的中断周期数和ticks数即PERIOD_REVISE_CYCLES,而暂不考虑ticks级别的修正,这样不用更改TCNT以调整触发相位,简单些,而且Pwm会因为中断周期固定而更准确。
而对于pwm功能,定义pwm_t结构体,其中ctn为所需要经历的总的溢出中断次数,ocr为溢出中断比较值,即当溢出中断次数达到ocr次后输出低电平,不足输出高电平,cur为当前pwm引脚的计数,这么设置好处是对于不需要pwm分级为256级的pwm应用,可以将pwm分级变小,即ctn设小一些,如此以提高pwm频率,如果ctn设为255则pwm频率约为30.5Hz,如果ctn设为15,则pwm频率为488.3Hz,这个大家可以进行取舍。
typedef struct{
uint8_t pin=0;
uint8_t cycles=0;
volatile uint8_t ticks=0;
bool activated=false;
}servo_t;
typedef struct{
uint8_t pin=0;
uint8_t ctn=255;
volatile uint8_t ocr=0;
uint8_t cur=0;
}pwm_t;
static servo_t servos;
#define PERIOD_REVISE_CYCLES 4
然后是中断相关初始化,对照atmega328p手册就能弄明白,或者参考另外一篇博文 Arduino UNO Infrared emission timer setup 。这里稍微说明下使用的模式是FastPWM,在TCNT2技术达到TOP位置0xFF和OCRA位置分别中断,选这个模式原因见前文所述。
static void initISR(){
servos.activated = false;
servos.cycles = PERIOD_REVISE_CYCLES;
servos.ticks = PERIOD_REVISE_TICKS;
COMPACtn = 0;
curChan = 0;
TIMSK2 = 0; // disable interrupts
TCCR2A = _BV(WGM21) | _BV(WGM20); // fast PWM mode, top 0xFF
TCCR2B = _BV(CS21); // prescaler 8
TCNT2 = 0;
TIFR2 = _BV(TOV2) | _BV(OCF2A);
TIMSK2 = _BV(TOIE2) | _BV(OCIE2A); //enable ovf & ocra interruption
inited = true;
}
PWM的中断处理,循环判断每个pwm通道是否需要切换电平或者重新计数。
ISR(TIMER2_OVF_vect)
{
for(uint8_t i=0;i<pwmCount;i++){
pwms.cur++;
if(pwms.cur <= pwms.ocr){
digitalWrite(pwms.pin, HIGH);
}else {
digitalWrite(pwms.pin, LOW);
}
if(pwms.cur == pwms.ctn){
pwms.cur = 0;
}
}
}
舵机驱动的中断处理,详细解释见前文。
ISR(TIMER2_COMPA_vect){
++COMPACtn;
if(COMPACtn == 1){
if(servos.activated){
digitalWrite( servos.pin, HIGH);
}
OCR2A = servos.ticks;
}else if(COMPACtn == servos.cycles + 1){
if(servos.activated){
digitalWrite(servos.pin, LOW);
}
}else if(curChan == MAX_SERVOS && COMPACtn >= PERIOD_REVISE_CYCLES){
curChan = 0;
OCR2A = 0;
COMPACtn = 0;
}else if(COMPACtn > CYCLES_PER_SERVO){
++curChan;
OCR2A = 0;
COMPACtn = 0;
}else if(COMPACtn > servos.cycles + 1){
if(servos.activated){
digitalWrite(servos.pin, LOW);
}
}
}
舵机的设置
void Timer2Servo::write(uint16_t value){
if(value < MIN_PULSE_WIDTH){
if(value > 180) value = 180;
value = map(value, 0, 180, min_, max_);
}
this->writeMicroseconds(value);
}
void Timer2Servo::writeMicroseconds(uint16_t value){
if(servoChan_ >= MAX_SERVOS){
return;
}
if(value < MIN_PULSE_WIDTH){
value = MIN_PULSE_WIDTH;
}
if(value > MAX_PULSE_WIDTH){
value = MAX_PULSE_WIDTH;
}
servos.cycles = value * TICKS_PER_MICROSECOND / TICKS_PER_CYCLE;
servos.ticks = (value * TICKS_PER_MICROSECOND) % TICKS_PER_CYCLE;
}
以上就是不考虑20ms周期精度,不考虑偶尔出现的因前一个中断处理未结束而后一个中断时间又到了导致后一个中断错过了,从而造成的高低电平脉冲时间不准确,此外上述的库无法对某个舵机输出2.5ms的高电平,也就是通常指的180°,因为最大的可设置毫秒数是2430,即使官方可设置毫秒数也不过544~2400,而我的前一个版本已经可以达到500~2430。那么接下来我们对这个进行修正。
首先我们修正可以达到的脉冲时间。由于前文所述,如果要控制8个舵机,最大只能达到2432us的脉冲,那么为了能达到2500us的脉冲时间,我们只能牺牲一个舵机的控制能力,虽然通常的应用场景,一个timer2控制7个舵机也是足够了。如果最大舵机数量设为8,如果给每个舵机多分配一个中断周期,即20个中断周期,那么最大可以达到2560us,已经可以满足,此时修正CYCLES数为16,但还没完,富余的16个修正中断周期有点多,我们再给每个舵机加一个中断周期,这样还需要修正9个中断周期,后文会解释为什么每个舵机还需要一个中断周期。
在修正完中断周期基本后,为了能更准确的达到20ms,需要在所有舵机包括虚拟舵机的中断周期结束后修正TICKS,为了能实现这个功能,需要在COMPACtn>PERIOD_REVISE_CYCLES满足后调整TCNT2。
另外需要解决的是,当某个舵机的脉冲接近128的整数倍,即脉冲ticks总数接近256的整数倍,也即所需要设置的ticks数接近0或者255,那么很容易导致某个COMPA中断被跳过,进而导致脉冲时间不准或者周期不准。为了修正这个问题,我们将舵机驱动对象重新定义:
typedef struct{
uint8_t pin=0;
volatile uint8_t cycles=0;
volatile uint8_t startTicks=0;
volatile uint8_t endTicks=0;
bool activated=false;
}servo_t;
即将原本单个ticks成员改为两个startTicks和endTicks,这样如果原本的ticks值离0或者255很近,则将整个舵机的脉冲在这个舵机的处理周期内进行偏移,这样每次的COMPA中断位置就不会离0或者255很近,也就很难miss。具体做法见代码:
void Timer2Servo::writeMicroseconds(uint16_t value){
if(servoChan_ >= MAX_SERVOS){
return;
}
if(value < MIN_PULSE_WIDTH){
value = MIN_PULSE_WIDTH;
}
if(value > MAX_PULSE_WIDTH){
value = MAX_PULSE_WIDTH;
}
value = value * TICKS_PER_MICROSECOND - TRIM_PULSE_TICK;
servos.cycles = value / TICKS_PER_CYCLE;
uint8_t ticks = value % TICKS_PER_CYCLE;
if(ticks>=256-2*TRIM_TICKS){
servos.cycles++;
servos.startTicks=3*TRIM_TICKS;
servos.endTicks=ticks+3*TRIM_TICKS;
}else{
servos.startTicks=TRIM_TICKS;
servos.endTicks=ticks+TRIM_TICKS;
}
}
而COMPA的中断处理函数也将变成:
// Handle compare A register to provider servo driver
ISR(TIMER2_COMPA_vect){
#ifdef __DEBUG
++compa_times;
#endif
++COMPACtn;
if(COMPACtn == 1){
OCR2A = servos.endTicks;
if(servos.activated){
digitalWrite( servos.pin, HIGH);
}
}else if(curChan >= MAX_SERVOS && COMPACtn > PERIOD_REVISE_CYCLES){
// also trim to adjust period, not too close to 255 encase miss the next
// interruption. TCNT2_TRIM + PERIOD_REVISE_TICKS is the actual revise.
TCNT2 = 255 - TCNT2_TRIM;
COMPACtn = 0;
curChan = 0;
OCR2A = servos.startTicks;
}
if(curChan < MAX_SERVOS && COMPACtn > servos.cycles){
// a bit larger than 0 to ensure not miss the next interruption
OCR2A = TRIM_TICKS;
if(servos.activated){
digitalWrite(servos.pin, LOW);
}
}
if(curChan < MAX_SERVOS && COMPACtn > CYCLES_PER_SERVO){
++curChan;
OCR2A = servos.startTicks;
COMPACtn = 0;
}
}
pwm的控制因为前述对TCNT2改动导致中断周期变化,因而PWM周期会有轻微抖动,好在抖动幅度不大,在30.6到30.7之间变化,所以精度影响小于0.5%,还可以接受吧,哈哈。
另外需要改进的是,由于digitalWrite的延时较大,如果pwm所有通道都在第1个周期拉高,那么第一个周期的处理时间会比较久,容易导致COMPA中断被miss,所以将pwm各通道的起始拉高周期错开,第i个通道在第i个周期拉高,所以pwm的结构体也发生变化:
typedef struct{
uint8_t pin=0;
volatile uint8_t start=0;
volatile uint8_t end=0;
}pwm_t;
这样牺牲了前一版本的频率可定制的灵活性,保证了精度。
Pwm的实现简单很多,这里不细说,看代码:
void Timer2Pwm::write(uint8_t pwm){
// uint8_t oldSREG = SREG; // Reverse these codes for future use.
// cli();
pwms.start = pwmChan_;
pwms.end = pwm + pwmChan_;
// SREG = oldSREG;
}
// Handle overflow interrupt to provide pwm
ISR(TIMER2_OVF_vect)
{
curPwm++;
for(uint8_t i=0;i<pwmCount;i++){
if(curPwm == pwms.start){
digitalWrite(pwms.pin, HIGH);
}else if(curPwm == pwms.end){
digitalWrite(pwms.pin, LOW);
}
}
}
最后要做的就是上逻辑分析仪看下输出的实际情况,然后做些数值上的修正,主要是digitalWrite会有一定的延时导致的,实际写的时候还需要仔细思考下分支判断的边界和顺序,这个只有动手写一遍才能体会,过于细节不详述。
附上几种方案实测对比:
测试程序为循环设置舵机脉冲时间,多个舵机脉冲时间相差5us,循环步进30us,代码地址:
https://github.com/atp798/Timer2ServoPwm/tree/master/examples/ServosAndPwms
官方库:
未修正的版本:
修正后的高精度版本:
粗略统计,官方的版本,周期误差稳定15us左右,脉冲误差1~2us,平均1us左右,未修正版本周期误差4~20us左右,不稳定,脉冲误差1~2us左右,平均1us,但是较容易出现偶然的脉冲误差整个周期即128us的情况。修正后版本的周期误差稳定小于1us,脉冲误差稳定在0.5~2us,平均小于1us。
最后修正版代码见:
https://github.com/atp798/Timer2ServoPwm
原博客格式更友好:http://www.straka.cn/blog/more-accurate-arduino-uno-timer2-servo-driver-library-than-official-one/
参考资料:
Topic: ServoTimer2 – drives up to 8 servos:
https://forum.arduino.cc/index.php/topic,21975.0.html
https://github.com/nabontra/ServoTimer2
G哥撸Arduino之:深入理解PWM输出:
https://www.arduino.cn/forum.php?mod=viewthread&tid=80668
关于如何修改ATMEGA328P的PWM频率:
https://www.arduino.cn/thread-83019-1-1.html
舵机常见问题原理分析及解决办法
https://blog.csdn.net/fang_chuan/article/details/51557069
舵机控制原理是什么_舵机的控制方法
http://m.elecfans.com/article/687067.html
页:
[1]