75 lines
3.8 KiB
C
75 lines
3.8 KiB
C
#include "sim_motion_queue.h"
|
|
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
|
|
static sim_posemath_segment line(double x0, double x1) {
|
|
sim_posemath_segment segment = {
|
|
{{x0, 0, 0}, 0, 0, 0, 0, 0, 0},
|
|
{{x1, 0, 0}, 0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0}, {0, 0, 1}, 1.0, 1.0, SIM_POSEMATH_LINEAR, 0
|
|
};
|
|
return segment;
|
|
}
|
|
|
|
int main(void) {
|
|
sim_motion_queue queue = {0};
|
|
sim_motion_command command = {0};
|
|
sim_motion_state state = {0};
|
|
char canonical[512] = {0};
|
|
uint32_t id = 0;
|
|
command.segment = line(0, 1);
|
|
command.tag = 42;
|
|
assert(sim_motion_init(&queue, 0.1, 10.0) == SIM_MOTION_OK);
|
|
assert(sim_motion_enqueue(&queue, &command, &id) == SIM_MOTION_OK && id == 1);
|
|
assert(sim_motion_set_paused(&queue, 1) == SIM_MOTION_OK);
|
|
assert(sim_motion_step(&queue) == SIM_MOTION_ERR_STATE && queue.tick == 0);
|
|
assert(sim_motion_set_paused(&queue, 0) == SIM_MOTION_OK);
|
|
command.segment = line(1, 2);
|
|
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_OK);
|
|
assert(sim_motion_set_profile(&queue, SIM_MOTION_PROFILE_BLEND) == SIM_MOTION_OK);
|
|
assert(sim_motion_set_lookahead(&queue, 2) == SIM_MOTION_OK);
|
|
assert(sim_motion_set_dynamics(&queue, 2.0, 20.0) == SIM_MOTION_OK);
|
|
assert(sim_motion_plan_hash(&queue) != 0);
|
|
{
|
|
sim_motion_command peek = {0};
|
|
assert(sim_motion_peek(&queue, 0, &peek) == SIM_MOTION_OK && peek.id == 1);
|
|
}
|
|
assert(sim_motion_step(&queue) == SIM_MOTION_OK);
|
|
assert(sim_motion_read_state(&queue, &state) == SIM_MOTION_OK);
|
|
assert(state.tick == 1 && state.queue_depth == 2 && state.position.tran.x > 0.01);
|
|
assert(sim_motion_state_hash(&queue) != 0);
|
|
assert(sim_motion_canonical(&queue, canonical, sizeof(canonical)) > 1);
|
|
assert(strstr(canonical, "\"profile\":\"blend\"") != NULL);
|
|
assert(sim_motion_set_profile(&queue, SIM_MOTION_PROFILE_EXACT_STOP) == SIM_MOTION_OK);
|
|
assert(sim_motion_set_lookahead(&queue, 0) == SIM_MOTION_ERR_RANGE);
|
|
assert(sim_motion_set_dynamics(&queue, 0.0, 1.0) == SIM_MOTION_ERR_RANGE);
|
|
{
|
|
sim_motion_queue override_queue = {0};
|
|
sim_motion_state override_state = {0};
|
|
sim_motion_command override_command = {0};
|
|
override_command.segment = line(0, 1);
|
|
assert(sim_motion_init(&override_queue, 0.1, 10.0) == SIM_MOTION_OK);
|
|
assert(sim_motion_set_feed_override(&override_queue, 0.5) == SIM_MOTION_OK);
|
|
assert(sim_motion_set_feed_override(&override_queue, 0.5) == SIM_MOTION_OK);
|
|
assert(sim_motion_set_feed_override(&override_queue, -0.1) == SIM_MOTION_ERR_RANGE);
|
|
assert(sim_motion_set_feed_override(&override_queue, 2.1) == SIM_MOTION_ERR_RANGE);
|
|
assert(sim_motion_enqueue(&override_queue, &override_command, 0) == SIM_MOTION_OK);
|
|
assert(sim_motion_step(&override_queue) == SIM_MOTION_OK);
|
|
assert(sim_motion_read_state(&override_queue, &override_state) == SIM_MOTION_OK);
|
|
assert(override_state.position.tran.x > 0.049 && override_state.position.tran.x < 0.051);
|
|
assert(override_state.velocity > 0.49 && override_state.velocity < 0.51);
|
|
sim_motion_reset(&override_queue);
|
|
override_command.flags = SIM_MOTION_COMMAND_FLAG_RAPID;
|
|
assert(sim_motion_enqueue(&override_queue, &override_command, 0) == SIM_MOTION_OK);
|
|
assert(sim_motion_step(&override_queue) == SIM_MOTION_OK);
|
|
assert(sim_motion_read_state(&override_queue, &override_state) == SIM_MOTION_OK);
|
|
assert(override_state.position.tran.x > 0.099 && override_state.position.tran.x < 0.101);
|
|
}
|
|
command.segment = line(9, 10);
|
|
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_ERR_STATE);
|
|
command.segment.motion_type = 99;
|
|
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_ERR_MOTION_TYPE);
|
|
return 0;
|
|
}
|