wangdequan/cnc_wams
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: sync axis task state parity work

Browse Source
This commit is contained in:
wangdequan
2026-07-07 18:45:26 -04:00
parent 16484afce6
commit 8ef67f94c2
562 changed files with 175971 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_motion_runtime.c
View File

@@ -459,16 +459,17 @@ int lcmot_read_status_json(char *out, int out_len)
"\"motionRuntimeReady\":true,"
"\"nativeHalSyncReady\":false,"
"\"cycle\":%lld,"
"\"motion\":{\"programLine\":%d,\"motionType\":%d,\"coordMode\":%d,"
"\"motion\":{\"programLine\":%d,\"id\":%d,\"motionType\":%d,\"coordMode\":%d,"
"\"teleopMode\":%d,\"inPosition\":%s,\"paused\":%s,\"aborted\":%s,"
"\"stepping\":%s,\"idForStep\":%d,\"motionId\":%d,"
"\"switchkinsType\":%d,\"requestedVel\":%.17g,\"currentVel\":%.17g,"
"\"commandQueueDepth\":%d},"
"\"queueDepth\":%d,\"activeDepth\":%d,\"commandQueueDepth\":%d},"
"\"axis\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g,\"a\":%.17g,\"b\":%.17g,\"c\":%.17g},"
"\"joint0\":{\"motorPosCmd\":%.17g,\"motorPosFb\":%.17g},"
"\"commandQueueDepth\":%d}",
"\"queueDepth\":%d,\"activeDepth\":%d,\"commandQueueDepth\":%d}",
state->cycle,
state->program_line,
state->motion_id,
state->motion_type,
state->coord_mode,
state->teleop_mode,
@@ -482,6 +483,8 @@ int lcmot_read_status_json(char *out, int out_len)
state->requested_vel,
state->current_vel,
state->queue_count,
state->in_position ? 0 : 1,
state->queue_count,
state->axis_fb[0],
state->axis_fb[1],
state->axis_fb[2],
@@ -490,6 +493,8 @@ int lcmot_read_status_json(char *out, int out_len)
state->axis_fb[5],
state->joint_cmd[0],
state->joint_fb[0],
state->queue_count,
state->in_position ? 0 : 1,
state->queue_count);
return write_output(buffer, out, out_len);
}

137
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_task_hal_wasm.cpp
View File

