commit 3f4d63ea58373d2f6ffa194d5178ce52a7e5b994
parent 6f5b5eb66a3dfc977d7df95b443b2e2b13ae4021
Author: Christian Ermann <christianermann@gmail.com>
Date: Tue, 22 Oct 2024 22:33:27 -0700
Add gcode interpreter
Diffstat:
| A | .gitignore | | | 1 | + |
| A | LICENSE | | | 19 | +++++++++++++++++++ |
| A | Makefile | | | 11 | +++++++++++ |
| A | include/command.h | | | 43 | +++++++++++++++++++++++++++++++++++++++++++ |
| A | include/machine.h | | | 91 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | include/timing.h | | | 22 | ++++++++++++++++++++++ |
| A | include/token.h | | | 56 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | include/tui.h | | | 29 | +++++++++++++++++++++++++++++ |
| A | src/command.c | | | 67 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | src/machine.c | | | 297 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | src/main.c | | | 223 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | src/timing.c | | | 62 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | src/token.c | | | 309 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | src/tui.c | | | 62 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 files changed, 1292 insertions(+), 0 deletions(-)
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1 @@
+*.gcode
diff --git a/LICENSE b/LICENSE
@@ -0,0 +1,19 @@
+Copyright (c) 2024 Christian Ermann
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/Makefile b/Makefile
@@ -0,0 +1,11 @@
+all:
+ gcc src/* -Iinclude -lm -o gcode-interpreter
+
+run: all
+ ./gcode-interpreter
+
+.PHONY: clean
+clean:
+ rm gcode-interpreter
+ rm error.log
+ rm out
diff --git a/include/command.h b/include/command.h
@@ -0,0 +1,43 @@
+#ifndef COMMAND_H
+#define COMMAND_H
+
+#include "token.h"
+
+#include <stdbool.h>
+
+typedef enum {
+ CODE_G = TOKEN_G_CODE,
+ CODE_M = TOKEN_M_CODE,
+} CodeType;
+
+typedef struct {
+ bool is_set;
+ CodeType type;
+ unsigned int value;
+} Code;
+
+typedef struct {
+ bool is_set;
+ float value;
+} Param;
+
+typedef struct {
+ Code code;
+ Param param_s;
+ Param param_x;
+ Param param_y;
+ Param param_z;
+ Param param_e;
+ Param param_f;
+} Command;
+
+typedef enum {
+ COMMAND_STATUS__UNRECOGNIZED_TOKEN,
+ COMMAND_STATUS__NOT_READY,
+ COMMAND_STATUS__READY,
+} CommandStatus;
+
+void command_init(Command* command);
+CommandStatus command_process_token(Command* command, const Token* token);
+
+#endif
diff --git a/include/machine.h b/include/machine.h
@@ -0,0 +1,91 @@
+#ifndef MACHINE_H
+#define MACHINE_H
+
+#include "command.h"
+#include "timing.h"
+
+typedef enum {
+ UNITS_MM,
+ UNITS_INCHES,
+} Units;
+
+static const char* UNITS_STRINGS[] = {
+ "mm",
+ "in",
+};
+
+typedef enum {
+ POSITIONING_MODE_ABSOLUTE,
+ POSITIONING_MODE_RELATIVE,
+} PositioningMode;
+
+static const char* POSITIONING_MODE_STRINGS[] = {
+ "absolute",
+ "relative",
+};
+
+typedef enum {
+ EXECUTE_STATUS__UNRECOGNIZED_COMMAND,
+ EXECUTE_STATUS__UNRECOGNIZED_GCODE,
+ EXECUTE_STATUS__UNRECOGNIZED_MCODE,
+ EXECUTE_STATUS__IDLE,
+ EXECUTE_STATUS__IN_PROGRESS,
+ EXECUTE_STATUS__FINISHED,
+} ExecutionStatus;
+
+static const char* MACHINE_ERROR_STRINGS[] = {
+ "command unrecognized.",
+ "gcode unrecognized.",
+ "mcode unrecognized.",
+};
+
+typedef struct {
+ float x;
+ float y;
+ float z;
+} ControlData;
+
+typedef struct {
+ ExecutionStatus status;
+ bool has_data;
+ ControlData data;
+} ExecutionResult;
+
+typedef enum {
+ MOVE_NONE,
+ MOVE_LINEAR,
+} MoveType;
+
+typedef struct {
+ float start;
+ float distance;
+} AxisStatus;
+
+typedef struct {
+ MoveType type;
+ Time start;
+ Time end;
+ Time duration;
+ AxisStatus x;
+ AxisStatus y;
+ AxisStatus z;
+} Move;
+
+typedef struct {
+ float x;
+ float y;
+ float z;
+ float feed_rate;
+ Units units;
+ PositioningMode positioning_mode;
+ Move current_move;
+} Machine;
+
+void machine_init(Machine* machine);
+ExecutionResult machine_execute_command(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+);
+
+#endif
diff --git a/include/timing.h b/include/timing.h
@@ -0,0 +1,22 @@
+#ifndef TIMING_H
+#define TIMING_H
+
+#include <stdbool.h>
+
+typedef struct {
+ long time_s;
+ long time_ns;
+} Time;
+
+Time time_make(long time_s, long time_ns);
+Time time_zero();
+Time time_get();
+
+Time time_add(const Time* t0, const Time* t1);
+Time time_sub(const Time* t0, const Time* t1);
+float time_div(const Time* t0, const Time* t1);
+
+int time_cmp(const Time* t0, const Time* t1);
+bool time_eql_zero(const Time* t);
+
+#endif
diff --git a/include/token.h b/include/token.h
@@ -0,0 +1,56 @@
+#ifndef TOKEN_H
+#define TOKEN_H
+
+#include <stdio.h>
+
+typedef struct {
+ FILE* file;
+ unsigned int line_idx;
+ unsigned int char_idx;
+} Cursor;
+
+void cursor_init(Cursor* cursor, FILE* file);
+char cursor_getc(Cursor* cursor);
+void cursor_ungetc(Cursor* cursor, char c);
+
+static const char* TOKEN_ERROR_STRINGS[] = {
+ "field value exceeded max length.",
+ "invalid character. unable to parse integer.",
+ "no digits found. unable to parse integer.",
+ "invalid character. unable to parse float.",
+ "no digits found. unable to parse float.",
+ "field type does not support value parsing.",
+ "field type unrecognized.",
+};
+
+typedef enum {
+ TOKEN_COMMENT = ';',
+ TOKEN_G_CODE = 'G',
+ TOKEN_M_CODE = 'M',
+ TOKEN_S_PARAM = 'S',
+ TOKEN_X_PARAM = 'X',
+ TOKEN_Y_PARAM = 'Y',
+ TOKEN_Z_PARAM = 'Z',
+ TOKEN_F_PARAM = 'F',
+ TOKEN_E_PARAM = 'E',
+ TOKEN_EOF = EOF,
+ TOKEN_ERROR = '!',
+} TokenType;
+
+typedef union {
+ float param_value;
+ unsigned int code_value;
+ unsigned int error_value;
+} TokenValue;
+
+typedef struct {
+ TokenType type;
+ TokenValue value;
+ unsigned int line_idx;
+ unsigned int char_idx;
+} Token;
+
+Token parse_token(Cursor* cursor);
+void recover_from_error(Cursor* cursor, const Token* token);
+
+#endif
diff --git a/include/tui.h b/include/tui.h
@@ -0,0 +1,29 @@
+#ifndef TUI_H
+#define TUI_H
+
+#include <unistd.h>
+
+#define type(str) write(1, str, sizeof(str))
+
+#define STR_(x) #x
+#define STR(x) STR_(x)
+
+#define esc "\x1b"
+#define csi esc "["
+
+#define switch_to csi "?1049"
+#define main_buffer "h"
+#define alternate_buffer "l"
+
+#define clear_buffer csi "2J"
+#define clear_line csi "2K"
+#define clear_to_end_of_line csi "0K"
+
+#define hide_cursor csi "?25l"
+#define show_cursor csi "?25h"
+#define set_cursor(n, m) csi STR(n) ";" STR(m) "H"
+
+void initialize_terminal();
+void type_line(const char* msg, unsigned int line);
+
+#endif
diff --git a/src/command.c b/src/command.c
@@ -0,0 +1,67 @@
+#include "command.h"
+
+void command_init(Command* command) {
+ command->code.is_set = false;
+ command->param_s.is_set = false;
+ command->param_x.is_set = false;
+ command->param_y.is_set = false;
+ command->param_z.is_set = false;
+ command->param_e.is_set = false;
+ command->param_f.is_set = false;
+}
+
+CommandStatus command_process_token(Command* command, const Token* token) {
+ switch (token->type) {
+ case TOKEN_M_CODE: {
+ if (command->code.is_set) {
+ return COMMAND_STATUS__READY;
+ }
+ command->code.is_set = true;
+ command->code.type = CODE_M;
+ command->code.value = token->value.code_value;
+ break;
+ }
+ case TOKEN_G_CODE: {
+ if (command->code.is_set) {
+ return COMMAND_STATUS__READY;
+ }
+ command->code.is_set = true;
+ command->code.type = CODE_G;
+ command->code.value = token->value.code_value;
+ break;
+ }
+ case TOKEN_S_PARAM: {
+ command->param_s.is_set = true;
+ command->param_s.value = token->value.param_value;
+ break;
+ }
+ case TOKEN_X_PARAM: {
+ command->param_x.is_set = true;
+ command->param_x.value = token->value.param_value;
+ break;
+ }
+ case TOKEN_Y_PARAM: {
+ command->param_y.is_set = true;
+ command->param_y.value = token->value.param_value;
+ break;
+ }
+ case TOKEN_Z_PARAM: {
+ command->param_z.is_set = true;
+ command->param_z.value = token->value.param_value;
+ break;
+ }
+ case TOKEN_E_PARAM: {
+ command->param_e.is_set = true;
+ command->param_e.value = token->value.param_value;
+ break;
+ }
+ case TOKEN_F_PARAM: {
+ command->param_f.is_set = true;
+ command->param_f.value = token->value.param_value;
+ break;
+ }
+ default:
+ return COMMAND_STATUS__UNRECOGNIZED_TOKEN;
+ }
+ return COMMAND_STATUS__NOT_READY;
+}
diff --git a/src/machine.c b/src/machine.c
@@ -0,0 +1,297 @@
+#include "machine.h"
+
+#include <math.h>
+
+#define DEFAULT_FEED_RATE 3000 // 3000 mm/m = 50 mm/s
+#define DEFAULT_UNITS UNITS_MM
+#define DEFAULT_POSITIONING_MODE POSITIONING_MODE_ABSOLUTE
+
+ExecutionResult machine_execute_m_code(Machine* machine, const Command* command);
+ExecutionResult machine_execute_g_code(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+);
+
+ExecutionResult machine_execute_g1(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+);
+ExecutionResult machine_execute_g21(Machine* machine, const Command* command);
+ExecutionResult machine_execute_g90(Machine* machine, const Command* command);
+ExecutionResult machine_execute_g91(Machine* machine, const Command* command);
+ExecutionResult machine_execute_g92(Machine* machine, const Command* command);
+
+void machine_init(Machine* machine) {
+ machine->x = 0;
+ machine->y = 0;
+ machine->z = 0;
+ machine->feed_rate = DEFAULT_FEED_RATE;
+ machine->units = DEFAULT_UNITS;
+ machine->positioning_mode = DEFAULT_POSITIONING_MODE;
+ machine->current_move.type = MOVE_NONE;
+}
+
+ExecutionResult machine_execute_command(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+) {
+ switch (command->code.type) {
+ case CODE_M:
+ return machine_execute_m_code(machine, command);
+ case CODE_G:
+ return machine_execute_g_code(machine, command, now);
+ default: {
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__UNRECOGNIZED_COMMAND,
+ .has_data = false,
+ };
+ return result;
+ }
+ }
+}
+
+ExecutionResult machine_execute_m_code(
+ Machine* machine,
+ const Command* command
+) {
+ switch (command->code.value) {
+ default: {
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__UNRECOGNIZED_MCODE,
+ .has_data = false,
+ };
+ return result;
+ }
+ }
+}
+
+ExecutionResult machine_execute_g_code(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+) {
+ switch (command->code.value) {
+ case 0:
+ case 1:
+ return machine_execute_g1(machine, command, now);
+ case 21:
+ return machine_execute_g21(machine, command);
+ case 90:
+ return machine_execute_g90(machine, command);
+ case 91:
+ return machine_execute_g91(machine, command);
+ case 92:
+ return machine_execute_g92(machine, command);
+ default: {
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__UNRECOGNIZED_GCODE,
+ .has_data = false,
+ };
+ return result;
+ }
+ }
+}
+
+float x_distance(Machine* machine, const Command* command) {
+ if (!command->param_x.is_set) {
+ return 0;
+ }
+ switch (machine->positioning_mode) {
+ case POSITIONING_MODE_ABSOLUTE:
+ return command->param_x.value - machine->x;
+ case POSITIONING_MODE_RELATIVE:
+ return command->param_x.value;
+ }
+}
+
+float y_distance(Machine* machine, const Command* command) {
+ if (!command->param_y.is_set) {
+ return 0;
+ }
+ switch (machine->positioning_mode) {
+ case POSITIONING_MODE_ABSOLUTE:
+ return command->param_y.value - machine->y;
+ case POSITIONING_MODE_RELATIVE:
+ return command->param_y.value;
+ }
+}
+
+float z_distance(Machine* machine, const Command* command) {
+ if (!command->param_z.is_set) {
+ return 0;
+ }
+ switch (machine->positioning_mode) {
+ case POSITIONING_MODE_ABSOLUTE:
+ return command->param_z.value - machine->z;
+ case POSITIONING_MODE_RELATIVE:
+ return command->param_z.value;
+ default:
+ return 0;
+ }
+}
+
+Time g1_execute_duration(Machine* machine, const Command* command) {
+ float dx = x_distance(machine, command);
+ float dy = y_distance(machine, command);
+ float dz = z_distance(machine, command);
+ float d = sqrtf((dx * dx) + (dy * dy) + (dz * dz));
+ float t = 60.0f * d / machine->feed_rate;
+ return time_make(0, (long)round(t * 1e9));
+}
+
+ExecutionResult machine_execute_g1_start(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+) {
+ if (command->param_f.is_set) {
+ machine->feed_rate = command->param_f.value;
+ }
+ machine->current_move.type = MOVE_LINEAR;
+ machine->current_move.start = *now;
+ machine->current_move.duration = g1_execute_duration(machine, command);
+ machine->current_move.end = time_add(now, &machine->current_move.duration);
+ machine->current_move.x.start = machine->x;
+ machine->current_move.y.start = machine->y;
+ machine->current_move.z.start = machine->z;
+ machine->current_move.x.distance = x_distance(machine, command);
+ machine->current_move.y.distance = y_distance(machine, command);
+ machine->current_move.z.distance = z_distance(machine, command);
+ if (time_eql_zero(&machine->current_move.duration)) {
+ machine->current_move.type = MOVE_NONE;
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__FINISHED,
+ .has_data = false,
+ };
+ return result;
+ }
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__IN_PROGRESS,
+ .has_data = false,
+ };
+ return result;
+}
+
+ExecutionResult machine_execute_g1_end(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+) {
+ machine->x = machine->current_move.x.start + machine->current_move.x.distance;
+ machine->y = machine->current_move.y.start + machine->current_move.y.distance;
+ machine->z = machine->current_move.z.start + machine->current_move.z.distance;
+ machine->current_move.type = MOVE_NONE;
+ ControlData data = {
+ .x = machine->x,
+ .y = machine->y,
+ .z = machine->z,
+ };
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__FINISHED,
+ .has_data = true,
+ .data = data,
+ };
+ return result;
+}
+
+ExecutionResult machine_execute_g1_tick(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+) {
+ Time elapsed = time_sub(now, &machine->current_move.start);
+ float percent_complete = time_div(&elapsed, &machine->current_move.duration);
+ float dx = machine->current_move.x.distance * percent_complete;
+ float dy = machine->current_move.y.distance * percent_complete;
+ float dz = machine->current_move.z.distance * percent_complete;
+ machine->x = machine->current_move.x.start + dx;
+ machine->y = machine->current_move.y.start + dy;
+ machine->z = machine->current_move.z.start + dz;
+ ControlData data = {
+ .x = machine->x,
+ .y = machine->y,
+ .z = machine->z,
+ };
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__IN_PROGRESS,
+ .has_data = true,
+ .data = data,
+ };
+ return result;
+}
+
+ExecutionResult machine_execute_g1(
+ Machine* machine,
+ const Command* command,
+ const Time* now
+) {
+ if (machine->current_move.type == MOVE_NONE) {
+ return machine_execute_g1_start(machine, command, now);
+ }
+ if (time_cmp(now, &machine->current_move.end) >= 0) {
+ return machine_execute_g1_end(machine, command, now);
+ }
+ return machine_execute_g1_tick(machine, command, now);
+}
+
+ExecutionResult machine_execute_g21(Machine* machine, const Command* command) {
+ machine->units = UNITS_MM;
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__FINISHED,
+ .has_data = false,
+ };
+ return result;
+}
+
+ExecutionResult machine_execute_g90(Machine* machine, const Command* command) {
+ machine->positioning_mode = POSITIONING_MODE_ABSOLUTE;
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__FINISHED,
+ .has_data = false,
+ };
+ return result;
+}
+
+ExecutionResult machine_execute_g91(Machine* machine, const Command* command) {
+ machine->positioning_mode = POSITIONING_MODE_RELATIVE;
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__FINISHED,
+ .has_data = false,
+ };
+ return result;
+}
+
+ExecutionResult machine_execute_g92(Machine* machine, const Command* command) {
+ if (
+ !command->param_x.is_set &&
+ !command->param_y.is_set &&
+ !command->param_z.is_set
+ ) {
+ machine->x = 0.0f;
+ machine->y = 0.0f;
+ machine->z = 0.0f;
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__FINISHED,
+ .has_data = false,
+ };
+ return result;
+ }
+
+ if (command->param_x.is_set) {
+ machine->x = command->param_x.value;
+ }
+ if (command->param_y.is_set) {
+ machine->y = command->param_y.value;
+ }
+ if (command->param_z.is_set) {
+ machine->z = command->param_z.value;
+ }
+ ExecutionResult result = {
+ .status = EXECUTE_STATUS__FINISHED,
+ .has_data = false,
+ };
+ return result;
+}
diff --git a/src/main.c b/src/main.c
@@ -0,0 +1,223 @@
+#include "token.h"
+#include "command.h"
+#include "machine.h"
+#include "timing.h"
+#include "tui.h"
+
+#include <assert.h>
+#include <string.h>
+#include <errno.h>
+
+void print_state(
+ const Machine* machine,
+ const ExecutionResult* execution_result
+) {
+ type(set_cursor(1, 2));
+ printf("gcode interpreter");
+ type(clear_to_end_of_line);
+
+ type(set_cursor(3, 2));
+ printf("machine state:");
+ type(clear_to_end_of_line);
+
+ type(set_cursor(4, 6));
+ printf(
+ "xyz: [ %4.3f %4.3f %4.3f ] (%s/sec)",
+ machine->x,
+ machine->y,
+ machine->z,
+ UNITS_STRINGS[machine->units]
+ );
+ type(clear_to_end_of_line);
+
+ type(set_cursor(5, 6));
+ printf(
+ "feed rate: [ %4.3f ] (%s/min)",
+ machine->feed_rate,
+ UNITS_STRINGS[machine->units]
+ );
+ type(clear_to_end_of_line);
+
+ type(set_cursor(6, 6));
+ printf(
+ "positioning_mode: %s",
+ POSITIONING_MODE_STRINGS[machine->positioning_mode]
+ );
+ type(clear_to_end_of_line);
+}
+
+void print_token_error(const Token* token, unsigned int* error_count) {
+ type(set_cursor(8, 2));
+ printf(
+ "%c [L%d C%d] -- %s\n",
+ token->type,
+ token->line_idx,
+ token->char_idx,
+ TOKEN_ERROR_STRINGS[token->value.error_value]
+ );
+ type(clear_to_end_of_line);
+
+ type(set_cursor(9, 2));
+ printf("total error count: %d", *error_count);
+ type(clear_to_end_of_line);
+
+ type(set_cursor(10, 2));
+ fprintf(
+ stderr,
+ "%c [L%d C%d] -- %s\n",
+ token->type,
+ token->line_idx,
+ token->char_idx,
+ TOKEN_ERROR_STRINGS[token->value.error_value]
+ );
+}
+
+void print_execution_error(
+ const Command* command,
+ const ExecutionResult* result,
+ unsigned int* error_count
+) {
+ type(set_cursor(8, 2));
+ printf(
+ "! [%c%-3d] -- %s",
+ command->code.type,
+ command->code.value,
+ MACHINE_ERROR_STRINGS[result->status]
+ );
+ type(clear_to_end_of_line);
+
+ type(set_cursor(9, 2));
+ printf("total error count: %d", *error_count);
+ type(clear_to_end_of_line);
+
+ type(set_cursor(10, 2));
+ fprintf(
+ stderr,
+ "! [%c%-3d] -- %s\n",
+ command->code.type,
+ command->code.value,
+ MACHINE_ERROR_STRINGS[result->status]
+ );
+}
+
+void save_output(
+ FILE* file,
+ const ExecutionResult* result,
+ const Time* now
+) {
+ if (!result->has_data) {
+ return;
+ }
+ fprintf(
+ file,
+ "%10ld (s) %10ld (ns) - [ %4.3f %4.3f %4.3f ]\n",
+ now->time_s,
+ now->time_ns,
+ result->data.x,
+ result->data.y,
+ result->data.z
+ );
+}
+
+int main(int argc, char** argv) {
+ freopen("error.log", "w", stderr);
+ initialize_terminal();
+
+ const char* filename = "CE3E3V2_3DBenchy.gcode";
+
+ FILE* file = fopen(filename, "r");
+ if (file == NULL) {
+ fprintf(stderr, "! [%s]: %s\n", filename, strerror(errno));
+ return -1;
+ }
+
+ const char* output_filename = "out";
+ FILE* output_file = fopen(output_filename, "w");
+ if (output_file == NULL) {
+ fprintf(stderr, "! [%s]: %s\n", filename, strerror(errno));
+ return -1;
+ }
+
+ Cursor cursor;
+ cursor_init(&cursor, file);
+
+ Command command;
+ command_init(&command);
+
+ Machine machine;
+ machine_init(&machine);
+
+ Token token;
+ CommandStatus command_status = COMMAND_STATUS__NOT_READY;
+ ExecutionResult execution_result = {
+ .status = EXECUTE_STATUS__IDLE,
+ .has_data = false,
+ };
+
+ unsigned int error_count = 0;
+
+ print_state(&machine, &execution_result);
+
+ Time now;
+ for (;;) {
+ now = time_get();
+
+ switch (execution_result.status) {
+ case EXECUTE_STATUS__IDLE: {
+ token = parse_token(&cursor);
+ if (token.type == TOKEN_ERROR) {
+ error_count += 1;
+ print_token_error(&token, &error_count);
+ recover_from_error(&cursor, &token);
+ }
+ if (token.type == TOKEN_EOF) {
+ return 0;
+ }
+
+ command_status = command_process_token(&command, &token);
+ if (command_status == COMMAND_STATUS__READY) {
+ execution_result = machine_execute_command(
+ &machine,
+ &command,
+ &now
+ );
+ save_output(output_file, &execution_result, &now);
+ print_state(&machine, &execution_result);
+ }
+ break;
+ }
+ case EXECUTE_STATUS__IN_PROGRESS: {
+ execution_result = machine_execute_command(
+ &machine,
+ &command,
+ &now
+ );
+ save_output(output_file, &execution_result, &now);
+ print_state(&machine, &execution_result);
+ break;
+ }
+ case EXECUTE_STATUS__UNRECOGNIZED_COMMAND:
+ case EXECUTE_STATUS__UNRECOGNIZED_MCODE:
+ case EXECUTE_STATUS__UNRECOGNIZED_GCODE:
+ error_count += 1;
+ print_execution_error(
+ &command,
+ &execution_result,
+ &error_count
+ );
+ case EXECUTE_STATUS__FINISHED: {
+ command_init(&command);
+ command_status = command_process_token(&command, &token);
+ assert(command_status == COMMAND_STATUS__NOT_READY);
+ execution_result.status = EXECUTE_STATUS__IDLE;
+ execution_result.has_data = false;
+ break;
+ }
+ default:
+ return 0;
+ }
+
+ }
+ return 0;
+}
+
diff --git a/src/timing.c b/src/timing.c
@@ -0,0 +1,62 @@
+#include "timing.h"
+
+#include <time.h>
+
+Time time_make(long time_s, long time_ns) {
+ const long billion = 1000000000;
+ long overflow = (time_ns >= 0 ? time_ns : time_ns - (billion - 1)) / billion;
+ Time t = {
+ .time_s = time_s + overflow,
+ .time_ns = time_ns - overflow * billion,
+ };
+ return t;
+}
+
+Time time_zero() {
+ return time_make(0, 0);
+};
+
+Time time_get() {
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ return time_make(ts.tv_sec, ts.tv_nsec);
+};
+
+Time time_add(const Time* t0, const Time* t1) {
+ long time_s = t0->time_s + t1->time_s;
+ long time_ns = t0->time_ns + t1->time_ns;
+ return time_make(time_s, time_ns);
+}
+
+Time time_sub(const Time* t0, const Time* t1) {
+ long time_s = t0->time_s - t1->time_s;
+ long time_ns = t0->time_ns - t1->time_ns;
+ return time_make(time_s, time_ns);
+}
+
+float time_div(const Time* t0, const Time* t1) {
+ long denominator = t1->time_s * 1e9 + t1->time_ns;
+ float q_a = ((float)t0->time_s / denominator) * 1e9;
+ float q_b = (float)t0->time_ns / denominator;
+ return q_a + q_b;
+}
+
+int time_cmp(const Time* t0, const Time* t1) {
+ if (t0->time_s > t1->time_s) {
+ return 1;
+ }
+ if (t0->time_s < t1->time_s) {
+ return -1;
+ }
+ if (t0->time_ns > t1->time_ns) {
+ return 1;
+ }
+ if (t0->time_ns < t1->time_ns) {
+ return -1;
+ }
+ return 0;
+}
+
+bool time_eql_zero(const Time* t) {
+ return t->time_s == 0 && t->time_ns == 0;
+}
diff --git a/src/token.c b/src/token.c
@@ -0,0 +1,309 @@
+#include "token.h"
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+
+#define MAX_FIELD_LEN 31
+
+void cursor_init(Cursor* cursor, FILE* file) {
+ cursor->file = file;
+ cursor->line_idx = 1;
+ cursor->char_idx = 1;
+}
+
+char cursor_getc(Cursor* cursor) {
+ char c = fgetc(cursor->file);
+ switch (c) {
+ case EOF:
+ break;
+ case '\n':
+ cursor->line_idx += 1;
+ case '\r':
+ cursor->char_idx = 0;
+ break;
+ default:
+ cursor->char_idx += 1;
+ }
+ return c;
+}
+
+void cursor_ungetc(Cursor* cursor, char c) {
+ ungetc(c, cursor->file);
+ switch (c) {
+ case EOF:
+ break;
+ default:
+ cursor->char_idx -= 1;
+ }
+}
+
+typedef enum {
+ TOKEN_STATUS__FIELD_VALUE_TOO_LONG,
+ TOKEN_STATUS__INVALID_CHAR_INT,
+ TOKEN_STATUS__NO_DIGITS_INT,
+ TOKEN_STATUS__INVALID_CHAR_FLOAT,
+ TOKEN_STATUS__NO_DIGITS_FLOAT,
+ TOKEN_STATUS__VALUE_PARSING_NOT_SUPPORTED,
+ TOKEN_STATUS__UNRECOGNIZED,
+ TOKEN_STATUS__OKAY,
+} TokenStatus;
+
+void skip_whitespace(Cursor* cursor) {
+ for (;;) {
+ char c = cursor_getc(cursor);
+ switch (c) {
+ case ' ':
+ case '\t':
+ case '\n':
+ case '\r':
+ continue;
+ default:
+ cursor_ungetc(cursor, c);
+ return;
+ }
+ }
+}
+
+void skip_until_whitespace(Cursor* cursor) {
+ for (;;) {
+ char c = cursor_getc(cursor);
+ switch (c) {
+ case EOF:
+ case ' ':
+ case '\t':
+ case '\n':
+ case '\r':
+ return;
+ default:
+ continue;
+ }
+ }
+}
+
+void skip_until_newline(Cursor* cursor) {
+ for (;;) {
+ char c = cursor_getc(cursor);
+ switch (c) {
+ case EOF:
+ case '\n':
+ return;
+ default:
+ continue;
+ }
+ }
+}
+
+typedef struct {
+ int length;
+ TokenStatus status;
+} ReadIntoBufferResult;
+
+ReadIntoBufferResult read_into_buffer(
+ Cursor* cursor,
+ char* buffer,
+ int max_len
+)
+{
+ ReadIntoBufferResult result;
+ int i;
+ char c;
+ for (i = 0; i < max_len; i += 1) {
+ c = cursor_getc(cursor);
+ switch (c) {
+ case EOF:
+ case ' ':
+ case '\t':
+ case '\n':
+ case '\r':
+ buffer[i] = 0;
+ result.length = i;
+ result.status = TOKEN_STATUS__OKAY;
+ return result;
+ default:
+ buffer[i] = c;
+ continue;
+ }
+ }
+ cursor_ungetc(cursor, c);
+ buffer[max_len - 1] = 0;
+ result.length = i;
+ result.status = TOKEN_STATUS__FIELD_VALUE_TOO_LONG;
+ return result;
+}
+
+typedef struct {
+ int value;
+ TokenStatus status;
+} ParseIntResult;
+
+ParseIntResult parse_int(char* str) {
+ ParseIntResult result;
+ char* end;
+ result.value = strtol(str, &end, 10);
+ if (end == str) {
+ result.status = TOKEN_STATUS__NO_DIGITS_INT;
+ }
+ else if (*end != '\0') {
+ result.status = TOKEN_STATUS__INVALID_CHAR_INT;
+ }
+ else {
+ result.status = TOKEN_STATUS__OKAY;
+ }
+ return result;
+}
+
+typedef struct {
+ float value;
+ TokenStatus status;
+} ParseFloatResult;
+
+ParseFloatResult parse_float(char* str) {
+ ParseFloatResult result;
+ char* end;
+ result.value = strtof(str, &end);
+ if (end == str) {
+ result.status = TOKEN_STATUS__NO_DIGITS_FLOAT;
+ }
+ else if (*end != '\0') {
+ result.status = TOKEN_STATUS__INVALID_CHAR_FLOAT;
+ }
+ else {
+ result.status = TOKEN_STATUS__OKAY;
+ }
+ return result;
+}
+
+Token parse_field(Cursor* cursor, TokenType type) {
+ Token t;
+ t.line_idx = cursor->line_idx;
+ t.char_idx = cursor->char_idx - 1;
+
+ char buffer[MAX_FIELD_LEN + 1];
+ ReadIntoBufferResult result = read_into_buffer(
+ cursor,
+ buffer,
+ MAX_FIELD_LEN + 1
+ );
+ if (result.status != TOKEN_STATUS__OKAY) {
+ t.type = TOKEN_ERROR;
+ t.value.error_value = result.status;
+ return t;
+ }
+
+ TokenValue value;
+ switch (type) {
+ case TOKEN_G_CODE:
+ case TOKEN_M_CODE: {
+ ParseIntResult result = parse_int(buffer);
+ if (result.status != TOKEN_STATUS__OKAY) {
+ t.type = TOKEN_ERROR;
+ t.value.error_value = result.status;
+ }
+ else {
+ t.type = type;
+ t.value.code_value = result.value;
+ }
+ break;
+ }
+ case TOKEN_S_PARAM:
+ case TOKEN_X_PARAM:
+ case TOKEN_Y_PARAM:
+ case TOKEN_Z_PARAM:
+ case TOKEN_F_PARAM:
+ case TOKEN_E_PARAM: {
+ ParseFloatResult result = parse_float(buffer);
+ if (result.status != TOKEN_STATUS__OKAY) {
+ t.type = TOKEN_ERROR;
+ t.value.error_value = result.status;
+ }
+ else {
+ t.type = type;
+ t.value.param_value = result.value;
+ }
+ break;
+ }
+ default:
+ t.type = TOKEN_ERROR;
+ t.value.error_value = TOKEN_STATUS__VALUE_PARSING_NOT_SUPPORTED;
+ break;
+ }
+ return t;
+}
+
+Token parse_comment(Cursor* cursor) {
+ Token t = {
+ .type = TOKEN_COMMENT,
+ .line_idx = cursor->line_idx,
+ .char_idx = cursor->char_idx - 1,
+ };
+ skip_until_newline(cursor);
+ return t;
+}
+
+Token parse_token(Cursor* cursor) {
+ skip_whitespace(cursor);
+
+ char c = cursor_getc(cursor);
+ switch (c) {
+ case ';':
+ return parse_comment(cursor);
+ case 'g':
+ case 'G':
+ return parse_field(cursor, TOKEN_G_CODE);
+ case 'm':
+ case 'M':
+ return parse_field(cursor, TOKEN_M_CODE);
+ case 's':
+ case 'S':
+ return parse_field(cursor, TOKEN_S_PARAM);
+ case 'x':
+ case 'X':
+ return parse_field(cursor, TOKEN_X_PARAM);
+ case 'y':
+ case 'Y':
+ return parse_field(cursor, TOKEN_Y_PARAM);
+ case 'z':
+ case 'Z':
+ return parse_field(cursor, TOKEN_Z_PARAM);
+ case 'f':
+ case 'F':
+ return parse_field(cursor, TOKEN_F_PARAM);
+ case 'e':
+ case 'E':
+ return parse_field(cursor, TOKEN_E_PARAM);
+ case EOF: {
+ Token t = {
+ .type = TOKEN_EOF,
+ .line_idx = cursor->line_idx,
+ .char_idx = cursor->char_idx,
+ };
+ return t;
+ }
+ default: {
+ cursor_ungetc(cursor, c);
+ Token t = {
+ .type = TOKEN_ERROR,
+ .line_idx = cursor->line_idx,
+ .char_idx = cursor->char_idx,
+ .value.error_value = TOKEN_STATUS__UNRECOGNIZED,
+ };
+ return t;
+ }
+ }
+}
+
+void recover_from_error(Cursor* cursor, const Token* token) {
+ switch (token->value.error_value) {
+ case TOKEN_STATUS__FIELD_VALUE_TOO_LONG:
+ case TOKEN_STATUS__UNRECOGNIZED:
+ skip_until_whitespace(cursor);
+ break;
+ default:
+ // most errors don't require any action to get back to a valid
+ // parsing state.
+ break;
+ }
+}
+
diff --git a/src/tui.c b/src/tui.c
@@ -0,0 +1,62 @@
+#include "tui.h"
+
+#include <stdlib.h>
+#include <signal.h>
+#include <termios.h>
+#include <unistd.h>
+#include <stdio.h>
+
+struct termios initial;
+
+void restore_terminal() {
+ type(
+ switch_to alternate_buffer
+ clear_buffer
+ switch_to main_buffer
+ show_cursor
+ );
+ // restore original termios params
+ tcsetattr(1, TCSANOW, &initial);
+}
+
+void restore_terminal_exit(int i) {
+ exit(1);
+}
+
+void initialize_terminal() {
+ // 1. save initial termios params
+ // 2. disable echo and canonical mode
+ struct termios t;
+ tcgetattr(1, &t);
+ initial = t;
+ t.c_lflag &= (~ECHO & ~ICANON);
+ tcsetattr(1, TCSANOW, &t);
+
+ // register cleanup function
+ atexit(restore_terminal);
+ signal(SIGTERM, restore_terminal_exit);
+ signal(SIGINT, restore_terminal_exit);
+
+ // disable output buffering
+ setvbuf(stdout, NULL, _IONBF, 0);
+
+ type(
+ switch_to alternate_buffer
+ clear_buffer
+ hide_cursor
+ );
+}
+
+void type_line(const char* msg, unsigned int line) {
+ const unsigned int buf_len = 64;
+ char buf[buf_len];
+ unsigned int str_len = snprintf(
+ buf,
+ buf_len - 1,
+ set_cursor("%d", "1"),
+ line
+ );
+ buf[str_len] = '\0';
+ type(buf);
+ //printf(msg);
+}
