Timer Interrupt and Servo Library Conflict

Projects Programming
1

I'm working on a robot that chases a particular color. For the vision sensor, I'm using a Pixy Cam (CMUCam5) on the pan/tilt platform (two RC servos). Because there is other chasing code that uses the pan servo position for a zero turn function (feedback for PID), I chose to perform all of the servo operations in the Arduino Mega. Shown in the code below, I'm communicating with the camera using I2C and using the Servo and TimerOne Libraries. I would like to run the Pixy polling and the eventual PID on a timer interrupt as opposed to an event timer so that I can assign priority, but I appear to be having a conflict with the Servo Library. Both methods are left in the code below, but the event timer code is commented out.

/****************
 Last Edited: 8/6
 *****************/

#include <Wire.h> 
#include <PixyI2C.h> 
#include <Servo.h> 
#include <TimerOne.h> // comment out for event timer

PixyI2C pixy;
/* pixel window */
const int minBlockX=120, maxBlockX=200, minBlockY=60, maxBlockY=140; 
const int panPin=8, tiltPin=13; 
const int minPanPos=20, maxPanPos=160, minTiltPos=40, maxTiltPos=140; 
const int panInc=4, tiltInc=4; 
volatile int panPos, tiltPos;                
//unsigned int lastSampleTime=0; // comment out for interrupt
//int sampleTime=50; // comment out for interrupt

Servo pan;
Servo tilt;

void setup() {
  pinMode(panPin,OUTPUT);
  pinMode(tiltPin,OUTPUT);
  pan.attach(panPin);
  tilt.attach(tiltPin);
  pan.write(90);
  tilt.write(90);
  Serial.begin(115200);
  Serial.print("Starting...\n");
  pixy.init();
  Timer1.initialize(50000); // comment out for event timer
  Timer1.attachInterrupt(pollPixy); // comment out for event timer
}


void loop() {
  //if (millis()-lastSampleTime>=sampleTime) {pollPixy();} // comment out for interrupt
}

void pollPixy() {
  Timer1.detachInterrupt(); // comment out for interrupt
  byte i, j=1;
  byte numBlocks = pixy.getBlocks(5);
  unsigned int blockMass[numBlocks];
  byte biggestBlock;
  unsigned int bigBlockX=0, bigBlockY=0;
 
  /* Runs through the blocks, gets the biggest one, and returns its position in the frame */    
  while(numBlocks && j==1) {
    biggestBlock=0;
    for(i=0;i<=numBlocks;i++) {
      if(pixy.blocks[i].width<999 && pixy.blocks[i].height<999) {
        blockMass[i]=pixy.blocks[i].width*pixy.blocks[i].height;
        if(i!=0) {
          if(blockMass[i]>blockMass[i-1]) {
            biggestBlock=i;
          }
        }
      }
    }
    bigBlockX=pixy.blocks[biggestBlock].x;
    bigBlockY=pixy.blocks[biggestBlock].y;
    
    /* increment servos to keep center of object in pixel window  */    
    if(bigBlockX>maxBlockX) {panPos-=panInc;} 
    if(bigBlockX<minBlockX) {panPos+=panInc;} 
    if(bigBlockY>maxBlockY) {tiltPos+=tiltInc;}
    if(bigBlockY<minBlockY) {tiltPos-=tiltInc;}
    
    /* limit servos to acceptable limits */
    if(panPos<minPanPos) {panPos=minPanPos;}
    if(panPos>maxPanPos) {panPos=maxPanPos;}
    if(tiltPos>maxTiltPos) {tiltPos=maxTiltPos;}
    if(tiltPos<minTiltPos) {tiltPos=minTiltPos;}
    
    pan.write(panPos); tilt.write(tiltPos);
    numBlocks=0; j=0;
    }
    while(!numBlocks && j==1) {
      delay(25);
      numBlocks=pixy.getBlocks();
      if(numBlocks) {
        break;
      }
      tilt.write(90);
      pan.write(90);
      byte k;
      for(k=90;k<maxPanPos;k+=5) {
        pan.write(k);
        delay(75);
        numBlocks=pixy.getBlocks();
        if(numBlocks) {
          break; 
        }  
      }
      for(k=maxPanPos;k>minPanPos;k-=5) {
        pan.write(k);
        delay(75);
        numBlocks=pixy.getBlocks();
        if(numBlocks) {
          break; 
        }
      }
      if(numBlocks) {
        break; 
      }
      j=0;  
    }
    Timer1.attachInterrupt(pollPixy); // comment out for event timer
    //lastSampleTime=millis(); // comment out for interrupt
  }

To ensure that the Servo Library does not attempt to use Timer1 (or any other one for that matter), I have commented out the following lines in the Servo.h file to relegate it to Timer5.

