I would like to control 2 motors using Timer2 @ 200Hz. The frequency of the timer is printed in the main loop every second. I'm using an Arduino Uno with two interrupt used for the encoders connected to pin 2 and 3.
The problem
The timer runs at 200Hz as expected but the motors are shaking even if I ask them not to spin. The Timer2 is not required by the servo library.
How can I solve this problem? How do you handle Timer interrupts and servo routines?
CODE
#include <Servo.h>
Servo motorDx, motorSx;
[...]
void setup()
{
// Setup Timer2 @ 200Hz
cli(); // Stops interrupts
TCCR2A = 0;// set entire TCCR1A register to 0
TCCR2B = 0;// same for TCCR1B
// set compare match register for 200 Hz increments
OCR2A = 77;// = (16*10^6) / (200*1024) - 1 -> 200 Hz
// turn on CTC mode
TCCR2A |= (1 << WGM21);
// Set CS10 and CS12 bits for 1024 prescaler
TCCR2B |= (1 << CS20) | (1 << CS21) | (1 << CS22);
// enable timer compare interrupt
TIMSK2 |= (1 << OCIE2A);
sei(); //enable global interrupts
attachInterrupt(0, MSencVel, RISING);
attachInterrupt(1, MDencoder, RISING);
motorDx.attach(10, 1000, 2000);
motorSx.attach(11, 1000, 2000);
}
void loop()
{
if(micros()-tOld >= 50000)
{
tOld = micros();
count += 1;
if(count >= 20) // Runs @ 1 Hz
{
count = 0;
serialRoutine();
// prints number of ISR/sec
Serial.println(contIsr)
contIsr=0;
}
}
}
ISR(TIMER2_COMPA_vect)
{
//sendValuesToMotors();
motorDX.write(90);
contIsr++;
}