120 lines
3.4 KiB
C++
120 lines
3.4 KiB
C++
/*
|
|
* Copyright (c) 2006-2018, RT-Thread Development Team
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* Change Logs:
|
|
* Date Author Notes
|
|
* 2019-03-05 whj4674672 first version
|
|
*/
|
|
|
|
#include <rtdevice.h>
|
|
#include <rtthread.h>
|
|
#include <board.h>
|
|
|
|
#include "planner/s_curve_planner/include/s_curve_position_planner.h"
|
|
|
|
/* defined the LED0 pin: PC13 */
|
|
#define LED0_PIN GET_PIN(C, 13)
|
|
|
|
namespace
|
|
{
|
|
|
|
constexpr rt_int32_t kWarmupIterations = 10;
|
|
constexpr rt_int32_t kBenchmarkIterations = 10000;
|
|
constexpr double kBenchmarkDtSec = 0.01;
|
|
constexpr double kMaxVelocityRad = 3.14;
|
|
constexpr double kMaxAccelerationRad = 31.4;
|
|
constexpr double kMaxJerkRad = 31.4;
|
|
constexpr double kPositionGain =2.0;
|
|
constexpr double kTargetPosA = 1.35;
|
|
constexpr double kTargetPosB = 1.35;
|
|
|
|
volatile double g_planner_sink = 0.0;
|
|
|
|
void run_position_planner_benchmark(const char* label, rt_int32_t target_switch_period)
|
|
{
|
|
cmvr::SCurvePositionPlanner1D planner(
|
|
kMaxVelocityRad,
|
|
kMaxAccelerationRad,
|
|
kMaxJerkRad);
|
|
|
|
planner.setPositionGain(kPositionGain);
|
|
planner.initialize(0.0, 0.0, 0.0);
|
|
planner.setTarget(kTargetPosA);
|
|
|
|
for (rt_int32_t i = 0; i < kWarmupIterations; ++i)
|
|
{
|
|
if (target_switch_period > 0 && (i % target_switch_period) == 0)
|
|
{
|
|
const bool use_target_a = ((i / target_switch_period) & 1) == 0;
|
|
planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB);
|
|
}
|
|
g_planner_sink = planner.update(kBenchmarkDtSec);
|
|
}
|
|
|
|
planner.initialize(0.0, 0.0, 0.0);
|
|
planner.setTarget(kTargetPosA);
|
|
|
|
const rt_tick_t start_tick = rt_tick_get();
|
|
|
|
for (rt_int32_t i = 0; i < kBenchmarkIterations; ++i)
|
|
{
|
|
if (target_switch_period > 0 && (i % target_switch_period) == 0)
|
|
{
|
|
const bool use_target_a = ((i / target_switch_period) & 1) == 0;
|
|
planner.setTarget(use_target_a ? kTargetPosA : kTargetPosB);
|
|
}
|
|
g_planner_sink = planner.update(kBenchmarkDtSec);
|
|
}
|
|
|
|
const rt_tick_t end_tick = rt_tick_get();
|
|
const rt_uint64_t elapsed_ticks = static_cast<rt_uint64_t>(end_tick - start_tick);
|
|
const rt_uint64_t elapsed_us =
|
|
(elapsed_ticks * 1000000ULL) / RT_TICK_PER_SECOND;
|
|
const rt_uint64_t avg_ns =
|
|
(elapsed_ticks * 1000000000ULL) /
|
|
(RT_TICK_PER_SECOND * static_cast<rt_uint64_t>(kBenchmarkIterations));
|
|
|
|
rt_kprintf(
|
|
"[PlannerBench] %s iter=%d dt=%.3fms switch=%d elapsed=%llu us avg=%llu ns pos=%.6f vel=%.6f moving=%d\n",
|
|
label,
|
|
kBenchmarkIterations,
|
|
kBenchmarkDtSec * 1000.0,
|
|
target_switch_period,
|
|
elapsed_us,
|
|
avg_ns,
|
|
planner.getPosition(),
|
|
planner.getVelocity(),
|
|
planner.isMoving() ? 1 : 0);
|
|
}
|
|
|
|
void run_planner_benchmarks()
|
|
{
|
|
rt_kprintf("[PlannerBench] start RT_TICK_PER_SECOND=%d\n", RT_TICK_PER_SECOND);
|
|
run_position_planner_benchmark("hold_target", 0);
|
|
run_position_planner_benchmark("retarget_20", 20);
|
|
run_position_planner_benchmark("retarget_5", 5);
|
|
rt_kprintf("[PlannerBench] done sink=%.6f\n", g_planner_sink);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
int main(void)
|
|
{
|
|
int count = 1;
|
|
|
|
rt_pin_mode(LED0_PIN, PIN_MODE_OUTPUT);
|
|
run_planner_benchmarks();
|
|
|
|
while (count++)
|
|
{
|
|
rt_pin_write(LED0_PIN, PIN_HIGH);
|
|
rt_thread_mdelay(100);
|
|
rt_pin_write(LED0_PIN, PIN_LOW);
|
|
rt_thread_mdelay(100);
|
|
}
|
|
|
|
return RT_EOK;
|
|
}
|