Adding a Motion Command Queue

A motion command queue is a very useful addition for motion contol. It allows new motion commands to decoded and queued while a previous motion command is being executed. This minimises delays between motion commands. This is really important for clean laser burns.

A queue is just a buffer with two pointers "head" and "tail". The head points to the next available buffer position and tail points to the next motion command to be executed. If the head pointer equals the tail pointer then the queue is empty. The queue "wraps" around when it get to the end of the buffer.

Here is a "push" example:

  // Queue length
  queue=head-tail;
  if (queue<0) queue=queue+QueueSize;
  if (queue<QueueSize-1) {
    // Push motion command
    cli();
    xNew[head]=iSin[cosAngle];
    yNew[head]=iSin[sinAngle];
    zNew[head]=0;
    .
    .
    .
    if (++head>=QueueSize) head=head-QueueSize;
    sei();
  }

What does it do:

1. Checks the queue has room for a new command.
2. Disables interrupts (no interupts while updating volatile variables!).
3. Sets the values to the head of the queue.
4. Updates the head pointer and check if it has to be rolled over.

Here is a "pop" example (inside the ISR)

  if (head!=tail) {
    // Determine next motion
    dx=xNew[tail]-xCurrent;
    dy=yNew[tail]-yCurrent;
    dz=zNew[tail]-zCurrent;
    .
    .
    .
    // Pop queue
    if (++tail>=QueueSize) tail=tail-QueueSize;
  }

What does it do:

1. Checks that the queue is not empty.
2. Gets the values from the tail of the queue.
3. Updates the tail pointer and checks if it has to be rolled over.

Motion Flow Control

By quering the queue length, the flow of motion commands can be managed.

"GCodeSender" does this by expecting an "ok" from the last sent command before sending the next command.

The motion controller code checks if it has room on the queue before returning an "ok" to the last motion command. It is best to keep a couple of queue slots free for compound commands like M30 (e.g. "End of program"). As M30 could be interpreted as:

1. Turn off laser or lift head to safe height.
2. Go to Home position.

Here is the update motion controller code:

/*
  Simple 3 Axis ISR Motion Controller - Part 3: Add a queue
  =========================================================
  Written by Alan Cooper (agp.cooper@gmail.com)
  This work is licensed under the
  Creative Commons Attribution - Non Commercial 2.5 License.
  This means you are free to copy and share the code (but not to sell it).

  Motion is in absolute steps
  Feed rate range is 1 to 4095 steps per second
*/

// Motor controller (CNC board) pin mapping: 
#define DirX           2
#define DirY           3
#define DirZ           4
#define StepX          5
#define StepY          6
#define StepZ          7
#define Enable         8        // Enable stepper motors (active low)
#define Laser         12        // Turn laser on or off

// Motion controller defaults
#define Feed        1000        // Default feed rate
#define QueueSize     16        // Queue size
#define xReverse   false        // Reverse X axis direction
#define yReverse    true        // Reverse Y axis direction
#define zReverse   false        // Reverse Z axis direction

// Motion controller queue and ISR variables
volatile int head=0;            // Queue pointer (head)
volatile int tail=0;            // Queue pointer (tail)
volatile int queue=0;           // Queue length
volatile long xNew[QueueSize];  // The target X co-ordinate
volatile long yNew[QueueSize];  // The target Y co-ordinate
volatile long zNew[QueueSize];  // The target Z co-ordinate
volatile long feed[QueueSize];  // Set motion feed rate
volatile long laser[QueueSize]; // Laser (on/off)
volatile long xCurrent=0;       // The current X co-ordinate
volatile long yCurrent=0;       // The current Y co-ordinate
volatile long zCurrent=0;       // The current Z co-ordinate
volatile long steps=0;          // Number of steps remaining in motion
ISR(TIMER2_OVF_vect)
{
  static long dx,dy,dz;
  static long ax,ay,az;
  static long sx,sy,sz;
  static long mx,my,mz;
  static long stepX,stepY,stepZ;
  static unsigned int phase=0;
  static unsigned int magic=8000;
  if (phase<0x8000) {
    phase+=magic;
    if (phase>=0x8000) {
      if (steps==0) {
        if (head!=tail) {
          // Determine next movement parameters (35us)
          dx=xNew[tail]-xCurrent;
          dy=yNew[tail]-yCurrent;
          dz=zNew[tail]-zCurrent;
          ax=abs(dx);
          ay=abs(dy);
          az=abs(dz);
          sx=xNew[tail]<xCurrent?-1:xNew[tail]>xCurrent?1:0;
          sy=yNew[tail]<yCurrent?-1:yNew[tail]>yCurrent?1:0;
          sz=zNew[tail]<zCurrent?-1:zNew[tail]>zCurrent?1:0;
          if ((ax>=ay)&&(ax>=az)) {
            mx=0;
            my=ay-(ax>>1);
            mz=az-(ax>>1);
            steps=ax;
          } else if ((ay>=ax)&&(ay>=az)) {
            mx=ax-(ay>>1);
            my=0;
            mz=az-(ay>>1);
            steps=ay;
          } else {
            mx=ax-(az>>1);
            my=ay-(az>>1);
            mz=0;
            steps=az;
          }
          // Set the stepper directions
          if (xReverse) {
            if (sx==1) PORTD&=~(1<<DirX); else PORTD|=1<<DirX;
          } else {
            if (sx==-1) PORTD&=~(1<<DirX); else PORTD|=1<<DirX;
          } 
          if (yReverse) {
            if (sy==1) PORTD&=~(1<<DirY); else PORTD|=1<<DirY;
          } else {
            if (sy==-1) PORTD&=~(1<<DirY); else PORTD|=1<<DirY;
          } 
          if (zReverse) {
            if (sz==1) PORTD&=~(1<<DirZ); else PORTD|=1<<DirZ;
          } else {
            if (sz==-1) PORTD&=~(1<<DirZ); else PORTD|=1<<DirZ;
          }
          // Set laser
          if (laser[tail]>0) {
            if (Laser<8) PORTD|=1<<Laser; else if (Laser<14) PORTB|=1<<Laser-8;
          } else {
            if (Laser<8) PORTD&=~(1<<Laser); else if (Laser<14) PORTB&=~(1<<Laser-8);
          }
          // Set feed
          magic=feed[tail]<<3;
          // Pop queue
          if (++tail>=QueueSize) tail=tail-QueueSize;
        }
      }
      // Reset step low
      PORTD&=~(1<<StepX);
      PORTD&=~(1<<StepY);
      PORTD&=~(1<<StepZ);
    }
  } else {
    phase+=magic;
    if (phase<0x8000) {
      if (steps>0) {
        // Advance steppers (25us)
        stepX=0;
        stepY=0;
        stepZ=0;
        if ((ax>=ay)&&(ax>=az)) {
          if (my>=0) {
            my-=ax;
            stepY=sy;
          }
          if (mz>=0) {
            mz-=ax;
            stepZ=sz;
          }
          my+=ay;
          mz+=az;
          stepX=sx;
        } else if ((ay>=ax)&&(ay>=az)) {
          if (mx>=0) {
            mx-=ay;
            stepX=sx;
          }
          if (mz>=0) {
            mz-=ay;
            stepZ=sz;
          }
          mx+=ax;
          mz+=az;
          stepY=sy;
        } else {
          if (mx>=0) {
            mx-=az;
            stepX=sx;
          }
          if (my>=0) {
            my-=az;
            stepY=sy;
          }
          mx+=ax;
          my+=ay;
          stepZ=sz;
        }
        xCurrent+=stepX;
        yCurrent+=stepY;
        zCurrent+=stepZ;      
        // Step HIGH
        if (stepX!=0) PORTD|=1<<StepX;
        if (stepY!=0) PORTD|=1<<StepY;
        if (stepZ!=0) PORTD|=1<<StepZ;
        steps--;
      }
    }
  }
}

