60 lines
3.4 KiB
C
60 lines
3.4 KiB
C
#include "sim_hal.h"
|
|
|
|
#include <assert.h>
|
|
#include <stdio.h>
|
|
|
|
static float get_float(const char *name) {
|
|
sim_hal_value value = {0};
|
|
assert(sim_hal_get_pin(name, &value) == SIM_HAL_OK);
|
|
assert(value.type == SIM_HAL_FLOAT);
|
|
return value.value.real;
|
|
}
|
|
|
|
int sim_hal_test_run(void) {
|
|
sim_hal_value bit = {SIM_HAL_BIT, {.bit = 1}};
|
|
sim_hal_value wrong = {SIM_HAL_S32, {.s32 = 1}};
|
|
sim_hal_value out = {0};
|
|
|
|
sim_hal_reset();
|
|
assert(sim_hal_register_pin("source", SIM_HAL_BIT) == SIM_HAL_OK);
|
|
assert(sim_hal_register_pin("target", SIM_HAL_BIT) == SIM_HAL_OK);
|
|
assert(sim_hal_register_pin("source", SIM_HAL_BIT) == SIM_HAL_ERR_DUPLICATE);
|
|
assert(sim_hal_set_pin("source", bit) == SIM_HAL_OK);
|
|
assert(sim_hal_set_pin("source", wrong) == SIM_HAL_ERR_TYPE);
|
|
assert(sim_hal_connect("source", "target") == SIM_HAL_OK);
|
|
assert(sim_hal_get_pin("target", &out) == SIM_HAL_OK && out.value.bit == 1);
|
|
assert(sim_hal_register_thread("servo", 2) == SIM_HAL_OK);
|
|
assert(sim_hal_register_thread("servo", 2) == SIM_HAL_ERR_DUPLICATE);
|
|
assert(sim_hal_register_function("servo", "update") == SIM_HAL_OK);
|
|
assert(sim_hal_register_function("missing", "bad") == SIM_HAL_ERR_NOT_FOUND);
|
|
assert(sim_hal_step(5) == SIM_HAL_OK);
|
|
assert(sim_hal_tick() == 5 && sim_hal_function_calls("update") == 2);
|
|
assert(sim_hal_set_function_failure("update", 1) == SIM_HAL_OK);
|
|
assert(sim_hal_step(1) == SIM_HAL_ERR_FUNCTION);
|
|
assert(sim_hal_tick() == 5);
|
|
assert(sim_hal_register_thread("invalid", 0) == SIM_HAL_ERR_RANGE);
|
|
|
|
sim_hal_reset();
|
|
assert(sim_hal_register_thread("servo-thread", 1) == SIM_HAL_OK);
|
|
assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_OK);
|
|
assert(sim_hal_set_pin("corexy_by_hal.j0-motor-pos-cmd", (sim_hal_value){SIM_HAL_FLOAT, {.real = 6.0f}}) == SIM_HAL_OK);
|
|
assert(sim_hal_set_pin("corexy_by_hal.j1-motor-pos-cmd", (sim_hal_value){SIM_HAL_FLOAT, {.real = 2.0f}}) == SIM_HAL_OK);
|
|
assert(sim_hal_set_pin("corexy_by_hal.alpha-fb", (sim_hal_value){SIM_HAL_FLOAT, {.real = 4.0f}}) == SIM_HAL_OK);
|
|
assert(sim_hal_set_pin("corexy_by_hal.beta-fb", (sim_hal_value){SIM_HAL_FLOAT, {.real = 1.0f}}) == SIM_HAL_OK);
|
|
assert(sim_hal_step(1) == SIM_HAL_OK);
|
|
assert(get_float("corexy_by_hal.alpha-cmd") == 4.0f);
|
|
assert(get_float("corexy_by_hal.beta-cmd") == 2.0f);
|
|
assert(get_float("corexy_by_hal.j0-motor-pos-fb") == 5.0f);
|
|
assert(get_float("corexy_by_hal.j1-motor-pos-fb") == 3.0f);
|
|
assert(sim_hal_function_calls("corexy_by_hal") == 1);
|
|
assert(sim_hal_corexy_transform("missing") == SIM_HAL_ERR_NOT_FOUND);
|
|
assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_ERR_DUPLICATE);
|
|
printf("{\"schema\":\"hal-corexy-sample.v1\",\"command\":{\"j0\":%.3f,\"j1\":%.3f,\"alpha\":%.3f,\"beta\":%.3f},\"feedback\":{\"alpha\":%.3f,\"beta\":%.3f,\"j0\":%.3f,\"j1\":%.3f},\"tick\":%u,\"function_calls\":%u}\n",
|
|
(double)get_float("corexy_by_hal.j0-motor-pos-cmd"), (double)get_float("corexy_by_hal.j1-motor-pos-cmd"),
|
|
(double)get_float("corexy_by_hal.alpha-cmd"), (double)get_float("corexy_by_hal.beta-cmd"),
|
|
(double)get_float("corexy_by_hal.alpha-fb"), (double)get_float("corexy_by_hal.beta-fb"),
|
|
(double)get_float("corexy_by_hal.j0-motor-pos-fb"), (double)get_float("corexy_by_hal.j1-motor-pos-fb"),
|
|
sim_hal_tick(), sim_hal_function_calls("corexy_by_hal"));
|
|
return 0;
|
|
}
|