Zumo, Leonardo, and Servo library

I’ve added a Parallax PING) sensor on a micro servo on the front of my Zumo, but I ran into a minor problem.
It turns out that the Arduino Servo library uses Timer1, which conflicts with the ZumoMotors on a Leonardo (or other ATmega32U4).

On the chance that anyone else has a similar configuration and wants to add a servo, it was very easy to modify
my copy of Servo.h to use Timer3 on AVR_ATmega32U4, and all’s working now.

Hi,

We are trying to build a small robot based on an arduino and a Zumo shield. The probem is that when we try to attach a servo (we are using the Servo library, and the problem appears only when we write myservo.attach(servoPin); ), the ZumoMotors don’t work as they should.

By searching the web, it appeared that the Servo library uses Timer1, which disables pins 9 and 10. This is probably where the problem comes from. It seems that you solved this problem by using another timer. Could you explain how you did that please ?
We are using an Arduino Uno board, which only have 3 timers, but this may not be a problem.

Thanks in advance !

Jonathan

Hello, Jonathan.

The Uno does not have Timer3 unlike the Leonardo, so hybernaut’s solution might not work for you. You could try disabling the buzzer on the Zumo and rewriting a servo library to use Timer2. You might also try using a software servo library, but I do not have any experience with it and do not know how well it will work. How many servos are you trying to control?

- Jeremy

Hello Jeremy,

Thanks for your answer. There seems to be 3 timers on the arduino uno (timer0, timer1 and timer2). One is used for the motors, another one for the buzzer as you mentionned, so we would like to use the last one for the servo. We just want to control one single servo.

Hyberaut already fixed that problem, we think we could use his solution to do something similar by using another available timer (and not the timer3, cause the uno doesn’t have one). Do you have any idea?

Once again, thank you for your help!

Jonathan

The three timers are all used. Timer0 is used by the Arduino for system timing. We will try to come up with some example sketches that show how to control a servo on the Zumo with an Uno.

- Jeremy

Hello,

Thank you very much for your code examples, they will help us a lot. If this is a problem, we can disable the buzzer, because this really is the servo that matters.

We are looking forward for your help,

Jonathan and Carole.

Hello,

We would like to know if there is something new. We still can’t use the servo and the Zumomotors, which is still an important problem. Can you show us how to use the timer2 for the Servo ? We don’t really need the buzzer, so this should be a solution. Do you have the code examples ?

Once again, thanks for your help,

Jonathan

Hi, Jonathan.

We hope to have an example for you in the next few days.

- Ben

Hello,

Thanks in advance,

Jonathan and Carole

I just wanted to give you a quick update: the servo example is almost done and should be posted tomorrow.

- Ben

Hello.

Here is the code we came up with for controlling a servo from an Arduino UNO using Timer 2. This should allow you to control a servo and run the motors simultaneously:

/** Arduino Uno Timer 2 Servo Example
This example code for the Arduino Uno shows how to use Timer 2 on
the ATmega328P to control a single servo.  This can be useful for
people who cannot use the Arduino IDE's Servo library.  For
example, the ZumoMotors library uses the same timer as the Servo
library (Timer 1), so the two libraries conflict.

The SERVO_PIN macro below specifies what pin to output the 
servo on.  This pin needs to be connected to the signal input
line of the servo.  The Arduino's GND needs to be connected to
the ground pin of the servo.  The servo's ground and power pins
need to be connected to an appropriate power supply.
*/

// This line specifies what pin we will use for sending the
// signal to the servo.  You can change this.
#define SERVO_PIN 11

// This is the time since the last rising edge in units of 0.5us.
uint16_t volatile servoTime = 0;

// This is the pulse width we want in units of 0.5us.
uint16_t volatile servoHighTime = 3000;

// This is true if the servo pin is currently high.
boolean volatile servoHigh = false;

void setup()
{
  servoInit();
}

void loop()
{
  servoSetPosition(1000);  // Send 1000us pulses.
  delay(1000);  
  servoSetPosition(2000);  // Send 2000us pulses.
  delay(1000);
}

// This ISR runs after Timer 2 reaches OCR2A and resets.
// In this ISR, we set OCR2A in order to schedule when the next
// interrupt will happen.
// Generally we will set OCR2A to 255 so that we have an
// interrupt every 128 us, but the first two interrupt intervals
// after the rising edge will be smaller so we can achieve
// the desired pulse width.
ISR(TIMER2_COMPA_vect)
{
  // The time that passed since the last interrupt is OCR2A + 1
  // because the timer value will equal OCR2A before going to 0.
  servoTime += OCR2A + 1;
  
  static uint16_t highTimeCopy = 3000;
  static uint8_t interruptCount = 0;
  
  if(servoHigh)
  {
    if(++interruptCount == 2)
    {
      OCR2A = 255;
    }

    // The servo pin is currently high.
    // Check to see if is time for a falling edge.
    // Note: We could == instead of >=.
    if(servoTime >= highTimeCopy)
    {
      // The pin has been high enough, so do a falling edge.
      digitalWrite(SERVO_PIN, LOW);
      servoHigh = false;
      interruptCount = 0;
    }
  } 
  else 
  {
    // The servo pin is currently low.
    
    if(servoTime >= 40000)
    {
      // We've hit the end of the period (20 ms),
      // so do a rising edge.
      highTimeCopy = servoHighTime;
      digitalWrite(SERVO_PIN, HIGH);
      servoHigh = true;
      servoTime = 0;
      interruptCount = 0;
      OCR2A = ((highTimeCopy % 256) + 256)/2 - 1;
    }
  }
}

void servoInit()
{
  digitalWrite(SERVO_PIN, LOW);
  pinMode(SERVO_PIN, OUTPUT);
  
  // Turn on CTC mode.  Timer 2 will count up to OCR2A, then
  // reset to 0 and cause an interrupt.
  TCCR2A = (1 << WGM21);
  // Set a 1:8 prescaler.  This gives us 0.5us resolution.
  TCCR2B = (1 << CS21);
  
  // Put the timer in a good default state.
  TCNT2 = 0;
  OCR2A = 255;
  
  TIMSK2 |= (1 << OCIE2A);  // Enable timer compare interrupt.
  sei();   // Enable interrupts.
}

void servoSetPosition(uint16_t highTimeMicroseconds)
{
  TIMSK2 &= ~(1 << OCIE2A); // disable timer compare interrupt
  servoHighTime = highTimeMicroseconds * 2;
  TIMSK2 |= (1 << OCIE2A); // enable timer compare interrupt
}

We also plan to add the information from this thread to the Zumo User’s Guide.

–David

1 Like

Hello,

This solved the problem ! We couldn’t rewrite the Servo library by our own, but you did, and that really is nice of you. Thank you very much, and yep this could be useful for many other persons using Zumo Robots.

Once again, thanks,

Regards,

Jonathan and Carole