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單片機的定時器這塊也不是特別瞭解，索性邊學邊自己也寫一個吧。網上的源碼並沒有太多的解釋，看了一遍後，發現和庫的源碼思路不太一致，主要在於跳變沿的中斷條件設置、判斷，以及時間修正邏輯。
先看官方庫代碼：
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 |
#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[MAX_SERVOS]; // static array of servo structures static volatile int8_t Channel[_Nbr_16timers ]; // 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[SERVO_INDEX(_timer,_channel)]) // 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[timer] < 0 ) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer else{ if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ) digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated }
Channel[timer]++; // increment to the next channel if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated digitalWrite( SERVO(timer,Channel[timer]).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[timer] = -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的代碼：
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 |
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[Channel].counter ) // are we on the final iteration for this channel { TCNT2 = servos[Channel].remainder; // yes, set count for overflow after remainder ticks } else if(ISRCount > servos[Channel].counter) { // we have finished timing the channel so pulse it low and move on if(servos[Channel].Pin.isActive == true) // check if activated digitalWrite( servos[Channel].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[Channel].Pin.isActive == true) // check if activated digitalWrite( servos[Channel].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[FRAME_SYNC_INDEX].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[chan].counter = pulsewidth / 128; servos[chan].remainder = 255 - (2 * (pulsewidth - ( servos[chan].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，這個大家可以進行取捨。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
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[MAX_SERVOS + 1];
#define PERIOD_REVISE_CYCLES 4 |
然後是中斷相關初始化，對照atmega328p手冊就能弄明白，或者參考另外一篇博文 Arduino UNO Infrared emission timer setup 。這裏稍微說明下使用的模式是FastPWM,在TCNT2技術達到TOP位置0xFF和OCRA位置分別中斷，選這個模式原因見前文所述。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
static void initISR(){ servos[MAX_SERVOS].activated = false; servos[MAX_SERVOS].cycles = PERIOD_REVISE_CYCLES; servos[MAX_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通道是否需要切換電平或者重新計數。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
ISR(TIMER2_OVF_vect) { for(uint8_t i=0;i<pwmCount;i++){ pwms[i].cur++; if(pwms[i].cur <= pwms[i].ocr){ digitalWrite(pwms[i].pin, HIGH); }else { digitalWrite(pwms[i].pin, LOW); } if(pwms[i].cur == pwms[i].ctn){ pwms[i].cur = 0; } } } |
舵機驅動的中斷處理，詳細解釋見前文。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 |
ISR(TIMER2_COMPA_vect){ ++COMPACtn; if(COMPACtn == 1){ if(servos[curChan].activated){ digitalWrite( servos[curChan].pin, HIGH); } OCR2A = servos[curChan].ticks; }else if(COMPACtn == servos[curChan].cycles + 1){ if(servos[curChan].activated){ digitalWrite(servos[curChan].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[curChan].cycles + 1){ if(servos[curChan].activated){ digitalWrite(servos[curChan].pin, LOW); } } } |
舵機的設置
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
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[servoChan_].cycles = value * TICKS_PER_MICROSECOND / TICKS_PER_CYCLE; servos[servoChan_].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中斷被跳過，進而導致脈衝時間不準或者週期不準。爲了修正這個問題，我們將舵機驅動對象重新定義：
|
1 2 3 4 5 6 7 |
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。具體做法見代碼：
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
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[servoChan_].cycles = value / TICKS_PER_CYCLE; uint8_t ticks = value % TICKS_PER_CYCLE;
if(ticks>=256-2*TRIM_TICKS){ servos[servoChan_].cycles++; servos[servoChan_].startTicks=3*TRIM_TICKS; servos[servoChan_].endTicks=ticks+3*TRIM_TICKS; }else{ servos[servoChan_].startTicks=TRIM_TICKS; servos[servoChan_].endTicks=ticks+TRIM_TICKS; } } |
而COMPA的中斷處理函數也將變成:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 |
// Handle compare A register to provider servo driver ISR(TIMER2_COMPA_vect){ #ifdef __DEBUG ++compa_times; #endif ++COMPACtn; if(COMPACtn == 1){ OCR2A = servos[curChan].endTicks; if(servos[curChan].activated){ digitalWrite( servos[curChan].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[0].startTicks; } if(curChan < MAX_SERVOS && COMPACtn > servos[curChan].cycles){ // a bit larger than 0 to ensure not miss the next interruption OCR2A = TRIM_TICKS; if(servos[curChan].activated){ digitalWrite(servos[curChan].pin, LOW); } } if(curChan < MAX_SERVOS && COMPACtn > CYCLES_PER_SERVO){ ++curChan; OCR2A = servos[curChan].startTicks; COMPACtn = 0; } } |
pwm的控制因爲前述對TCNT2改動導致中斷週期變化，因而PWM週期會有輕微抖動，好在抖動幅度不大，在30.6到30.7之間變化，所以精度影響小於0.5%，還可以接受吧，哈哈。
另外需要改進的是，由於digitalWrite的延時較大，如果pwm所有通道都在第1個週期拉高，那麼第一個週期的處理時間會比較久，容易導致COMPA中斷被miss，所以將pwm各通道的起始拉高週期錯開，第i個通道在第i個週期拉高，所以pwm的結構體也發生變化：
|
1 2 3 4 5 |
typedef struct{ uint8_t pin=0; volatile uint8_t start=0; volatile uint8_t end=0; }pwm_t; |
這樣犧牲了前一版本的頻率可定製的靈活性，保證了精度。
Pwm的實現簡單很多，這裏不細說，看代碼：
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
void Timer2Pwm::write(uint8_t pwm){ // uint8_t oldSREG = SREG; // Reverse these codes for future use. // cli(); pwms[pwmChan_].start = pwmChan_; pwms[pwmChan_].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[i].start){ digitalWrite(pwms[i].pin, HIGH); }else if(curPwm == pwms[i].end){ digitalWrite(pwms[i].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
參考資料：
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
