Arduino UNO基于Timer2的舵机驱动库（精度比官方的高）

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通道是否需要切换电平或者重新计数。



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;

      }

   }

}

舵机驱动的中断处理，详细解释见前文。

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);

     }

  }

}

舵机的设置

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中断被跳过，进而导致脉冲时间不准或者周期不准。为了修正这个问题，我们将舵机驱动对象重新定义：

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[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的中断处理函数也将变成:

// 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的结构体也发生变化：

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[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);

      }

   }

}