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

feat: complete working8 pause semantics

Browse Source
This commit is contained in:
wangdequan
2026-07-07 10:27:02 -04:00
parent 83402c506e
commit 4ba45b2ec5
987 changed files with 340641 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -39,6 +39,9 @@ typedef struct {
int teleop_mode;
int in_position;
int paused;
int stepping;
int id_for_step;
int motion_id;
int aborted;
int switchkins_type;
double requested_vel;
@@ -187,6 +190,13 @@ static int queue_pop(LcmotCommand *command)
return 1;
}
static int is_motion_command(int type)
{
return type == LCMOT_CMD_LINEAR_MOVE ||
type == LCMOT_CMD_CIRCULAR_MOVE ||
type == LCMOT_CMD_JOG_INCR;
}
static void sync_hal_pins(void)
{
int i;
@@ -263,19 +273,29 @@ static void apply_command(const LcmotCommand *command)
switch (command->type) {
case LCMOT_CMD_PAUSE:
state->paused = 1;
state->stepping = 0;
state->id_for_step = state->motion_id;
state->motion_type = 0;
state->current_vel = 0.0;
state->in_position = 0;
break;
case LCMOT_CMD_STEP:
state->paused = 0;
state->stepping = 1;
state->id_for_step = state->motion_id;
state->paused = 1;
break;
case LCMOT_CMD_RESUME:
state->paused = 0;
state->stepping = 0;
state->id_for_step = state->motion_id;
break;
case LCMOT_CMD_ABORT:
state->aborted = 1;
state->paused = 0;
state->stepping = 0;
state->id_for_step = state->motion_id;
state->motion_type = 0;
state->current_vel = 0.0;
state->in_position = 1;
state->queue_head = 0;
state->queue_tail = 0;
@@ -287,6 +307,7 @@ static void apply_command(const LcmotCommand *command)
break;
case LCMOT_CMD_LINEAR_MOVE:
case LCMOT_CMD_CIRCULAR_MOVE:
state->motion_id += 1;
state->program_line = command->line;
state->motion_type = command->type == LCMOT_CMD_CIRCULAR_MOVE ? 2 : 1;
state->coord_mode = 1;
@@ -302,8 +323,14 @@ static void apply_command(const LcmotCommand *command)
state->current_vel = state->requested_vel;
state->in_position = 1;
state->motion_type = 0;
if (state->stepping) {
state->paused = 1;
state->stepping = 0;
state->current_vel = 0.0;
}
break;
case LCMOT_CMD_JOG_INCR:
state->motion_id += 1;
i = command->axis_index;
if (i < 0 || i >= 9) {
i = 0;
@@ -320,6 +347,11 @@ static void apply_command(const LcmotCommand *command)
state->current_vel = state->requested_vel;
state->in_position = 1;
state->motion_type = 0;
if (state->stepping) {
state->paused = 1;
state->stepping = 0;
state->current_vel = 0.0;
}
break;
default:
break;
@@ -381,6 +413,14 @@ int lcmot_write_command_json(const char *json)
} else {
return -1;
}
if (command.type == LCMOT_CMD_PAUSE ||
command.type == LCMOT_CMD_STEP ||
command.type == LCMOT_CMD_RESUME ||
command.type == LCMOT_CMD_ABORT) {
apply_command(&command);
sync_hal_pins();
return 0;
}
return queue_push(command);
}
@@ -396,6 +436,7 @@ int lcmot_step_servo(long period_ns, int cycles)
if (!lcmot_state.aborted && lcmot_state.queue_count > 0) {
command = lcmot_state.queue[lcmot_state.queue_head];
if (!lcmot_state.paused ||
(lcmot_state.stepping && is_motion_command(command.type)) ||
command.type == LCMOT_CMD_STEP ||
command.type == LCMOT_CMD_RESUME ||
command.type == LCMOT_CMD_ABORT) {
@@ -420,7 +461,9 @@ int lcmot_read_status_json(char *out, int out_len)
"\"cycle\":%lld,"
"\"motion\":{\"programLine\":%d,\"motionType\":%d,\"coordMode\":%d,"
"\"teleopMode\":%d,\"inPosition\":%s,\"paused\":%s,\"aborted\":%s,"
"\"switchkinsType\":%d,\"requestedVel\":%.17g,\"currentVel\":%.17g},"
"\"stepping\":%s,\"idForStep\":%d,\"motionId\":%d,"
"\"switchkinsType\":%d,\"requestedVel\":%.17g,\"currentVel\":%.17g,"
"\"commandQueueDepth\":%d},"
"\"axis\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g,\"a\":%.17g,\"b\":%.17g,\"c\":%.17g},"
"\"joint0\":{\"motorPosCmd\":%.17g,\"motorPosFb\":%.17g},"
"\"commandQueueDepth\":%d}",
@@ -432,9 +475,13 @@ int lcmot_read_status_json(char *out, int out_len)
state->in_position ? "true" : "false",
state->paused ? "true" : "false",
state->aborted ? "true" : "false",
state->stepping ? "true" : "false",
state->id_for_step,
state->motion_id,
state->switchkins_type,
state->requested_vel,
state->current_vel,
state->queue_count,
state->axis_fb[0],
state->axis_fb[1],
state->axis_fb[2],

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

@@ -31,6 +31,7 @@ struct TaskRuntime {
std::string state = "ESTOP";
std::string mode = "MANUAL";
std::string interp_state = "IDLE";
std::string interp_resume_state = "IDLE";
std::string exec_state = "DONE";
bool task_paused = false;
bool single_stepping = false;
@@ -394,7 +395,10 @@ void forward_timed_motion_sample(TaskRuntime &state)
if (state.active_segment_index >= static_cast<int>(state.motion_plan.size())) {
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.task_paused = false;
state.single_stepping = false;
state.next_program_line = std::max(state.opened_line_count, state.opened_source_line_count);
return;
}
@@ -416,7 +420,10 @@ void forward_timed_motion_sample(TaskRuntime &state)
state.next_program_line = std::min(std::max(segment.line, 1), std::max(max_line, 1));
if (state.active_segment_index == static_cast<int>(state.motion_plan.size()) - 1 && progress >= 1.0) {
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.task_paused = false;
state.single_stepping = false;
state.next_program_line = std::max(state.opened_line_count, state.opened_source_line_count);
state.events.push_back("task_motion_plan_complete");
}
@@ -479,6 +486,7 @@ std::string status_json()
out << ",\"task\":{\"state\":\"" << json_escape(state.state) << "\"";
out << ",\"mode\":\"" << json_escape(state.mode) << "\"";
out << ",\"interpState\":\"" << json_escape(state.interp_state) << "\"";
out << ",\"interpResumeState\":\"" << json_escape(state.interp_resume_state) << "\"";
out << ",\"execState\":\"" << json_escape(state.exec_state) << "\"";
out << ",\"taskPaused\":" << (state.task_paused ? "true" : "false");
out << ",\"singleStepping\":" << (state.single_stepping ? "true" : "false");
@@ -527,6 +535,7 @@ int lctask_init_session(const char *session_json)
state.state = "ESTOP_RESET";
state.mode = "MANUAL";
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.events.push_back("task_session_init");
@@ -573,6 +582,7 @@ int lctask_open_program(const char *path)
state.active_segment_index = 0;
state.run_elapsed_seconds = 0.0;
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.events.push_back(std::string("task_open_program:") + path);
return 0;
@@ -617,6 +627,7 @@ int lctask_send_command_json(const char *command_json)
state.active_segment_index = 0;
state.run_elapsed_seconds = 0.0;
state.interp_state = "READING";
state.interp_resume_state = "READING";
state.exec_state = "WAITING_FOR_MOTION";
state.task_paused = false;
state.single_stepping = false;
@@ -624,6 +635,9 @@ int lctask_send_command_json(const char *command_json)
return 0;
}
if (contains_token(command_json, "EMC_TASK_PLAN_PAUSE")) {
if (state.interp_state != "PAUSED") {
state.interp_resume_state = state.interp_state.empty() ? "READING" : state.interp_state;
}
state.interp_state = "PAUSED";
state.exec_state = "PAUSED";
state.task_paused = true;
@@ -631,19 +645,24 @@ int lctask_send_command_json(const char *command_json)
return forward_motion_command("{\"type\":\"EMC_TRAJ_PAUSE\"}");
}
if (contains_token(command_json, "EMC_TASK_PLAN_STEP")) {
if (state.interp_state != "PAUSED") {
state.interp_resume_state = state.interp_state.empty() ? "READING" : state.interp_state;
}
state.single_stepping = true;
state.task_paused = true;
if (state.interp_state == "PAUSED") {
state.interp_state = "READING";
}
state.exec_state = "WAITING_FOR_MOTION";
state.interp_state = "PAUSED";
state.exec_state = "PAUSED";
state.events.push_back("task_plan_step");
forward_motion_command("{\"type\":\"EMC_TRAJ_STEP\"}");
return 0;
}
if (contains_token(command_json, "EMC_TASK_PLAN_RESUME")) {
state.interp_state = "READING";
state.exec_state = "WAITING_FOR_MOTION";
const std::string resume = state.interp_resume_state == "PAUSED" || state.interp_resume_state.empty()
? "READING"
: state.interp_resume_state;
state.interp_state = resume;
state.interp_resume_state = resume;
state.exec_state = resume == "IDLE" ? "DONE" : "WAITING_FOR_MOTION";
state.task_paused = false;
state.single_stepping = false;
state.events.push_back("task_plan_resume");
@@ -651,6 +670,7 @@ int lctask_send_command_json(const char *command_json)
}
if (contains_token(command_json, "EMC_TASK_ABORT")) {
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.task_paused = false;
state.single_stepping = false;
@@ -661,6 +681,7 @@ int lctask_send_command_json(const char *command_json)
if (contains_token(command_json, "EMC_TASK_PLAN_EXECUTE")) {
state.mode = "MDI";
state.interp_state = "READING";
state.interp_resume_state = "READING";
state.exec_state = "WAITING_FOR_MOTION";
state.task_paused = false;
enqueue_mdi(state, command_json);
@@ -669,6 +690,7 @@ int lctask_send_command_json(const char *command_json)
if (contains_token(command_json, "EMC_JOG_INCR")) {
state.mode = "MANUAL";
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "WAITING_FOR_MOTION";
state.task_paused = false;
state.events.push_back("task_jog_incr");
@@ -677,6 +699,7 @@ int lctask_send_command_json(const char *command_json)
if (contains_token(command_json, "EMC_JOINT_HOME")) {
state.mode = "MANUAL";
state.interp_state = "IDLE";
state.interp_resume_state = "IDLE";
state.exec_state = "DONE";
state.task_paused = false;
state.single_stepping = false;
@@ -708,7 +731,8 @@ int lctask_run_cycles(long task_period_ns, long servo_period_ns, int task_cycles
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;
if (state.interp_state == "READING" && has_program_work) {
const bool stepping = state.single_stepping;
if ((state.interp_state == "READING" || stepping) && has_program_work) {
if (state.motion_plan_loaded) {
const double delta_seconds = task_period_ns > 0 ? static_cast<double>(task_period_ns) / 1000000000.0 : 0.01;
state.run_elapsed_seconds += delta_seconds;
@@ -716,18 +740,24 @@ int lctask_run_cycles(long task_period_ns, long servo_period_ns, int task_cycles
} else {
enqueue_linear_move_from_line(state, state.program_lines[state.next_program_line]);
}
if (!state.motion_plan_loaded && state.next_program_line >= state.executable_line_count) {
if (stepping) {
state.interp_state = "PAUSED";
state.exec_state = "PAUSED";
state.task_paused = true;
state.single_stepping = false;
} else if (!state.motion_plan_loaded && state.next_program_line >= state.executable_line_count) {
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_plan_complete");
}
if (state.single_stepping) {
state.interp_state = "PAUSED";
state.exec_state = "PAUSED";
state.task_paused = true;
}
} else if (stepping) {
state.interp_state = "PAUSED";
state.exec_state = "PAUSED";
state.task_paused = true;
state.single_stepping = false;
}
if (lcmot_step_servo(servo_period_ns, servo_per_task) != 0) {
return -1;

153
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_wasm.c
View File

@@ -1048,6 +1048,158 @@ static void append_queued_lines_probe(char *out, size_t size, size_t *offset)
append(out, size, offset, line);
}
static void append_pause_resume_probe(char *out, size_t size, size_t *offset)
{
init_motion_state();
TP_STRUCT tp;
EmcPose start;
EmcPose end1;
EmcPose end2;
struct state_tag_t tag;
memset(&tp, 0, sizeof(tp));
memset(&start, 0, sizeof(start));
memset(&end1, 0, sizeof(end1));
memset(&end2, 0, sizeof(end2));
memset(&tag, 0, sizeof(tag));
end1.tran.x = 1.0;
end2.tran.x = 2.0;
const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 4);
const int set_cycle_rc = tpSetCycleTime(&tp, 0.001);
const int set_pos_rc = tpSetPos(&tp, &start);
const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0);
const int set_vlimit_rc = tpSetVlimit(&tp, 1.0);
const int set_amax_rc = tpSetAmax(&tp, 10.0);
const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0);
const int add_line1_rc = tpAddLine(
&tp,
end1,
EMC_MOTION_TYPE_FEED,
1.0,
1.0,
10.0,
100.0,
status.enables_new,
0,
-1,
tag);
const int add_line2_rc = tpAddLine(
&tp,
end2,
EMC_MOTION_TYPE_FEED,
1.0,
1.0,
10.0,
100.0,
status.enables_new,
0,
-1,
tag);
int cycle_rc = 0;
int cycles_before_pause = 0;
for (; cycles_before_pause < 200 && !tpIsDone(&tp); ++cycles_before_pause) {
cycle_rc = tpRunCycle(&tp, 1000000);
if (cycle_rc < 0) {
break;
}
}
const int pause_rc = tpPause(&tp);
int pause_cycles = 0;
for (; pause_cycles < 10000; ++pause_cycles) {
cycle_rc = tpRunCycle(&tp, 1000000);
if (cycle_rc < 0 || fabs(status.current_vel) < 1e-9) {
break;
}
}
EmcPose paused_pos;
EmcPose hold_pos;
EmcPose final_pos;
memset(&paused_pos, 0, sizeof(paused_pos));
memset(&hold_pos, 0, sizeof(hold_pos));
memset(&final_pos, 0, sizeof(final_pos));
const int pause_get_pos_rc = tpGetPos(&tp, &paused_pos);
for (int hold = 0; hold < 100; ++hold) {
cycle_rc = tpRunCycle(&tp, 1000000);
if (cycle_rc < 0) {
break;
}
}
const int hold_get_pos_rc = tpGetPos(&tp, &hold_pos);
const int paused_velocity_zero = fabs(status.current_vel) < 1e-9 ? 1 : 0;
const int paused_position_frozen =
fabs(paused_pos.tran.x - hold_pos.tran.x) < 1e-6 &&
fabs(paused_pos.tran.y - hold_pos.tran.y) < 1e-6 &&
fabs(paused_pos.tran.z - hold_pos.tran.z) < 1e-6;
const int resume_rc = tpResume(&tp);
int resume_cycles = 0;
for (; resume_cycles < 30000 && !tpIsDone(&tp); ++resume_cycles) {
cycle_rc = tpRunCycle(&tp, 1000000);
if (cycle_rc < 0) {
break;
}
}
const int final_get_pos_rc = tpGetPos(&tp, &final_pos);
const int near_end = pose_near(&final_pos, &end2);
char line[1536];
snprintf(line, sizeof(line),
"tp_pause_create=%d\n"
"tp_pause_set_cycle_time=%d\n"
"tp_pause_set_pos=%d\n"
"tp_pause_set_vmax=%d\n"
"tp_pause_set_vlimit=%d\n"
"tp_pause_set_amax=%d\n"
"tp_pause_set_term_cond=%d\n"
"tp_pause_add_line1=%d\n"
"tp_pause_add_line2=%d\n"
"tp_pause_rc=%d\n"
"tp_pause_cycles_before=%d\n"
"tp_pause_decel_cycles=%d\n"
"tp_pause_get_pos=%d\n"
"tp_pause_hold_get_pos=%d\n"
"tp_paused_velocity_zero=%d\n"
"tp_paused_position_frozen=%d\n"
"tp_resume_rc=%d\n"
"tp_resume_cycles=%d\n"
"tp_resume_get_pos=%d\n"
"tp_resume_done=%d\n"
"tp_pause_resume_final_depth=%d\n"
"tp_pause_resume_final_pos_near_end=%d\n"
"tp_pause_resume_final_pos=%.12g,%.12g,%.12g\n",
create_rc,
set_cycle_rc,
set_pos_rc,
set_vmax_rc,
set_vlimit_rc,
set_amax_rc,
set_term_rc,
add_line1_rc,
add_line2_rc,
pause_rc,
cycles_before_pause,
pause_cycles,
pause_get_pos_rc,
hold_get_pos_rc,
paused_velocity_zero,
paused_position_frozen,
resume_rc,
resume_cycles,
final_get_pos_rc,
tpIsDone(&tp),
tpQueueDepth(&tp),
near_end,
final_pos.tran.x,
final_pos.tran.y,
final_pos.tran.z);
append(out, size, offset, line);
}
EMSCRIPTEN_KEEPALIVE
char *lctp_run_probe(void)
{
@@ -1091,6 +1243,7 @@ char *lctp_run_probe(void)
append_linear_probe(out, 8192, &offset);
append_arc_probe(out, 8192, &offset);
append_queued_lines_probe(out, 8192, &offset);
append_pause_resume_probe(out, 8192, &offset);
return out;
}