added files

firmware/MotionControllerRP/src/command_parser/command_parser.cpp

@@ -0,0 +1,138 @@
+
+//*** INCLUDE ***************************************************************************
+
+#include "command_parser.h"
+#include <cstring>
+
+//*** CLASS *****************************************************************************
+
+//--- GCodeCommand ----------------------------------------------------------------------
+
+GCodeCommand::GCodeCommand() {
+  reset();
+}
+
+void GCodeCommand::set_command(const char* cmd) {
+  if (cmd) {
+    command = cmd;
+  } else {
+    command = "";
+  }
+}
+
+const std::string& GCodeCommand::get_command() const {
+  return command;
+}
+
+void GCodeCommand::reset() {
+  // Initialize all word values to NaN to represent "not set"
+  for (float& value : word_values) {
+    value = std::numeric_limits<float>::quiet_NaN();
+  }
+  command[0] = 0;
+}
+
+void GCodeCommand::set_value(char word, float value) {
+  if (word >= 'A' && word <= 'Z') {
+    word_values[word-'A'] = value;
+  }
+}
+
+float GCodeCommand::get_value(char word) const {
+  return get_value(word, std::numeric_limits<float>::quiet_NaN());
+}
+
+float GCodeCommand::get_value(char word, float default_value) const {
+  if (word >= 'A' && word <= 'Z') {
+    float value = word_values[word-'A'];
+    return std::isnan(value) ? default_value : value;
+  }
+  return default_value;
+}
+
+bool GCodeCommand::has_word(char word) const {
+  if (word >= 'A' && word <= 'Z') {
+    return !std::isnan(word_values[word-'A']);
+  }
+  return false;
+}
+
+//--- CommandParser ---------------------------------------------------------------------
+
+CommandParser::CommandParser() : buffer_index(0), command_processor(nullptr) {
+
+}
+
+void CommandParser::set_command_processor(ICommandProcessor* cp) {
+  command_processor = cp;
+}
+
+void CommandParser::update() {
+  if(command_processor == nullptr)
+    return;
+
+  if(command_ready == false)
+    return;
+
+  // Call user callback with parsed command
+  if(command_processor->can_process_command(command)) {
+    std::string reply;
+    command_processor->process_command(command, reply);
+    command_processor->send_reply(reply.c_str());
+
+    // mark command as processed
+    command_ready = false;
+  }
+}
+
+// Feed input chars one by one
+void CommandParser::add_input_character(char c) {
+  if (c == '\n' || c == '\r') {
+    if (buffer_index > 0) {
+      buffer[buffer_index] = '\0';
+      parse_line(buffer);
+      buffer_index = 0;
+    }
+  } else if (buffer_index < sizeof(buffer) - 1) {
+    buffer[buffer_index++] = c;
+  }
+}
+
+bool CommandParser::parse_line(const char* line) {
+  command_ready = false;
+
+  // Copy line into a mutable buffer
+  char line_copy[256];
+  strncpy(line_copy, line, sizeof(line_copy));
+  line_copy[sizeof(line_copy) - 1] = '\0';
+
+  // Tokenize the first word (e.g., G0, M3, T1)
+  char* saveptr = nullptr;
+  char* token = strtok_r(line_copy, " ", &saveptr);
+  if (!token || token[0] < 'A' || token[0] > 'Z') {
+    command_processor->send_reply("error: malformed command\n");
+    return false;
+  }
+
+  command.reset();
+  command.set_command(token);
+
+  // Parse remaining words (e.g., X1.0, Y2.5, F200)
+  while ((token = strtok_r(nullptr, " ", &saveptr))) {
+    if (token[0] >= 'A' && token[0] <= 'Z') {
+      command.set_value(token[0], strtof(token + 1, nullptr));
+    } else {
+      command_processor->send_reply("error: invalid parameter\n");
+      return false;
+    }
+  }
+
+  command_ready = true;
+  return true;
+}
+
+// returns false if the command could not yet be processed.
+// The function will be called witht the same command later to try again.
+bool CommandParser::handle_gcode_command(const GCodeCommand& cmd) {
+  return false;
+}

firmware/MotionControllerRP/src/command_parser/command_parser.h

@@ -0,0 +1,65 @@
+#pragma once
+
+//*** INCLUDE ***************************************************************************
+
+#include <functional>
+#include <string>
+#include <stdlib.h>
+#include <stdint.h>
+#include "utilities/math3d.h"
+
+//*** CLASS *****************************************************************************
+
+//--- GCodeCommand ----------------------------------------------------------------------
+
+class GCodeCommand {
+  public:
+    GCodeCommand();
+
+    void                reset();
+    void                set_command(const char* cmd);
+    const std::string&  get_command() const;
+    void                set_value(char word, float value);
+    float               get_value(char word) const;
+    float               get_value(char word, float default_value) const;
+    bool                has_word(char word) const;
+
+  private:
+    std::string command;
+    float word_values[26];
+};
+
+//--- ICommandProcessor -----------------------------------------------------------------
+
+class ICommandProcessor {
+  public:
+    virtual ~ICommandProcessor() {};
+
+    virtual void send_reply(const char* str) = 0;
+    virtual bool can_process_command(const GCodeCommand& cmd) = 0;
+    virtual void process_command(const GCodeCommand& cmd, std::string& reply) = 0;
+};
+
+//--- CommandParser ---------------------------------------------------------------------
+
+class CommandParser {
+  public:
+    CommandParser();
+
+    void set_command_processor(ICommandProcessor* cp);
+    void add_input_character(char c);  // Feed input chars one by one
+    void update();  // Feed input chars one by one
+
+  protected:
+    bool parse_line(const char* line);
+    bool handle_gcode_command(const GCodeCommand& cmd);
+
+  private:
+    char buffer[255];
+    int buffer_index;
+    ICommandProcessor* command_processor;
+
+    GCodeCommand command;
+    bool command_ready;
+};
+