Hey all! I am experimenting trying to make a 4 legged walking robot walk stably across the ground using an MPU6050. However whenever I try to run the timer and interrupt it causes several of the servos to jitter and I am not sure why. Any advice welcome!
basic code below, I can upload more if necessary.
[code]
void setup() {
Serial.begin(9600);
servo0.attach(14);
servo1.attach(3);
servo2.attach(4);
servo3.attach(5);
servo4.attach(6);
servo5.attach(7);
servo6.attach(8);
servo7.attach(22);
servo8.attach(23);
servo9.attach(11);
servo10.attach(12);
servo11.attach(13);
standCalibrated();
initTimer();
sit();
}
void loop() {
readMPU();
}
void initTimer () {
//this sets up timer1 to work as the PID
noInterrupts();
TCCR2A = 0;
TCCR2B = 0;
OCR2A = 9999;
TCCR2B |= (1 << WGM12);
TCCR2B |= (1 << CS12);
TIMSK2 |= (1 << OCIE1A);
interrupts();
}
ISR(TIMER2_COMPA_vect)
{
//first we are making the side to side (ie xy plane) tilt work
gyroRateZ = map(gyroZ, -32768, 32767, -250, 250);
gyroAngleZ = (float)gyroRateZ*sampleTime/1000;
accAngleXY = atan2(accY, accX)*RAD_TO_DEG;
//apply complementary filter
currAngleXY = 0.9934 * (prevAngleXY + gyroAngleZ) + 0.0066 * (accAngleXY);
prevAngleXY = currAngleXY;
errorXY = currAngleXY - targetAngleXY;
errorSumXY = errorSumXY + errorXY;
errorSumXY = constrain(errorSumXY, -300, 300);
PID = Kp*(errorXY) + Ki*(errorSumXY)sampleTime - Kd(currAngleXY-prevAngleXY)/sampleTime;
prevAngleXY = currAngleXY;
}[/code]
Hey,
It is most likely an interrupt conflict. The Servo library uses hardware timers and interrupts to drive the signals and since the AVR chip used can only perform one interrupt at any time, if two conflict with each other it can most likely cause the signals to be offset due to missed interrupts.
It may be best to change your structure to the following form:
while(1)
{
if(taskTimeElapsed)
{
..]
}
if(otherTaskTimeElapsed)
{
..]
}
}
Where the taskTimeElapsed can be determined using the () functionmillis. Each time you enter the code block (condition is true), reset your counter to curent time.
taskTimeElapsed can be as simple as comparing a saved time + set value (duration between events) to current time: ex: if( (startTime + durationTime) >= millis() ). Once inside the block of code, simply do startTime = millis(); to reset the counter.
Doing this will allow you to still have precise enough timing while not causing interrupt issues. If you still wish to use interrupts, you’ll need to map out when they happen so as to prevent conflicts between them. This will mean checking out the Servo library code to understand better how it works.
Also, please note that if uploading more than a few lines of code, it is typically best to place it in an attachment instead. This makes the topic far more readable once there are multiple posts.
Sincerely,
I was under the impression that the servo library uses timer 5 on the Arduino Mega. Would that still cause problems even though I am using a different timer? Or is it likely just because there are multiple interrupts trying to run at once?
Thanks for the tips!
If your usage was overlapping with the Servo library on the same hardware (the timer(s) it needs), you’d most likely get far bigger problems (such as the library not working at all or causing very erratic behavior).
From what you were describing, it seems like it is still working, just not great. This would indicate interrupt conflicts as the main probable cause.
We recommend using the method above to reduce at last one source of continuous interrupts that may be causing the issues.
