131 lines
7.1 KiB
C
131 lines
7.1 KiB
C
#include "sim_linuxcnc_tp.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
static sim_posemath_pose pose(double x, double y, double z) {
|
|
sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0};
|
|
return value;
|
|
}
|
|
|
|
int main(void) {
|
|
sim_linuxcnc_tp *planner = calloc(1, sim_linuxcnc_tp_size());
|
|
sim_tp_state state = {0};
|
|
char canonical[512] = {0};
|
|
sim_posemath_pose origin = pose(0, 0, 0);
|
|
sim_posemath_pose first = pose(0, 0, 10);
|
|
sim_posemath_pose second = pose(20, 20, 10);
|
|
assert(planner != NULL);
|
|
assert(sim_linuxcnc_tp_init(planner, .001, 35, 300) == 0);
|
|
assert(sim_linuxcnc_tp_set_axis_dynamics(planner, 0, 7, 40) == 0);
|
|
assert(fabs(sim_linuxcnc_tp_jog_velocity(planner, 0) - 7) < 1e-9);
|
|
assert(sim_linuxcnc_tp_set_axis_dynamics(planner, 4, 11, 20) == 0);
|
|
assert(fabs(sim_linuxcnc_tp_jog_velocity(planner, 4) - 11) < 1e-9);
|
|
assert(sim_linuxcnc_tp_set_axis_dynamics(planner, 9, 7, 40) != 0);
|
|
assert(sim_linuxcnc_tp_set_axis_dynamics(planner, 0, NAN, 40) != 0);
|
|
assert(sim_linuxcnc_tp_set_axis_dynamics(planner, 0, 7, 0) != 0);
|
|
assert(sim_linuxcnc_tp_set_feed_limit(planner, 1.3) == 0);
|
|
assert(sim_linuxcnc_tp_set_feed_override(planner, 1.3) == 0);
|
|
assert(sim_linuxcnc_tp_set_feed_override(planner, 1.31) != 0);
|
|
assert(sim_linuxcnc_tp_set_scale_minimum(planner, .02) == 0);
|
|
assert(sim_linuxcnc_tp_set_jog_scale(planner, .02) == 0);
|
|
assert(sim_linuxcnc_tp_set_velocity_scale(planner, .02) == 0);
|
|
assert(sim_linuxcnc_tp_set_jog_scale(planner, .01) != 0);
|
|
assert(sim_linuxcnc_tp_set_scale_minimum(planner, NAN) != 0);
|
|
assert(sim_linuxcnc_tp_set_blend(planner, 1, 0, 25, 3, 50, .2) == 0);
|
|
assert(sim_linuxcnc_tp_set_blend(planner, 2, 0, 25, 3, 50, .2) != 0);
|
|
assert(sim_linuxcnc_tp_set_blend(planner, 1, 0, 0, 3, 50, .2) != 0);
|
|
assert(sim_linuxcnc_tp_set_blend(planner, 1, 0, 25, 3, NAN, .2) != 0);
|
|
assert(sim_linuxcnc_tp_init(planner, 0.001, 35.0, 300.0) == 0);
|
|
assert(sim_linuxcnc_tp_queue_capacity(planner) > 0);
|
|
assert(sim_linuxcnc_tp_velocity_mm_min(planner) == 0.0);
|
|
assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0);
|
|
assert(sim_linuxcnc_tp_set_profile(planner, 0, 0.0) == 0);
|
|
assert(sim_linuxcnc_tp_add_line(planner, &first, 20.0, 300.0, 16, 1) == 0);
|
|
assert(sim_linuxcnc_tp_add_line(planner, &second, 35.0, 300.0, 18, 1) == 0);
|
|
for (int index = 0; index < 20; ++index) assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
uint32_t checkpoint_size = sim_linuxcnc_tp_checkpoint_size();
|
|
uint8_t *checkpoint = malloc(checkpoint_size);
|
|
uint64_t continuation[100];
|
|
assert(checkpoint != NULL);
|
|
assert(sim_linuxcnc_tp_checkpoint_export(planner, checkpoint, checkpoint_size) == 0);
|
|
for (int index = 0; index < 100; ++index) {
|
|
assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
continuation[index] = sim_linuxcnc_tp_state_hash(planner);
|
|
}
|
|
const uint64_t before_invalid = sim_linuxcnc_tp_state_hash(planner);
|
|
assert(sim_linuxcnc_tp_checkpoint_restore(planner, checkpoint, checkpoint_size - 1) != 0);
|
|
checkpoint[0] ^= 1;
|
|
assert(sim_linuxcnc_tp_checkpoint_restore(planner, checkpoint, checkpoint_size) != 0);
|
|
assert(sim_linuxcnc_tp_state_hash(planner) == before_invalid);
|
|
checkpoint[0] ^= 1;
|
|
assert(sim_linuxcnc_tp_checkpoint_restore(planner, checkpoint, checkpoint_size) == 0);
|
|
for (int index = 0; index < 100; ++index) {
|
|
assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
assert(sim_linuxcnc_tp_state_hash(planner) == continuation[index]);
|
|
}
|
|
free(checkpoint);
|
|
for (int index = 0; index < 5000; ++index) {
|
|
int result = sim_linuxcnc_tp_step(planner);
|
|
if (result != 0) fprintf(stderr, "tp_step[%d]=%d\\n", index, result);
|
|
assert(result == 0);
|
|
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
|
|
if (state.queue_depth == 0) break;
|
|
}
|
|
assert(state.queue_depth == 0);
|
|
assert(state.position.tran.x > 19.9 && state.position.tran.y > 19.9 && state.position.tran.z > 9.9);
|
|
assert(sim_linuxcnc_tp_state_hash(planner) != 0);
|
|
assert(sim_linuxcnc_tp_canonical(planner, canonical, sizeof(canonical)) > 1);
|
|
assert(sim_linuxcnc_tp_reset(planner) == 0);
|
|
assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0);
|
|
assert(sim_linuxcnc_tp_set_axis_limit(planner, 0, -5.0, 5.0) == 0);
|
|
assert(sim_linuxcnc_tp_set_jog_scale(planner, 0.5) == 0);
|
|
assert(sim_linuxcnc_tp_jog(planner, 0, 1, 1.0, 21) == 0);
|
|
assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
|
|
assert(state.position.tran.x > 0.0 && sim_linuxcnc_tp_jog(planner, 0, 1, 100.0, 22) != 0);
|
|
assert(sim_linuxcnc_tp_abort(planner) == 0);
|
|
assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0);
|
|
assert(sim_linuxcnc_tp_jog(planner, 0, 1, -1.0, 23) != 0);
|
|
assert(sim_linuxcnc_tp_jog(planner, 0, 1, NAN, 23) != 0);
|
|
assert(sim_linuxcnc_tp_jog(planner, 0, 1, 0.0, 23) == 0);
|
|
for (int index = 0; index < 3000; ++index) {
|
|
assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
|
|
assert(state.position.tran.x <= 5.0);
|
|
if (state.queue_depth == 0) break;
|
|
}
|
|
assert(state.queue_depth == 0 && fabs(state.position.tran.x - 5.0) < 1e-9);
|
|
assert(sim_linuxcnc_tp_jog(planner, 0, 1, 0.0, 24) != 0);
|
|
assert(sim_linuxcnc_tp_jog(planner, 0, -1, 0.0, 25) == 0);
|
|
for (int index = 0; index < 50; ++index) assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
|
|
assert(state.position.tran.x < 5.0 && state.position.tran.x > -5.0);
|
|
assert(sim_linuxcnc_tp_abort(planner) == 0);
|
|
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0 && state.queue_depth == 0);
|
|
assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0);
|
|
first = pose(1, 0, 0); second = pose(2, 0, 0);
|
|
assert(sim_linuxcnc_tp_add_line(planner, &first, 10, 300, 101, 1) == 0);
|
|
assert(sim_linuxcnc_tp_add_line(planner, &second, 10, 300, 102, 1) == 0);
|
|
assert(sim_linuxcnc_tp_set_paused(planner, 1) == 0);
|
|
assert(sim_linuxcnc_tp_step_line(planner) == 0);
|
|
for (int tick = 0; tick < 3000 && sim_linuxcnc_tp_is_stepping(planner); ++tick) assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
assert(!sim_linuxcnc_tp_is_stepping(planner));
|
|
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
|
|
/* motion/control.c detects an ID transition after the servo sample;
|
|
a transition may advance less than one cycle into the following TC. */
|
|
assert(state.position.tran.x >= 1.0 && state.position.tran.x < 1.0 + 10.0 * 0.001);
|
|
assert(sim_linuxcnc_tp_queue_depth(planner) == 1);
|
|
assert(sim_linuxcnc_tp_step_line(planner) == 0);
|
|
for (int tick = 0; tick < 3000 && sim_linuxcnc_tp_is_stepping(planner); ++tick) assert(sim_linuxcnc_tp_step(planner) == 0);
|
|
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
|
|
assert(fabs(state.position.tran.x - 2.0) < 1e-9);
|
|
assert(sim_linuxcnc_tp_axis_limit(planner, 0, 0) == -5.0);
|
|
assert(sim_linuxcnc_tp_axis_limit(planner, 0, 1) == 5.0);
|
|
puts(canonical);
|
|
free(planner);
|
|
return 0;
|
|
}
|