src/input/gcode.c

Go to the documentation of this file.

/*
 * gcode.c
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "gcode.h"

#include "yarf.h"
#include "commands.h"

#include <math.h>
#include <stdbool.h>
#include <string.h>

#define N_SEEN     _BV(0)
#define CODE_SEEN  _BV(1)
#define X_SEEN     _BV(2)
#define Y_SEEN     _BV(3)
#define Z_SEEN     _BV(4)
#define E_SEEN     _BV(5)
#define F_SEEN     _BV(6)
#define S_SEEN     _BV(7)
#define CS_SEEN    _BV(8)
#define COORD_SEEN (X_SEEN | Y_SEEN | Z_SEEN | E_SEEN | F_SEEN | S_SEEN)

static FILE *host;
static long expected_line_number;

typedef struct {
  int16_t fields;    
  char letter;       
  int number;        
  long line_number;  
  float x;           
  float y;           
  float z;           
  float e;           
  float f;           
  float s;           
} gcode_command;


static int
get_signed_long(char *s, long *l)
{
  int nb_chars = 0;
  int stat = sscanf(s, "%ld%n", l, &nb_chars);
  if (stat != 1) {
    return stat;
  }

  return nb_chars;
}

static int
get_signed_int(char *s, int *i)
{
  int nb_chars = 0;
  int stat = sscanf(s, "%d%n", i, &nb_chars);
  if (stat != 1) {
    return stat;
  }

  return nb_chars;
}

static int
get_uint8_t(char *s, uint8_t *u)
{
  unsigned int i;
  int nb_chars = 0;
  int stat = sscanf(s, "%u%n", &i, &nb_chars);
  if (stat != 1) {
    return stat;
  }

  if (i > 255) {
    return -1;
  }

  *u = (uint8_t)i;
  return nb_chars;
}


static int
get_float(char *s, float *f)
{
  int nb_chars = 0;
  int stat = sscanf(s, "%f%n", f, &nb_chars);
  if (stat != 1) {
    return stat;
  }

  return nb_chars;
}

static unsigned char
count_whitespace(char *s)
{
  unsigned char i = 0;
  while(*s == ' '  ||
        *s == '\t' ||
        *s == '\n' ||
        *s == '\r' ||
        *s == '\t') {
    s += 1;
    i += 1;
  }

  return i;
}

static void
clear_command(gcode_command *cmd) {
  cmd->fields = 0;
  cmd->letter = 0;
  cmd->number = 0;
  cmd->line_number = 0;
  cmd->x = NAN;
  cmd->y = NAN;
  cmd->z = NAN;
  cmd->e = NAN;
  cmd->f = NAN;
  cmd->s = NAN;
}

static uint8_t
checksum(char *s, size_t length)
{
  uint8_t cs = 0;
  unsigned char i;
  while (i < length && s[i] != '*' && s[i] != 0) {
    cs = cs ^ s[i];
    i += 1;
  }
  return cs;
}

static int
parse_command(gcode_command *cmd)
{
  char buffer[GCODE_BUFFER_SIZE];
  if (fgets(buffer, GCODE_BUFFER_SIZE, host) == NULL) {
    /* An error was encountered while reading the input */
    return GCODE_PARSE_COMMAND_READ_FAILURE;
  }
  //fprintf(host, "// got: %s", buffer);

  size_t cmd_length = strlen(buffer);
  
  /* Skip any whitespace */
  unsigned char cur_pos = count_whitespace(buffer);
  if (cur_pos == cmd_length) {
    return GCODE_PARSE_COMMAND_EMPTY;
  }

  clear_command(cmd);
  int16_t fields = 0;
  int nb_chars;
  /* Parse part of command based on letter */  
  while (cur_pos < cmd_length) {
    switch (buffer[cur_pos]) {
      case 'N':
        /* Line number */
        if (fields & (N_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= N_SEEN;
        nb_chars = get_signed_long(&buffer[cur_pos], &(cmd->line_number));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case 'G':
      case 'M':
      case 'T':
        /* Command code */
        if (fields & (CODE_SEEN | COORD_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cmd->letter = buffer[cur_pos];
        cur_pos += 1;
        fields |= CODE_SEEN;
        nb_chars = get_signed_int(&buffer[cur_pos], &(cmd->number));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case 'X':
        if (fields & (X_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= X_SEEN;
        nb_chars = get_float(&buffer[cur_pos], &(cmd->x));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case 'Y':
        if (fields & (Y_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= Y_SEEN;
        nb_chars = get_float(&buffer[cur_pos], &(cmd->y));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case 'Z':
        if (fields & (Z_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= Z_SEEN;
        nb_chars = get_float(&buffer[cur_pos], &(cmd->z));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case 'E':
        if (fields & (E_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= E_SEEN;
        nb_chars = get_float(&buffer[cur_pos], &(cmd->e));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case 'F':
        if (fields & (F_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= F_SEEN;
        nb_chars = get_float(&buffer[cur_pos], &(cmd->f));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case 'S':
        if (fields & (S_SEEN | CS_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= S_SEEN;
        nb_chars = get_float(&buffer[cur_pos], &(cmd->s));
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        break;
      case '*':
        if (fields & CS_SEEN || !(fields & N_SEEN)) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += 1;
        fields |= CS_SEEN;
        uint8_t received_cs = 0;
        nb_chars = get_uint8_t(&buffer[cur_pos], &received_cs);
        if (nb_chars <= 0) {
          return GCODE_PARSE_COMMAND_SYNTAX_ERROR;
        }
        cur_pos += nb_chars;
        if (received_cs != checksum(buffer, cmd_length)) {
          return GCODE_PARSE_COMMAND_CHECKSUM_FAILED;
        }
        break;
      default:
        return GCODE_PARSE_COMMAND_UNEXPECTED_CHARACTER;
    }
    cur_pos += count_whitespace(&buffer[cur_pos]);
  }
  cmd->fields = fields;
  return GCODE_PARSE_COMMAND_SUCCESS;
}

static inline void
send_resend(unsigned long line_number)
{
  fprintf(host,"rs %ld\n",line_number);
}


static inline void
send_reply(gcode_command *cmd, cmd_response_t *r)
{
  switch (r->status) {
    case OK:
      fputs("ok",host);
      if (! isnan(r->T)) { // Conversion to double is to prevent compiler warning (GCC Bug 35649)
        fprintf(host, " T:%.1f",(double)r->T);
      }
      if (! isnan(r->B)) {
        fprintf(host, " B:%.1f",(double)r->B);
      }
      if (! isnan(r->X)) {
        fprintf(host, " X:%.2f",(double)r->X);
      }
      if (! isnan(r->Y)) {
        fprintf(host, " Y:%.2f",(double)r->Y);
      }
      if (! isnan(r->Z)) {
        fprintf(host, " Z:%.2f",(double)r->Z);
      }
      if (! isnan(r->E)) {
        fprintf(host, " E:%.2f",(double)r->E);
      }
      if (r->info != NULL) {
        fputs(" // ",host);
        fputs(r->info,host);
      }
      break;
    case INVALID:
      if (cmd->fields & N_SEEN) {
        fprintf(host,"rs %ld ",cmd->line_number);
      } else {
        fputs("ok ",host);
      }
      fprintf(host,"// command '%c%d' failed", cmd->letter, cmd->number);
      if (r->info != NULL) {
        fputs(": ",host);
        fputs(r->info,host);
      }
      break;
    case FAULT:
      fputs("!!",host);
      if (r->info != NULL) {
        fputs(" // ",host);
        fputs(r->info,host);
      }
      break;
  }
  fputs("\n",host);
}

static void
command_unknown(cmd_response_t *r)
{
  r->status = INVALID;
  r->info = "unknown command";
}

static inline void
dispatch_command(gcode_command *cmd, cmd_response_t *r)
{
  switch (cmd->letter) {
    case 'G':
      switch (cmd->number) {
        case 1:
          /* G1: Controlled move */
          cmd_controlled_move(r, cmd->x, cmd->y, cmd->z, cmd->e, cmd->f);
          break;
        case 20:
          /* G20: Set Units to Inches */
          cmd_set_units_inches(r);
          break;
        case 21:
          /* G21: Set Units to Millimeters */
          cmd_set_units_millimeters(r);
          break;
        case 28:
          /* G28: Move to origin */
          cmd_move_to_origin(r,
            !isnan(cmd->x),
            !isnan(cmd->y),
            !isnan(cmd->z));
          break;
        case 90:
          /* G90: Set to absolute positioning */
          cmd_set_absolute_positioning(r);
          break;
        case 91:
          /* G91: Set to relative positioning */
          cmd_set_relative_positioning(r);
          break;
        case 92:
          /* G92: Set Position */
          cmd_set_position(r, cmd->x, cmd->y, cmd->z, cmd->e);
          break;
        default:
          command_unknown(r);
          break;
      }
      break;
    case 'M':
      switch (cmd->number) {
        case 84:
          /* M84: Stop idle hold */
          cmd_stop_idle_hold(r);
          break;
        case 104:
          /* M104: Set Extruder Temperature (Fast) */
          cmd_set_extruder_temp_async(r, cmd->s);
          break;
        case 105:
          /* M105: Get extruder and build base temperature */
          cmd_read_nozzle_and_printbed_temp(r);
          break;
        case 109:
          /* M109: Set Extruder Temperature */
          cmd_set_extruder_temp_sync(r, cmd->s);
          break;
        case 113:
          /* M113: Set Extruder PWM */
          cmd_set_extruder_pwm(r, cmd->s);
          break;
        case 114:
          /* M114: Get Current Position */
          cmd_get_position(r);
          break;
        case 116:
          /* M116: Wait */
          cmd_wait(r);
          break;
        case 140:
          /* M140: Set Bed Temperature (Fast) */
          cmd_set_printbed_temp_async(r, cmd->s);
          break;
        default:
         command_unknown(r);
         break;
      }
      break;
    case 'T':
      /* T: tool select */
      cmd_select_tool(r, cmd->number);
      break;
    default:
      command_unknown(r);
      break;
  }
}

void
gcode_init(FILE *host_stream) {
  host = host_stream;
  expected_line_number = 0;
  cmd_init();
}

int
gcode_process_command()
{
  gcode_command cmd;
  int parse_result = parse_command(&cmd);
  if (parse_result != GCODE_PARSE_COMMAND_SUCCESS) {
    send_resend(expected_line_number);
    return parse_result;
  }
  
  // At this point parsing has succeeded and the checksum is correct
  if (cmd.fields & N_SEEN) {
    if (cmd.letter == 'M' && cmd.number == 110) {
      expected_line_number = cmd.line_number + 1;
      fputs("ok\n",host);
      return GCODE_PROCESS_COMMAND_SUCCESS;
    } else if (cmd.line_number != expected_line_number) {
      send_resend(expected_line_number);
      return GCODE_UNEXPECTED_LINE;
    }
    expected_line_number += 1;
  }
  
  // At this point the line number is correct as well
  cmd_response_t r;
  dispatch_command(&cmd,&r);
  send_reply(&cmd, &r);
  
  return GCODE_PROCESS_COMMAND_SUCCESS;
}