Issues with timmer interrupts while using the Arduino standard servo library

I had issues using timer interrupts and the arduino standard servo library at the same time. Because I could not find a solution on the internet I wanted to share my solution here. So this might help someone with a similar but probably rare problem.

I am using the atmega2560 on my own pcb with the megacore boardsmanager. I am driving several stepper motors with s2209 stepper drivers and the accelStepper library and one servo with the standard arduino servo library.

My system is communicating through CAN bus (MCP2515). When I wanted to add a recurring status update (once a second) through a timer interrupt, my code would not compile anymore. I got this error message:

Error compiling for board ATmega2560.

Here my two functions which worked in the end. (setupHeartBeat() needs to be placed in setup(). )

void setupHeartBeat()
    // initialize timer4 

// *
//    CPU frequency 16Mhz
//    maximum timer counter value (256 for 8bit, 65536 for 16bit timer)
//    Divide CPU frequency through the choosen prescaler (16000000 / 256 = 62500)
//    Divide result through the desired frequency (62500 / 1Hz = 62500)
//    Verify the result against the maximum timer counter value (62500 < 65536 success) if fail, choose bigger

  noInterrupts();           // disable all interrupts

  // reset the registers to 0 before making the relevant settings
  TCCR4A = 0;
  TCCR4B = 0;
  TCNT4  = 0;

  OCR4A = 62500;            // compare match register 16MHz/256/1Hz (section 17.2.1 of atmega2560 manual)
  TCCR4B |= (1 << WGM42);   // CTC mode (Table 17-2 of atmega2560 manual)
  TCCR4B |= (1 << CS42);    // 256 prescaler (Table 17-6 of atmega2560 manual)
  TIMSK4 |= (1 << OCIE4A);  // enable timer compare interrupt (section 19.9.6 of atmega2560 manual)
  interrupts();             // enable all interrupts
// Timer SETUP
ISR(TIMER4_COMPA_vect)          // timer compare interrupt service routine
   digitalWrite(ledPin, digitalRead(ledPin) ^ 1);   // toggle LED pin

This was a test Skript to toggle an LED just like the Blink example.

I figured it must have had to do with a timer collision. So I looked through all the stuff I used which might use timers or interrupts. Initially I choose timer 4 because timer 0 is reserved for arduino standards like millis() and I read somewhere that timer 5 is used for servos in case of the atmega2560. When removing the servo library in a test script the timer interrupt worked as intended. Looking into the servo library it seemed that the servo library occupies all the 16-Bit timers that were left, which must have caused the error. I commented the relevant section (timer4) for my mcu and the problem was solved. So the LED blinks while a servo does its job.

Here the section of ServoTimers.h that I changed:

#elif defined(__AVR_ATmega640__) || defined(__AVR_ATmega1280__) \
|| defined(__AVR_ATmega2560__)
#define _useTimer5
#define _useTimer1
#define _useTimer3
// #define _useTimer4
// typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t;
typedef enum { _timer5, _timer1, _timer3, _Nbr_16timers } timer16_Sequence_t;

Just because I was using the megacore boardsmanager I had to consider which servo library to change.

If someone knows a better solution to this issue than changing a standard library I would be happy for any suggestions.