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

Implement AXIS parity UI and execution baseline

Browse Source
This commit is contained in:
wangdequan
2026-08-23 07:28:37 +08:00
parent 1325504544
commit 06f96f85e5
463 changed files with 84893 additions and 692 deletions
Download Patch File Download Diff File Expand all files Collapse all files

143
wasm-core/src/sim_motion_queue.c
View File

@@ -4,6 +4,40 @@
#include <stdio.h>
#include <string.h>
#define MOTION_META_SLOTS 16u
typedef struct motion_meta {
const sim_motion_queue *owner;
int profile;
uint32_t lookahead;
double acceleration_limit;
double jerk_limit;
double current_velocity;
double current_acceleration;
uint32_t paused;
} motion_meta;
static motion_meta motion_metadata[MOTION_META_SLOTS];
static motion_meta *meta_for(const sim_motion_queue *queue, int create) {
motion_meta *free_slot = NULL;
size_t index;
if (queue == NULL) return NULL;
for (index = 0; index < MOTION_META_SLOTS; ++index) {
if (motion_metadata[index].owner == queue) return &motion_metadata[index];
if (free_slot == NULL && motion_metadata[index].owner == NULL) free_slot = &motion_metadata[index];
}
if (!create || free_slot == NULL) return NULL;
memset(free_slot, 0, sizeof(*free_slot));
free_slot->owner = queue;
free_slot->profile = SIM_MOTION_PROFILE_BLEND;
free_slot->lookahead = 1;
free_slot->acceleration_limit = 1e9;
free_slot->jerk_limit = 1e12;
free_slot->paused = 0;
return free_slot;
}
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
@@ -37,7 +71,17 @@ static void zero_state(sim_motion_state *state) {
}
int sim_motion_reset(sim_motion_queue *queue) {
motion_meta *meta;
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
meta = meta_for(queue, 1);
if (meta == NULL) return SIM_MOTION_ERR_BUFFER;
meta->profile = SIM_MOTION_PROFILE_BLEND;
meta->lookahead = 1;
meta->acceleration_limit = 1e9;
meta->jerk_limit = 1e12;
meta->current_velocity = 0.0;
meta->current_acceleration = 0.0;
meta->paused = 0;
queue->head = 0; queue->count = 0; queue->next_id = 1; queue->tick = 0;
zero_state(&queue->state);
return SIM_MOTION_OK;
@@ -50,6 +94,59 @@ int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_
return sim_motion_reset(queue);
}
int sim_motion_set_profile(sim_motion_queue *queue, int profile) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || (profile != SIM_MOTION_PROFILE_BLEND && profile != SIM_MOTION_PROFILE_EXACT_STOP)) return SIM_MOTION_ERR_ARGUMENT;
meta->profile = profile;
return SIM_MOTION_OK;
}
int sim_motion_set_lookahead(sim_motion_queue *queue, uint32_t depth) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || depth == 0 || depth > SIM_MOTION_QUEUE_CAPACITY) return SIM_MOTION_ERR_RANGE;
meta->lookahead = depth;
return SIM_MOTION_OK;
}
int sim_motion_set_dynamics(sim_motion_queue *queue, double acceleration_limit, double jerk_limit) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || !isfinite(acceleration_limit) || !isfinite(jerk_limit) || acceleration_limit <= 0.0 || jerk_limit <= 0.0) return SIM_MOTION_ERR_RANGE;
meta->acceleration_limit = acceleration_limit;
meta->jerk_limit = jerk_limit;
return SIM_MOTION_OK;
}
int sim_motion_set_paused(sim_motion_queue *queue, uint32_t paused) {
motion_meta *meta = meta_for(queue, 1);
if (meta == NULL || paused > 1u) return SIM_MOTION_ERR_ARGUMENT;
meta->paused = paused;
return SIM_MOTION_OK;
}
int sim_motion_peek(const sim_motion_queue *queue, uint32_t offset, sim_motion_command *command) {
uint32_t index;
if (queue == NULL || command == NULL) return SIM_MOTION_ERR_ARGUMENT;
if (offset >= queue->count) return SIM_MOTION_ERR_RANGE;
index = (queue->head + offset) % SIM_MOTION_QUEUE_CAPACITY;
*command = queue->commands[index];
return SIM_MOTION_OK;
}
static double planned_velocity(const sim_motion_queue *queue, uint32_t offset) {
motion_meta *meta = meta_for(queue, 0);
double velocity = 0.0;
uint32_t index;
if (queue == NULL || offset >= queue->count) return 0.0;
index = (queue->head + offset) % SIM_MOTION_QUEUE_CAPACITY;
velocity = queue->commands[index].segment.velocity;
if (meta == NULL || meta->profile == SIM_MOTION_PROFILE_EXACT_STOP) return velocity;
for (index = 1; index <= meta->lookahead && offset + index < queue->count; ++index) {
uint32_t next = (queue->head + offset + index) % SIM_MOTION_QUEUE_CAPACITY;
if (queue->commands[next].segment.velocity < velocity) velocity = queue->commands[next].segment.velocity;
}
return velocity;
}
int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out) {
uint32_t index;
int result;
@@ -77,14 +174,29 @@ int sim_motion_step(sim_motion_queue *queue) {
sim_motion_command *command;
double length;
double delta;
double desired_velocity;
double acceleration;
motion_meta *meta;
int result;
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
meta = meta_for(queue, 1);
if (meta->paused) { queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
if (queue->count == 0) { queue->tick++; queue->state.tick = queue->tick; queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
command = &queue->commands[queue->head];
length = segment_length(&command->segment);
if (length <= 0.0) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
queue->state.active_id = command->id; queue->state.active_tag = command->tag; queue->state.motion_type = command->segment.motion_type;
delta = command->segment.velocity * queue->cycle_time / length;
desired_velocity = planned_velocity(queue, 0);
acceleration = (desired_velocity - meta->current_velocity) / queue->cycle_time;
if (acceleration > meta->current_acceleration + meta->jerk_limit * queue->cycle_time) acceleration = meta->current_acceleration + meta->jerk_limit * queue->cycle_time;
if (acceleration < meta->current_acceleration - meta->jerk_limit * queue->cycle_time) acceleration = meta->current_acceleration - meta->jerk_limit * queue->cycle_time;
if (acceleration > meta->acceleration_limit) acceleration = meta->acceleration_limit;
if (acceleration < -meta->acceleration_limit) acceleration = -meta->acceleration_limit;
meta->current_acceleration = acceleration;
meta->current_velocity += acceleration * queue->cycle_time;
if ((desired_velocity - meta->current_velocity) * (desired_velocity - meta->current_velocity) < 1e-18) meta->current_velocity = desired_velocity;
if (meta->current_velocity < 0.0) meta->current_velocity = 0.0;
delta = meta->current_velocity * queue->cycle_time / length;
if (!isfinite(delta) || delta < 0.0) { queue->state.status = SIM_MOTION_ERR_NONFINITE; return queue->state.status; }
queue->state.progress += delta;
if (queue->state.progress >= 1.0) {
@@ -101,7 +213,8 @@ int sim_motion_step(sim_motion_queue *queue) {
result = sim_posemath_segment_point(&command->segment, queue->state.progress, &queue->state.position);
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
}
queue->state.velocity = command->segment.velocity;
queue->state.velocity = meta->current_velocity;
if (queue->count == 0) { meta->current_velocity = 0.0; meta->current_acceleration = 0.0; queue->state.velocity = 0.0; }
queue->tick++;
queue->state.tick = queue->tick;
queue->state.queue_depth = queue->count;
@@ -137,6 +250,19 @@ uint64_t sim_motion_state_hash(const sim_motion_queue *queue) {
return fnv1a64(bytes, sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int) * 2);
}
uint64_t sim_motion_plan_hash(const sim_motion_queue *queue) {
motion_meta *meta = meta_for(queue, 0);
unsigned char bytes[sizeof(uint32_t) * 3 + sizeof(int) + SIM_MOTION_QUEUE_CAPACITY * sizeof(double)];
size_t offset = 0;
uint32_t index;
if (queue == NULL || meta == NULL) return 0;
memcpy(bytes + offset, &meta->profile, sizeof(meta->profile)); offset += sizeof(meta->profile);
memcpy(bytes + offset, &meta->lookahead, sizeof(meta->lookahead)); offset += sizeof(meta->lookahead);
memcpy(bytes + offset, &queue->count, sizeof(queue->count)); offset += sizeof(queue->count);
for (index = 0; index < queue->count; ++index) { double velocity = planned_velocity(queue, index); memcpy(bytes + offset, &velocity, sizeof(velocity)); offset += sizeof(velocity); }
return fnv1a64(bytes, offset);
}
uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity) {
sim_motion_state state;
char discard[1];
@@ -144,10 +270,15 @@ uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint3
uint32_t destination_capacity = output == 0 ? 1 : capacity;
int written;
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
written = snprintf(destination, destination_capacity,
"{\"active_id\":%u,\"active_tag\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
state.active_id, state.active_tag, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
state.progress, state.queue_depth, state.status, state.tick, state.velocity);
{
motion_meta *meta = meta_for(queue, 0);
const char *profile = meta != NULL && meta->profile == SIM_MOTION_PROFILE_EXACT_STOP ? "exact_stop" : "blend";
uint32_t lookahead = meta == NULL ? 1u : meta->lookahead;
written = snprintf(destination, destination_capacity,
"{\"active_id\":%u,\"active_tag\":%u,\"lookahead\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"profile\":\"%s\",\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
state.active_id, state.active_tag, lookahead, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
profile, state.progress, state.queue_depth, state.status, state.tick, state.velocity);
}
if (written < 0) return 0;
if (output == 0 || capacity <= (uint32_t)written) return (uint32_t)written + 1;
return (uint32_t)written + 1;