Rune Harlyk
37 lines
1.2 KiB
C++
37 lines
1.2 KiB
C++
#include <Arduino.h>
|
|
#include <unity.h>
|
|
#include "esp_timer.h"
|
|
#include "gait/state.h"
|
|
#include "gait/bezier_state.h"
|
|
|
|
void test_gaitPlanner_calculateStep_time() {
|
|
WalkState gaitPlanner;
|
|
body_state_t body_state = {
|
|
128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
|
|
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
|
|
const int num_steps = 1000;
|
|
|
|
uint64_t start = esp_timer_get_time() / 1000;
|
|
for (int i = 0; i < num_steps; i++) {
|
|
gaitPlanner.step(body_state, command, 0.02f);
|
|
}
|
|
uint64_t end = esp_timer_get_time() / 1000;
|
|
|
|
uint64_t duration = end - start;
|
|
uint64_t max_duration = num_steps / 2; // Minimum 0.5 ms per step
|
|
|
|
char message[50];
|
|
snprintf(message, sizeof(message), "The step calculation took: %llu ms (%llu ms per iter)", duration,
|
|
duration / num_steps);
|
|
ESP_LOGI("Test planner", message);
|
|
TEST_ASSERT_MESSAGE(duration <= max_duration, message);
|
|
}
|
|
|
|
void setup() {
|
|
vTaskDelay(2000 / portTICK_PERIOD_MS); // Allow time for Serial to initialize if running on an ESP32
|
|
UNITY_BEGIN();
|
|
RUN_TEST(test_gaitPlanner_calculateStep_time);
|
|
UNITY_END();
|
|
}
|
|
|
|
void loop() {} |