48 lines
2.0 KiB
C
48 lines
2.0 KiB
C
#include "sim_linuxcnc_tp.h"
|
|
|
|
#include <assert.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, 0.001, 35.0, 300.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 < 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);
|
|
puts(canonical);
|
|
free(planner);
|
|
return 0;
|
|
}
|