A simple 3 axis XY plane gCode interpreter

If you have one of these then you will need Grbl to run it.

The above image came from Protoneer so can I suggest having a look at their site.

Not Grbl

This project "Not Grbl" replaces the great Grbl firmware with an inferior partially compatible version.

Implemented:

Assumes:

Immediate codes:

Works with the Arduino serial window (for testing only).

Works well with gcode sender by Otto Hermansson.

Also works okay with Grbl Controller by "Zapmaker".


The Arduino code has the follow defaults

Hardware mapping :

Baud rate:

Stepper/controller settings:

Axis directions:

General Approach

This project uses the "Blink without delay" approach to control the stepper motors.

This avoids the use of hardware specific timer (for interrupts).

In exchange the code is awkward as it has to "pass through" as fast as possible.

Basically code processes each character as it is received.

Code Overview

Setup()

loop () {

// Some LED flasshing code to show it is busy

tokeniseGCode(); // Tokenise a character at a time from the serial stream

parseGCode(); // Parse the code on end of line received

advanceSteppers(); // Advance the stepper motors one step

}

More about the code later.

Serial Control

Uses XON/XOFF to control the serial flow.

But G Code Sender and Grbl Controller wait for an "ok" from "Not Grbl" before sending the next command.

Actually Grbl Controller sends 10 commands before waiting for an "ok" which is no problem.