// Say which 16 bit timers can be used and in what order
#if 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 ;

When I run it using the event timer it runs perfectly every time, but once I change over to the interrupt absolutely nothing happens, even though it compiles without error. I've narrowed it down to being either a conflict between the timers or the serial communication being interrupted by Timer1 somehow, leaning heavily towards the first. Has anyone encountered a similar issue before? If so, what steps did you take to solve it?

2

There is a ServoTimer2 library that may get around the problem.

...R

3

You are calling delay() in an ISR, this simply waits forever.

You ISR should be short and sweet, not large and complex. You are probably
much better polling?

What do you mean by "assign priority"?

4

MarkT, you are exactly right about the delay()s and the shorter ISR. Guess I just wasn't thinking. I've moved the bulk of the operation to the void loop, but I still couldn't seem to get any response. I've made sure that any delay() occurs between detaching and reattaching the interrupt. The updated code looks something like this:

/****************
 Last Edited: 8/6
 *****************/

#include <Wire.h> 
#include <PixyI2C.h> 
#include <Servo.h>  
#include <TimerOne.h>

PixyI2C pixy;
/* pixel window */
const int minBlockX=120, maxBlockX=200, minBlockY=60, maxBlockY=140; 
const int panPin=8, tiltPin=13; 
const int minPanPos=20, maxPanPos=160, minTiltPos=40, maxTiltPos=140; 
const int panInc=4, tiltInc=4; 
int panPos, tiltPos;                

volatile byte j=0;
byte i;
volatile int numBlocks;
int biggestBlock;
unsigned int blockMass[5], bigBlockX=0, bigBlockY=0;

Servo pan;
Servo tilt;

void setup() {
  pinMode(panPin,OUTPUT);
  pinMode(tiltPin,OUTPUT);
  pan.attach(panPin);
  tilt.attach(tiltPin);
  pan.write(90);
  tilt.write(90);
  pixy.init();
  Timer1.initialize(50000);
  Timer1.attachInterrupt(pollPixy);
}


void loop() {
  /* Runs through the blocks, gets the biggest one, and returns its position in the frame */    
  while(numBlocks && j==1) {
    Timer1.detachInterrupt();
    biggestBlock=0;
    for(i=0;i<=numBlocks;i++) {
      if(pixy.blocks[i].width<999 && pixy.blocks[i].height<999) {
        blockMass[i]=pixy.blocks[i].width*pixy.blocks[i].height;
        if(i!=0) {
          if(blockMass[i]>blockMass[i-1]) {
            biggestBlock=i;
          }
        }
      }
    }
    bigBlockX=pixy.blocks[biggestBlock].x;
    bigBlockY=pixy.blocks[biggestBlock].y;
    
    /* increment servos to keep center of object in pixel window  */    
    if(bigBlockX>maxBlockX) {panPos-=panInc;} 
    if(bigBlockX<minBlockX) {panPos+=panInc;} 
    if(bigBlockY>maxBlockY) {tiltPos+=tiltInc;}
    if(bigBlockY<minBlockY) {tiltPos-=tiltInc;}
    
    /* limit servos to acceptable limits */
    if(panPos<minPanPos) {panPos=minPanPos;}
    if(panPos>maxPanPos) {panPos=maxPanPos;}
    if(tiltPos>maxTiltPos) {tiltPos=maxTiltPos;}
    if(tiltPos<minTiltPos) {tiltPos=minTiltPos;}
    
    pan.write(panPos); tilt.write(tiltPos);
    j=0;
    Timer1.attachInterrupt(pollPixy);
    }
    while(!numBlocks && j==1) {
      Timer1.detachInterrupt();
      delay(25);
      numBlocks=pixy.getBlocks();
      if(numBlocks) {
        break;
      }
      tilt.write(90);
      pan.write(90);
      for(byte k=90;k<maxPanPos;k+=5) {
        pan.write(k);
        delay(75);
        numBlocks=pixy.getBlocks();
        if(numBlocks) {
          break; 
        }  
      }
      for(byte k=maxPanPos;k>minPanPos;k-=5) {
        pan.write(k);
        delay(75);
        numBlocks=pixy.getBlocks();
        if(numBlocks) {
          break; 
        }
      }
      if(numBlocks) {
        break; 
      }
      j=0; 
      Timer1.attachInterrupt(pollPixy);
    }
  }

void pollPixy() {
  j=1;
  numBlocks = pixy.getBlocks(5);
  }

As far as "assigning priority": I have other tasks that I'd like to put on timers. From my understanding, the different timers have priority relative to each other (when two interrupts are called at the same time, which one is executed first). I was intending on using this to prioritize the other tasks.