Close

Adding 3D Line to the ISR

A project log for Motion Controller

An update to "not GRBL" but using an ISR.

agp.cooperagp.cooper 01/14/2018 at 05:060 Comments

Adding 3D Line to the ISR

Okay, it get complicated quickly but this is really only the beginning!

/*
  Simple 3 Axis ISR Motion Controller - Part 2: Add Motion Control 
  ================================================================
  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).

  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 xReverse   false        // Reverse X axis direction
#define yReverse    true        // Reverse Y axis direction
#define zReverse   false        // Reverse Z axis direction

// Motion controller and ISR variables
volatile int xNew;  // The target X co-ordinate
volatile int yNew;  // The target Y co-ordinate
volatile int zNew;  // The target Z co-ordinate
volatile int feed;  // Set motion feed rate
volatile int laser; // Laser (on/off)
volatile int xCurrent=0;       // The current X co-ordinate
volatile int yCurrent=0;       // The current Y co-ordinate
volatile int zCurrent=0;       // The current Z co-ordinate
volatile int steps=-1;         // Number of steps remaining in motion
ISR(TIMER2_OVF_vect)
{
  static int dx,dy,dz;
  static int ax,ay,az;
  static int sx,sy,sz;
  static int mx,my,mz;
  static int stepX,stepY,stepZ;
  static unsigned int phase=0;
  static unsigned int magic=8000;
  if (phase<0x8000) {
    phase+=magic;
    if (phase>=0x8000) {
      if (steps==0) {
        // Determine next movement parameters
        dx=xNew-xCurrent;
        dy=yNew-yCurrent;
        dz=zNew-zCurrent;
        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);
        } else {
          digitalWrite(DirX,(sx+1)>>1);
        } 
        if (yReverse) {
          digitalWrite(DirY,(1-sy)>>1);
        } else {
          digitalWrite(DirY,(sy+1)>>1);
        }
        if (zReverse) {
          digitalWrite(DirZ,(1-sz)>>1);
        } else {
          digitalWrite(DirZ,(sz+1)>>1);
        }
        // Set laser
        if (laser>0) {
          digitalWrite(Laser,HIGH);
        } else {
          digitalWrite(Laser,LOW);
        }
        // Set feed
        magic=feed<<3;
      }
      // Reset step low
      digitalWrite(StepX,LOW);
      digitalWrite(StepY,LOW);
      digitalWrite(StepZ,LOW);
    }
  } 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) digitalWrite(StepX,HIGH);
        if (stepY!=0) digitalWrite(StepY,HIGH);
        if (stepZ!=0) digitalWrite(StepZ,HIGH);
        steps--;
        // Set steps to indicate new motion not set
        if (steps==0) steps=-1;
      }
    }
  }
}

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))); 
  }

  // Give some time for mechanical set up
  delay(5000);
}

void loop()
{
  /* Motion Command */
  // Used to keep track of motion
  static int sinAngle=-1;
  static int cosAngle=0;
  
  // if last motion complete
  if (steps==-1) {
    if (sinAngle==-1) {
      sinAngle=0;
      cosAngle=90;
      cli();
      xNew=iSin[cosAngle];
      yNew=iSin[sinAngle];
      zNew=0;
      feed=1000;
      laser=0; // Laser off
      sei();
    } else if (sinAngle==-2) {
      cli();
      xNew=0;
      yNew=0;
      zNew=0;
      feed=1000;
      laser=0; // Laser off
      sei();
    } else {
      sinAngle+=1;
      cosAngle+=1;
      if (sinAngle>=360) sinAngle-=360;
      if (cosAngle>=360) cosAngle-=360;
      cli();
      xNew=iSin[cosAngle];
      yNew=iSin[sinAngle];
      zNew=0;
      feed=1000;
      laser=1; // Laser on
      sei();
      if (sinAngle==0) sinAngle=-2;
    }
    // Set steps to indicate a new motion has been set
    steps=0;
  }
}

This code is a variation of the Turret Scanner but simplified a little. I have also used another variation on my Laser CNC machine (with the pin mappings adjusted for the Laser board) testing different feed rates:

As you can see it works okay.

Note the gap at the top of the circle for the faster feed rates (lighter burns). They are not due to a code error. It is just that lasers need time to start a burn but less time to maintain a burn.

AlanX

Discussions