So "Not Grbl" will reply with an "ok" after every command is sent and interpreted (but usually before the command is executed".

Motion Control

Steppers have a maximum under load (pulse per second (pps) or speed that they can start and stop from without losing steps. For most steppers this is in the order of 500 pps. Not Grbl at the beginning of a movement will ramp up from 500 pps to the specified speed and ramp down to 500 pps before the end of the movement.

The ramps will slow the average speed of short segments so filtering out of these short segments from the gCode will help a lot (particularly with laser over burning).

Maximum Stepper Speed

On my Arduino UNO the "pass through" frequency varied from 12kHz (idle) down to about 8kHz (working). This implies a maximum pulse rate of 8kHz (or 4800mm/min on my 100 steps per MM machine). In reality 2400 mm/min would be a practical maximum.

I don't think many steppers will work to 8kHz anyway.

Immediate Codes

Basically immediate codes (providing they are not in-bedded in comment fields) will executed after the Enter key.

Laser Control

if Tool 0 (i.e. "T0") is added to the top of your G Code then:

Nothing stopping you from using the default tool (i.e. "T1") and using the M3/M5 codes to turn on/off the laser.

The spindle control command (S) is recognised but not implemented.

Hardware Mapping

The current hardware mapping models D2-D8 and D12 at present (see below):

The above image came from Protoneer so can I suggest having a look at their site.

Code

The code has been tested with a Chinese Laser Engrave and works within expectations.

It is about 500 lines long:

// Simple 3 axis XY plane gCode interpreter
// Implemented:
//   G0   Rapid movement
//   G1   Cut movement
//   G92  Set position
//   M0   Stop (pause)
//   M3   Spindle on
//   M5   Spindle off
//   M17  Enable motors
//   M18  Disable motors
//   M30  End program (reset)
//   T0   Laser
//   T1   Engraver (default)
//   T?   Tools (1-255)
//   S?   Spindle (?)
// Assumes:
//   G17  XY plane only
//   G21  Metric
//   G90  Absolute coordinates
//   G94  mm/min
// Immediate codes:
//   !    Pause
//   ~    Resume
//   %    Reset
//   ?    Status   

// Hardware mapping 
#define MXstep         2
#define MYstep         3
#define MZstep         4
#define MXDir          5
#define MYDir          6
#define MZDir          7
#define Enable         8
#define Spindle       12
#define BAUD        9600
#define StepsPerMM   100

// Controller parameters
bool xReverse=false;
bool yReverse=true;
bool zReverse=false;
long tool=1;         // Current tool 1 (tool 0 is a laser)   
long gCode=0;        // Current G Code (rapid movement)
long mCode=5;        // Current M Code (spindle/laser off)
long spindle=16000;  // Spindle RPM
const int bufferSize=64;
char cmd[bufferSize];
long value[bufferSize];
int head=0;
int tail=0;
// Movement parameters
long stepCountDown;  // Movement steps remaining (-1 is movement just completed and 0 is ready for new movement)
long stepCountUp;    // Movement steps completed (-1 is movement just completed and 0 is ready for new movement)
long pause;          // -1 is paused or 0 is not paused
long xNew,yNew,zNew;
long xCurrent,yCurrent,zCurrent;
long feedConstant,feedRate,cutFeedRate,rapidFeedRate;
unsigned long cutFeedInterval,rapidFeedInterval,feedInterval;
long dx,dy,dz,ax,ay,az,sx,sy,sz,mx,my,mz;

// G-Code Tokeniser
  
void tokeniseGCode(void)
{
  static char currentChar=' ';
  static char lastChar=' ';
  static bool readCode=true;
  static char lastCode='%';
  static char number[13]="";
  static bool serialStopped=false;
  static int nptr=0;
  int queue;
  
  queue=head-tail;
  if (queue<0) queue=queue+bufferSize;
  
  // Software serial flow control (XON/XOFF)
  if ((!serialStopped)&&((Serial.available()>48)||(4*queue>bufferSize*3))){
    // XOFF
    Serial.write(0x13);
    serialStopped=true;
  } else if ((serialStopped)&&(Serial.available()<32)&&(2*queue<bufferSize)) {
    // XON
    Serial.write(0x11);
    serialStopped=false;
  }
  
  // Tokenise and pre-process incoming g-Code stream
  if (Serial.available()>0) {
    lastChar=currentChar;
    currentChar=Serial.read();
    // Cover all End Of Line options
    if ((lastChar=='\n')&&(currentChar=='\r')) currentChar=' ';
    if ((lastChar=='\r')&&(currentChar=='\n')) currentChar=' ';
    // Cover comment options
    if (currentChar=='*') readCode=false;                        // Check sum - ignore
    if (currentChar==';') readCode=false;                        // End of line comment
    if (currentChar=='(') readCode=false;                        // Inbedded comment
    if (currentChar==')') readCode=true;                         // End of inbedded comment
    if ((currentChar=='\r')||(currentChar=='\n')) readCode=true; // End of line
    if (readCode) {
      // Check for immediate (non-gCode) commands
      if (currentChar=='%') { // Reset
        readCode=true;
        nptr=0;
        number[0]='\0';
        Serial.println("$ Reset");
        Serial.println("ok"); // Needed here
        Serial.end();
        setup();
      }  else if (currentChar=='!') { // Stop (indefinite pause)
        pause=-1;
        Serial.println("$ Paused");
      } else if (currentChar=='~') { // Resumed
        pause=0;
        Serial.println("$ Resumed");
      } else if (currentChar=='?') { // Status
        Serial.print("$ Status: ");
        if (pause!=0) {
          Serial.println("paused");
        } else {
          if (stepCountDown>0) {
            Serial.println("working");
          } else {
            Serial.println("idle");
          }
        }
        Serial.print("$ Steppers: ");
        if (digitalRead(Enable)==LOW) {
          Serial.println("enabled");
        } else {
          Serial.println("disabled");
        }
        Serial.print("$ Tool: ");
        if (tool==0) {
          Serial.println("laser");
        } else {
          Serial.println(tool);
        }
      }
      // gCodes
      if ((currentChar>='a')&&(currentChar<='z')) currentChar=currentChar-32;
      if ((currentChar>='A')&&(currentChar<='Z')||(currentChar>='0')&&(currentChar<='9')||(currentChar=='-')||(currentChar=='.')||(currentChar=='\r')||(currentChar=='\n')) {
        if ((currentChar>='A')&&(currentChar<='Z')||(currentChar=='\r')||(currentChar=='\n')) {
          // Finish old currentChar (set value)
          if ((lastCode>='A')&&(lastCode<='Z')) {
            if ((lastCode=='X')||(lastCode=='Y')||(lastCode=='Z')) {
              // Convert mm to steps
              if (StepsPerMM==100) {
                {
                  strcat(number,"00");
                  char* tmp=strchr(number,'.');
                  if (tmp!=NULL) {
                    *tmp=*(tmp+1);
                    *(tmp+1)=*(tmp+2);
                    *(tmp+2)='\0';
                  }
                }
                value[head]=atol(number); // 26 us
              } else {  
                value[head]=(long)(atof(number)*StepsPerMM); // 126 us
              }
            } else { 
              value[head]=atol(number);
            }
          }
          // Start new currentChar
          lastCode=currentChar;
          if (++head>=bufferSize) head=head-bufferSize;
          cmd[head]=currentChar;
          value[head]=0;
          nptr=0;
        } else if ((currentChar>='0')&&(currentChar<='9')||(currentChar=='-')||(currentChar=='.')) {
          number[nptr]=currentChar;
          number[++nptr]='\0';
        }
      }
    }
  }
}

void parseGCode(void)
{  
  static bool xFlag=false;
  static bool yFlag=false;
  static bool zFlag=false;
  static bool fFlag=false;
  static bool sFlag=false;
  static long lastGCode=0;

  // Process end of movement codes (mainly mCodes)
  if (stepCountDown==-1) {
    stepCountDown=0;
    stepCountUp=0;
    if (mCode==0) {
      // Stop (pause)
      pause=-1;
    } else if (mCode==5) {
      // Spindle off (laser 0ff)
      digitalWrite(Spindle,LOW);
    } else if (mCode==30) {
      // Reset
      Serial.println("ok"); // Needed here
      Serial.end();
      setup();
    } else {
      // Reserved for expansion  
    }
    mCode=-1; // Reset M Code
  }

  // Process start of movement codes
  if ((stepCountDown==0)&&(head!=tail)&&((cmd[head]=='\r')||(cmd[head]=='\n'))) { 
    tail++;
    if (tail>=bufferSize) tail=tail-bufferSize;
    if ((cmd[tail]=='\r')||(cmd[tail]=='\n')) Serial.println("ok");
  
    // Decode G,X,Y,Z,F,S,T
    if (cmd[tail]=='G') {
      lastGCode=gCode;
      gCode=value[tail];
      xFlag=false;
      yFlag=false;
      zFlag=false;
      fFlag=false;
    } else if (cmd[tail]=='X') {
      xNew=value[tail];
      xFlag=true;
    } else if (cmd[tail]=='Y') {
      yNew=value[tail];
      yFlag=true;
      } else if (cmd[tail]=='Z') {
      zNew=value[tail];
      zFlag=true;
    } else if (cmd[tail]=='F') {
      feedRate=value[tail];
      if (feedRate<=0) feedRate=1;
      feedInterval=feedConstant/feedRate;
      fFlag=true;
    } else if (cmd[tail]=='S') {
      spindle=value[tail];
      if (spindle<0) spindle=0;
    } else if (cmd[tail]=='T') {
      tool=value[tail];
      if (tool<0) tool=0;
      if (tool>255) tool=255;
    }
    
    // Start of movement mCodes
    if (cmd[tail]=='M') {
      mCode=value[tail];
      if (mCode==3) {
        // Spindle on (laser on)
        digitalWrite(Spindle,HIGH);
      } else if (mCode==17) {
        // Enable (all motors)
        digitalWrite(Enable,LOW);
      } else if (mCode==18) {
        // Disable (all motors)
        digitalWrite(Enable,HIGH);
      } else {
        // Reserved for expansion
      }
    } 
        
    // Execute
    if ((cmd[tail]=='\r')||(cmd[tail]=='\n')) {
      if (gCode==0) {
        // Rapid movement
        if (fFlag) {
          rapidFeedRate=feedRate;
          rapidFeedInterval=feedInterval;
        } else {
          feedRate=rapidFeedInterval;
          feedInterval=rapidFeedInterval;
        }
      } else if (gCode==1) {
        // Cutting movement
        if (fFlag) {
          cutFeedRate=feedRate;
          cutFeedInterval=feedInterval;
        } else {
          feedRate=cutFeedInterval;
          feedInterval=cutFeedInterval;
        }
      } else if (gCode==92) {
        // Set current position
        if (xFlag) xCurrent=xNew;
        if (yFlag) yCurrent=yNew;
        if (zFlag) zCurrent=zNew;
        gCode=lastGCode;
      } else {          
        // Reserved for expansion
      } 
      xFlag=false;
      yFlag=false;
      zFlag=false;
      fFlag=false;
      
      // Laser
      if (tool==0) {
        zNew=zCurrent;                            // Laser cannot change Z
        if (gCode==1) digitalWrite(Spindle,HIGH);  // Laser off for rapid moves
        if (gCode==0) digitalWrite(Spindle,LOW); // Laser on for cutting moves
      }

      // Determine movement parameters
      dx=xNew-xCurrent;
      dy=yNew-yCurrent;
      dz=zNew-zCurrent;
      ax=abs(dx);
      ay=abs(dy);
      az=abs(dz);
      // Execution parameters
      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);
        stepCountDown=ax;
      } else if ((ay>=ax)&&(ay>=az)) {
        mx=ax-(ay>>1);
        my=0;
        mz=az-(ay>>1);
        stepCountDown=ay;
      } else {
        mx=ax-(az>>1);
        my=ay-(az>>1);
        mz=0;
        stepCountDown=az;
      }
      if (stepCountDown>0) {
        stepCountUp=1;
      } else {
        stepCountDown=-1;
        stepCountUp=-1;        
      }
      // Set the stepper directions
      if (xReverse) {
        digitalWrite(MXDir,(1-sx)>>1);
      } else {
        digitalWrite(MXDir,(sx+1)>>1);
      } 
      if (yReverse) {
        digitalWrite(MYDir,(1-sy)>>1);
      } else {
        digitalWrite(MYDir,(sy+1)>>1);
      }
      if (zReverse) {
        digitalWrite(MZDir,(1-sz)>>1);
      } else {
        digitalWrite(MZDir,(sz+1)>>1);
      }
    }
  }
}

