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

......

}

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

}

}

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

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;

}

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;

}

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;

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;

}

}

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;

}

}

1

2

3

4

5

typedef struct{

uint8_t pin=0;

volatile uint8_t start=0;

volatile uint8_t end=0;

}pwm_t;

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

}

}

}