@@ -35,17 +35,26 @@ struct TaskRuntime {
std::string exec_state = "DONE";
bool task_paused = false;
bool single_stepping = false;
bool no_force_homing = false;
bool all_homed = false;
std::vector<int> homed = std::vector<int>(5, 0);
bool homing = false;
int home_cycles_remaining = 0;
std::string home_state = "UNHOMED";
std::string open_program;
int opened_line_count = 0;
int opened_source_line_count = 0;
int executable_line_count = 0;
int next_program_line = 0;
int program_start_line = 0;
long long task_cycle = 0;
long long servo_cycle = 0;
bool motion_plan_loaded = false;
int motion_plan_id = 0;
int active_segment_index = 0;
double run_elapsed_seconds = 0.0;
std::string motion_plan_program_path;
std::string error_text;
std::vector<MotionSegment> motion_plan;
std::map<std::string, std::string> staged_files;
std::vector<std::string> program_lines;
@@ -268,6 +277,20 @@ std::string axis_json(const std::map<std::string, double> &start_axes,
return out.str();
}
std::string bool_array_json(const std::vector<int> &values)
{
std::ostringstream out;
out << "[";
for (std::size_t i = 0; i < values.size(); ++i) {
if (i > 0) {
out << ",";
}
out << (values[i] ? "true" : "false");
}
out << "]";
return out.str();
}
std::string trim_copy(const std::string &value)
{
std::size_t begin = 0;
@@ -301,6 +324,45 @@ int forward_motion_command(const std::string &json)
return lcmot_write_command_json(json.c_str());
}
void set_error(TaskRuntime &state, const std::string &error)
{
state.error_text = error;
state.events.push_back("task_error:" + error);
}
bool task_accepts_plan_run(TaskRuntime &state)
{
if (state.state != "ON") {
set_error(state, "RUN_REJECTED_STATE_NOT_ON");
return false;
}
if (state.mode != "AUTO") {
set_error(state, "RUN_REJECTED_MODE_NOT_AUTO");
return false;
}
if (state.interp_state != "IDLE") {
set_error(state, "RUN_REJECTED_INTERP_NOT_IDLE");
return false;
}
if (state.homing) {
set_error(state, "RUN_REJECTED_HOMING_ACTIVE");
return false;
}
if (!state.all_homed && !state.no_force_homing) {
set_error(state, "RUN_REJECTED_NOT_HOMED");
return false;
}
if (state.open_program.empty()) {
set_error(state, "PROGRAM_OPEN_REQUIRED");
return false;
}
if (!state.motion_plan_loaded || state.motion_plan.empty()) {
set_error(state, "INTERPRETER_PLAN_REQUIRED");
return false;
}
return true;
}
void enqueue_linear_move_from_line(TaskRuntime &state, const std::string &line)
{
std::ostringstream command;
@@ -367,6 +429,7 @@ bool load_motion_plan(TaskRuntime &state, const char *plan_json)
}
state.motion_plan = std::move(segments);
state.motion_plan_loaded = true;
state.motion_plan_id += 1;
state.active_segment_index = 0;
state.run_elapsed_seconds = 0.0;
state.motion_plan_program_path = json_string_after(plan_json, "\"programPath\"", state.open_program);
@@ -491,7 +554,20 @@ std::string status_json()
out << ",\"taskPaused\":" << (state.task_paused ? "true" : "false");
out << ",\"singleStepping\":" << (state.single_stepping ? "true" : "false");
out << ",\"cycle\":" << state.task_cycle;
out << ",\"taskCycle\":" << state.task_cycle;
out << ",\"file\":\"" << json_escape(state.open_program) << "\"";
out << ",\"openProgram\":\"" << json_escape(state.open_program) << "\"";
out << ",\"programOpen\":" << (state.open_program.empty() ? "false" : "true");
out << ",\"programStartLine\":" << state.program_start_line;
out << ",\"planId\":" << state.motion_plan_id;
out << ",\"motionPlanLoaded\":" << (state.motion_plan_loaded ? "true" : "false");
out << ",\"errorText\":\"" << json_escape(state.error_text) << "\"";
out << ",\"motionEnabled\":" << (state.state == "ON" ? "true" : "false");
out << ",\"allHomed\":" << (state.all_homed ? "true" : "false");
out << ",\"homed\":" << bool_array_json(state.homed);
out << ",\"noForceHoming\":" << (state.no_force_homing ? "true" : "false");
out << ",\"homing\":" << (state.homing ? "true" : "false");
out << ",\"homeState\":\"" << json_escape(state.home_state) << "\"";
out << ",\"openedLineCount\":" << state.opened_line_count;
out << ",\"openedSourceLineCount\":" << state.opened_source_line_count;
out << ",\"executableLineCount\":" << state.executable_line_count;
@@ -537,6 +613,11 @@ int lctask_init_session(const char *session_json)
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.no_force_homing = contains_token(session_json, "\"noForceHoming\":true") ||
contains_token(session_json, "\"no_force_homing\":true");
if (state.no_force_homing) {
state.home_state = "NO_FORCE_HOMING";
}
state.events.push_back("task_session_init");
const std::string ini_path = json_string_after(session_json, "\"iniPath\"", "task-hal-session.ini");
@@ -584,6 +665,7 @@ int lctask_open_program(const char *path)
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.error_text.clear();
state.events.push_back(std::string("task_open_program:") + path);
return 0;
}
@@ -604,17 +686,43 @@ int lctask_send_command_json(const char *command_json)
return -1;
}
if (contains_token(command_json, "EMC_TASK_SET_STATE")) {
state.state = json_string_after(command_json, "\"state\"", state.state);
const std::string requested_state = json_string_after(command_json, "\"state\"", state.state);
if (requested_state == "ON" && state.state == "ESTOP") {
set_error(state, "POWER_ON_REJECTED_ESTOP_ACTIVE");
return -1;
}
state.state = requested_state;
if (state.state == "ESTOP" || state.state == "OFF") {
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.task_paused = false;
state.single_stepping = false;
state.homing = false;
state.home_cycles_remaining = 0;
state.all_homed = false;
std::fill(state.homed.begin(), state.homed.end(), 0);
state.home_state = "UNHOMED";
forward_motion_command("{\"type\":\"EMC_TRAJ_ABORT\"}");
}
state.error_text.clear();
state.events.push_back("task_set_state:" + state.state);
return 0;
}
if (contains_token(command_json, "EMC_TASK_SET_MODE")) {
state.mode = json_string_after(command_json, "\"mode\"", state.mode);
if (state.mode == "MANUAL") {
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.task_paused = false;
state.single_stepping = false;
}
state.events.push_back("task_set_mode:" + state.mode);
return 0;
}
if (contains_token(command_json, "EMC_TASK_PLAN_RUN")) {
if (state.open_program.empty()) {
if (!task_accepts_plan_run(state)) {
return -1;
}
state.next_program_line = json_int_after(command_json, "\"line\"", 0);
@@ -626,11 +734,13 @@ int lctask_send_command_json(const char *command_json)
}
state.active_segment_index = 0;
state.run_elapsed_seconds = 0.0;
state.program_start_line = state.next_program_line;
state.interp_state = "READING";
state.interp_resume_state = "READING";
state.exec_state = "WAITING_FOR_MOTION";
state.task_paused = false;
state.single_stepping = false;
state.error_text.clear();
state.events.push_back("task_plan_run");
return 0;
}
@@ -675,6 +785,7 @@ int lctask_send_command_json(const char *command_json)
state.task_paused = false;
state.single_stepping = false;
state.run_elapsed_seconds = 0.0;
state.error_text.clear();
state.events.push_back("task_abort");
return forward_motion_command("{\"type\":\"EMC_TRAJ_ABORT\"}");
}
@@ -697,6 +808,10 @@ int lctask_send_command_json(const char *command_json)
return forward_motion_command(command_json);
}
if (contains_token(command_json, "EMC_JOINT_HOME")) {
if (state.state != "ON" || state.mode != "MANUAL") {
set_error(state, "HOME_REJECTED_STATE_OR_MODE");
return -1;
}
state.mode = "MANUAL";
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
@@ -706,6 +821,12 @@ int lctask_send_command_json(const char *command_json)
state.next_program_line = 0;
state.active_segment_index = 0;
state.run_elapsed_seconds = 0.0;
state.all_homed = false;
std::fill(state.homed.begin(), state.homed.end(), 0);
state.homing = true;
state.home_cycles_remaining = 1;
state.home_state = "HOMING";
state.error_text.clear();
state.events.push_back("task_joint_home");
forward_motion_command("{\"type\":\"EMC_TRAJ_LINEAR_MOVE\",\"line\":1,\"x\":0,\"y\":0,\"z\":0,\"a\":0,\"b\":0,\"c\":0,\"velocity\":0}");
return 0;
@@ -728,9 +849,19 @@ int lctask_run_cycles(long task_period_ns, long servo_period_ns, int task_cycles
}
for (int i = 0; i < task_cycles; ++i) {
state.task_cycle += 1;
if (state.homing && state.home_cycles_remaining > 0) {
state.home_cycles_remaining -= 1;
if (state.home_cycles_remaining == 0) {
state.homing = false;
state.all_homed = true;
std::fill(state.homed.begin(), state.homed.end(), 1);
state.home_state = "HOMED";
state.events.push_back("task_home_complete");
}
}
const bool has_program_work = state.motion_plan_loaded
? state.active_segment_index < static_cast<int>(state.motion_plan.size())
: state.next_program_line < state.executable_line_count;
: false;
const bool stepping = state.single_stepping;
if ((state.interp_state == "READING" || stepping) && has_program_work) {
if (state.motion_plan_loaded) {