void advanceSteppers(void)
{ 
  static bool cycleFlag=false;
  static unsigned long feedPreviousMicros=0;
  static unsigned long feedCurrentMicros=0;
  long stepX,stepY,stepZ;
  unsigned long ramp;

  if ((pause==0)&&(stepCountDown>0)) {
    // Maximum 1g ramp and 500pps (pull-in)
    ramp=400*max(max(6-stepCountUp,0),max(6-stepCountDown,0));
    feedCurrentMicros=micros();
    if (feedCurrentMicros-feedPreviousMicros>feedInterval+ramp) {
      feedPreviousMicros=feedCurrentMicros;
      // 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 pulse (high for at least 10 us)
      digitalWrite(MXstep,abs(stepX));
      digitalWrite(MYstep,abs(stepY));
      digitalWrite(MZstep,abs(stepZ));
      delayMicroseconds(10);
      digitalWrite(MXstep,LOW);
      digitalWrite(MYstep,LOW);
      digitalWrite(MZstep,LOW);
      stepCountDown--;
      stepCountUp++;
      // Flag end of movement 
      if (stepCountDown==0) {
        stepCountDown=-1;
        stepCountUp=-1;
      }
    }
  } else {
    feedPreviousMicros=micros(); // Reset feed rate clock to ensure near full cycle on start
  }
}