int iSin[360];
void setup()
{
  // LED
  pinMode(LED_BUILTIN,OUTPUT);

  // Initialise Motor Controller Hardware
  pinMode(DirX,OUTPUT);
  pinMode(StepX,OUTPUT);
  pinMode(DirY,OUTPUT);
  pinMode(StepY,OUTPUT);
  pinMode(DirZ,OUTPUT);
  pinMode(StepZ,OUTPUT);
  pinMode(Enable,OUTPUT);
  pinMode(Laser,OUTPUT);
  digitalWrite(DirX,LOW);
  digitalWrite(StepX,LOW);
  digitalWrite(DirY,LOW);
  digitalWrite(StepY,LOW);
  digitalWrite(DirZ,LOW);
  digitalWrite(StepZ,LOW);
  digitalWrite(Enable,LOW);   // Active low
  digitalWrite(Laser,LOW);    // Active high

  // Use Timer 2 for ISR
  // Good for ATmega48A/PA/88A/PA/168A/PA/328/P
  cli();
  TIMSK2=0;                                     // Clear timer interrupts
  TCCR2A=(0<<COM2A0)|(0<<COM2B0)|(3<<WGM20);    // Fast PWM
  TCCR2B=(1<<WGM22)|(2<<CS20);                  // 2 MHz clock and Mode 7
  OCR2A=243;                                    // Set for 8197 Hz
  OCR2B=121;                                    // Not used
  TIMSK2=(1<<TOIE2)|(0<<OCIE2A)|(0<<OCIE2B);    // Set interrupts (on Top Overflow)
  sei();

  // Prepare circle array
  for (int i=0;i<360;i++) {
    iSin[i]=(int)(200*sin(radians(i))); 
  }

}

void loop()
{
  /* Motion Command */
  // Used to keep track of motion
  static int sinAngle=-1;
  static int cosAngle=0;
  
  // Queue length
  queue=head-tail;
  if (queue<0) queue=queue+QueueSize;
  if (queue<QueueSize-1) {

    if (sinAngle==-1) {
      sinAngle=0;
      cosAngle=90;
      cli();
      xNew[head]=(long)iSin[cosAngle];
      yNew[head]=(long)iSin[sinAngle];
      zNew[head]=0;
      feed[head]=1000;
      laser[head]=0; // Laser off
      if (++head>=QueueSize) head=head-QueueSize;
      sei();
    } else if (sinAngle==-2) {
      cli();
      xNew[head]=0;
      yNew[head]=0;
      zNew[head]=0;
      feed[head]=1000;
      laser[head]=0; // Laser off
      if (++head>=QueueSize) head=head-QueueSize;
      sei();
    } else {
      sinAngle+=1;
      cosAngle+=1;
      if (sinAngle>=360) sinAngle-=360;
      if (cosAngle>=360) cosAngle-=360;
      cli();
      xNew[head]=(long)iSin[cosAngle];
      yNew[head]=(long)iSin[sinAngle];
      zNew[head]=0;
      feed[head]=1000;
      laser[head]=1; // Laser on
      if (++head>=QueueSize) head=head-QueueSize;
      sei();
      if (sinAngle==0) sinAngle=-2;
    }
  }
}

AlanX