ISR Motion Control

Reworked the code to use an ISR (Interrupt Service Routine).

The ISR is pretty heavy duty but with the code optimisation it runs in less than 25 us. So it will run with a 16 MHz input clock (i.e. 31372.55 Hz interrupt rate). Still I slowed input clock down to 2 MHz for a 3921.57 Hz interrupt rate. As there is no ramp code, it is likely the stepper motors will miss steps if you try to go too fast.

Here is the updated code:

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

// 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  // Active low
#define Laser   12  // Turn laser on or off

// Set motion (rate) steps per second
unsigned long rate=1000;

// Motor Controller direction settings
bool xReverse=false;
bool yReverse=true;
bool zReverse=false;

/* Motion ISR */
volatile long xNew=0;           // The target X co-ordinate
volatile long yNew=0;           // The target Y co-ordinate
volatile long zNew=0;           // The target Z co-ordinate
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=-1;         // Number of steps remaining in motion
volatile unsigned int magic=0;  // Magic number (used by ISR)
volatile unsigned int phase=0;  // Accumulator (used by ISR)
ISR(TIMER2_OVF_vect)
{
  static long stepX,stepY,stepZ;
  static long dx,dy,dz,ax,ay,az,sx,sy,sz,mx,my,mz;
  
  if (phase<0x8000) {
    phase+=magic;
    if (phase>=0x8000) {
      if (steps>0) {
        // Advance steppers
        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--;
        // Disable ISR 
        if (steps==0) steps=-1;
      }
    }
    if (steps==0) {
      // Determine next movement parameters
      dx=xNew-xCurrent;
      dy=yNew-yCurrent;
      dz=zNew-zCurrent;
      // Check that there is something to do
      if ((dx!=0)||(dy!=0)||(dz!=0)) {
        ax=abs(dx);
        ay=abs(dy);
        az=abs(dz);
        sx=xNew<xCurrent?-1:xNew>xCurrent?1:0;
        sy=yNew<yCurrent?-1:yNew>yCurrent?1:0;
        sz=zNew<zCurrent?-1:zNew>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) {
          // digitalWrite(DirX,(1-sx)>>1);
          if (sx==1) PORTD&=~(1<<DirX); else PORTD|=1<<DirX;
        } else {
          // digitalWrite(DirX,(sx+1)>>1);
          if (sx==-1) PORTD&=~(1<<DirX); else PORTD|=1<<DirX;
        } 
        if (yReverse) {
          // digitalWrite(DirY,(1-sy)>>1);
          if (sy==1) PORTD&=~(1<<DirY); else PORTD|=1<<DirY;
        } else {
          // digitalWrite(DirY,(sy+1)>>1);
          if (sy==-1) PORTD&=~(1<<DirY); else PORTD|=1<<DirY;
        } 
        if (zReverse) {
          // digitalWrite(DirZ,(1-sz)>>1);
          if (sz==1) PORTD&=~(1<<DirZ); else PORTD|=1<<DirZ;
        } else {
          // digitalWrite(DirZ,(sz+1)>>1);
          if (sz==-1) PORTD&=~(1<<DirZ); else PORTD|=1<<DirZ;
        } 
      }
    }
  } else {
    phase+=magic;
    // Reset step LOW
    PORTD&=~(1<<StepX);
    PORTD&=~(1<<StepY);
    PORTD&=~(1<<StepZ);
  }
}

#include <math.h> // For PI, radians(), sin() and cos()
int iSin[360];
int iCos[360];
void setup()
{
  // LED
  pinMode(LED_BUILTIN,OUTPUT);

  // Initialise Motor Controller
  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

  // Disable interrupts
  cli();

  // Use Timer 2 for ISR 
  // Good for ATmega48A/PA/88A/PA/168A/PA/328/P
  TIMSK2=0;                                     // Timer interrupts off
  TCCR2A=(0<<COM2A0)|(0<<COM2B0)|(1<<WGM20);    // Phase correct PWM, no output
  TCCR2B=(0<<WGM22)|(2<<CS20);                  // 2 MHz clock (3921.57 Hz)
  OCR2A=128;                                    // Set 50% duty
  TIMSK2=(1<<TOIE2);                            // Set interrupt on overflow (=BOTTOM)

  // Enable interrupts
  sei();
  
  // Prepare circle array
  for (int i=0;i<360;i++) {
    iSin[i]=(int)(200*sin(radians(i)));
    iCos[i]=(int)(200*cos(radians(i)));   
  }
  
  // Restart Timer
  phase=0;
  magic=rate*52224/3125;   // 2 MHz clock
}

void loop()
{
  // Used to keep track of motion
  static int angle=-1;
  
  // Update only when "steps==-1"
  if (steps==-1) {
    // Make a circle
    if (angle==-1) {
      digitalWrite(Laser,LOW);
      angle=0;
      xNew=iCos[angle];
      yNew=iSin[angle];
      zNew=0;
    } else {
      digitalWrite(Laser,HIGH);
      angle+=15;
      if (angle>=360) angle=0;
      xNew=iCos[angle];
      yNew=iSin[angle];
      zNew=0;
    }
    // Set ready for ISR
    steps=0;
    
    // Working indicator
    digitalWrite(LED_BUILTIN,!digitalRead(LED_BUILTIN));    
  }
}

AlanX