void setup()
{
  // Reset Global Variables
  tool=1;
  gCode=0;  
  mCode=5;    
  spindle=16000;
  pause=0;
  head=0;
  tail=0;
  cmd[0]='\n';
  xNew=0;
  yNew=0;
  zNew=0;
  xCurrent=0;
  yCurrent=0;
  zCurrent=0;
  stepCountDown=0;
  stepCountUp=0;
  feedRate=1200;
  cutFeedRate=300;
  rapidFeedRate=1200;
  feedConstant=60000000/StepsPerMM;
  cutFeedInterval=feedConstant/cutFeedRate;
  rapidFeedInterval=feedConstant/rapidFeedRate;
  feedInterval=rapidFeedInterval;
  // Initialise H-Bridge
  pinMode(MXDir,OUTPUT);
  pinMode(MXstep,OUTPUT);
  pinMode(MYDir,OUTPUT);
  pinMode(MYstep,OUTPUT);
  pinMode(MZDir,OUTPUT);
  pinMode(MZstep,OUTPUT);
  pinMode(Enable,OUTPUT);
  pinMode(Spindle,OUTPUT);
  digitalWrite(MXDir,LOW);
  digitalWrite(MXstep,LOW);
  digitalWrite(MYDir,LOW);
  digitalWrite(MYstep,LOW);
  digitalWrite(MZDir,LOW);
  digitalWrite(MZstep,LOW);
  digitalWrite(Enable,LOW);
  digitalWrite(Spindle,LOW);
  // Initialise LED
  pinMode(LED_BUILTIN,OUTPUT);
  digitalWrite(LED_BUILTIN,LOW);
  // Initialize serial:
  Serial.begin(BAUD,SERIAL_8N1);
  Serial.println("Grbl v0.1a");
}

void loop()
{
  static unsigned long intervalMillisLED=0;
  static unsigned long currentMillisLED=0;
  static unsigned long previousMillisLED=0;

  // Use LED to indicate working
  if (stepCountDown==0) {
    intervalMillisLED=1000;
  } else {
    intervalMillisLED=100;    
  }
  currentMillisLED=millis();
  if (currentMillisLED-previousMillisLED>intervalMillisLED) {
    previousMillisLED=currentMillisLED;   
    digitalWrite(LED_BUILTIN,!digitalRead(LED_BUILTIN));
  }
  // Process the serial stream
  tokeniseGCode();   // Tokenise a character at a time from the serial stream
  parseGCode();      // Parse the code on end of line received
  advanceSteppers(); // Advance the stepper motors one step
}