278 lines
8.0 KiB
C++
278 lines
8.0 KiB
C++
|
//
|
||
|
|
// Created by Administrator on 2025/12/24.
|
||
|
|
//
|
||
|
|
|
||
|
|
#include "erpc/service/include/servo_service_impl.h"
|
||
|
|
#include <string>
|
||
|
|
#include "common/type/common_type.h"
|
||
|
|
|
||
|
|
const std::vector<ServoDriver::Config> ServoServiceImpl::kServoCfg = {
|
||
|
|
{"eye_l_up", "pwm1", 1, 20000000, 500000, 2500000, -0.5f * PI, 0.5f * PI, 0.0f},
|
||
|
|
// {"eye_r_up", "pwm1", 2, 20000000, 500000, 2500000, 0.0f, kPi, 0.5f * kPi},
|
||
|
|
// ...
|
||
|
|
};
|
||
|
|
|
||
|
|
ServoServiceImpl::ServoServiceImpl()
|
||
|
|
: manager_(kServoCfg),
|
||
|
|
init_err_(RT_EOK),
|
||
|
|
mode_(MotionMode::Immediate),
|
||
|
|
mutex_(RT_NULL)
|
||
|
|
{
|
||
|
|
mutex_ = rt_mutex_create("sv_svc", RT_IPC_FLAG_PRIO);
|
||
|
|
if (!mutex_) {
|
||
|
|
init_err_ = -RT_ENOMEM;
|
||
|
|
rt_kprintf("[ServoService][E] mutex create FAILED, err=%d\n", (int)init_err_);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
init_err_ = manager_.init(true, true);
|
||
|
|
if (init_err_ != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] init FAILED, err=%d\n", (int)init_err_);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
manager_.setUpdatePeriodMs(1);
|
||
|
|
|
||
|
|
for (const auto& cfg : kServoCfg) {
|
||
|
|
ServoControl* servo = manager_.get(cfg.servo_id);
|
||
|
|
if (!servo) {
|
||
|
|
init_err_ = -RT_ENOSYS;
|
||
|
|
rt_kprintf("[ServoService][E] missing servo control, id=%s\n", cfg.servo_id.c_str());
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
servo->setConstraints(
|
||
|
|
3.14,
|
||
|
|
31.4,
|
||
|
|
31.4);
|
||
|
|
}
|
||
|
|
|
||
|
|
init_err_ = manager_.start();
|
||
|
|
if (init_err_ != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] manager start FAILED, err=%d\n", (int)init_err_);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ServoServiceImpl::~ServoServiceImpl()
|
||
|
|
{
|
||
|
|
if (mutex_) {
|
||
|
|
rt_mutex_delete(mutex_);
|
||
|
|
mutex_ = RT_NULL;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
bool ServoServiceImpl::setConstraints(const char* id,
|
||
|
|
float max_velocity_rad,
|
||
|
|
float max_acceleration_rad,
|
||
|
|
float max_jerk_rad)
|
||
|
|
{
|
||
|
|
if (init_err_ != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] setConstraints: service not ready, init_err=%d\n",
|
||
|
|
(int)init_err_);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!id) {
|
||
|
|
rt_kprintf("[ServoService][E] setConstraints: id is null\n");
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (max_velocity_rad <= 0.0f ||
|
||
|
|
max_acceleration_rad <= 0.0f ||
|
||
|
|
max_jerk_rad <= 0.0f) {
|
||
|
|
rt_kprintf("[ServoService][E] setConstraints: invalid constraints, id=%s v=%.4f a=%.4f j=%.4f\n",
|
||
|
|
id,
|
||
|
|
(double)max_velocity_rad,
|
||
|
|
(double)max_acceleration_rad,
|
||
|
|
(double)max_jerk_rad);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
ServoControl* servo = manager_.get(std::string(id));
|
||
|
|
if (!servo) {
|
||
|
|
rt_kprintf("[ServoService][E] setConstraints: servo not found, id=%s\n", id);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
servo->setConstraints(max_velocity_rad, max_acceleration_rad, max_jerk_rad);
|
||
|
|
rt_kprintf("[ServoService] setConstraints: id=%s v=%.4f a=%.4f j=%.4f\n",
|
||
|
|
id,
|
||
|
|
(double)max_velocity_rad,
|
||
|
|
(double)max_acceleration_rad,
|
||
|
|
(double)max_jerk_rad);
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool ServoServiceImpl::setMode(RpcMotionMode mode)
|
||
|
|
{
|
||
|
|
if (init_err_ != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] setMode: service not ready, init_err=%d\n",
|
||
|
|
(int)init_err_);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!mutex_) {
|
||
|
|
rt_kprintf("[ServoService][E] setMode: mutex is null\n");
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
MotionMode motion_mode;
|
||
|
|
switch (mode) {
|
||
|
|
case RpcMotionModeImmediate:
|
||
|
|
motion_mode = MotionMode::Immediate;
|
||
|
|
break;
|
||
|
|
case RpcMotionModeSCurve:
|
||
|
|
motion_mode = MotionMode::SCurve;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
rt_kprintf("[ServoService][E] setMode: invalid mode=%d\n", (int)mode);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
rt_mutex_take(mutex_, RT_WAITING_FOREVER);
|
||
|
|
mode_ = motion_mode;
|
||
|
|
rt_mutex_release(mutex_);
|
||
|
|
|
||
|
|
rt_kprintf("[ServoService] setMode: mode=%d\n", (int)mode);
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool ServoServiceImpl::setUpdatePeriodMs(uint32_t ms)
|
||
|
|
{
|
||
|
|
if (init_err_ != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] setUpdatePeriodMs: service not ready, init_err=%d\n",
|
||
|
|
(int)init_err_);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
manager_.setUpdatePeriodMs(ms);
|
||
|
|
manager_.wake();
|
||
|
|
|
||
|
|
rt_kprintf("[ServoService] setUpdatePeriodMs: ms=%u\n", (unsigned)ms);
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
int32_t ServoServiceImpl::move(const list_ServoCmd_1_t* cmds)
|
||
|
|
{
|
||
|
|
if (init_err_ != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] move: service not ready, init_err=%d\n",
|
||
|
|
(int)init_err_);
|
||
|
|
return init_err_;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!cmds) {
|
||
|
|
rt_kprintf("[ServoService][E] move: cmds is null\n");
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!cmds->elements || cmds->elementsCount == 0) {
|
||
|
|
rt_kprintf("[ServoService][E] move: empty list (elements=%p, count=%d)\n",
|
||
|
|
cmds->elements,
|
||
|
|
(int)cmds->elementsCount);
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!mutex_) {
|
||
|
|
rt_kprintf("[ServoService][E] move: mutex is null\n");
|
||
|
|
return -RT_ENOMEM;
|
||
|
|
}
|
||
|
|
|
||
|
|
rt_mutex_take(mutex_, RT_WAITING_FOREVER);
|
||
|
|
const MotionMode motion_mode = mode_;
|
||
|
|
rt_mutex_release(mutex_);
|
||
|
|
|
||
|
|
for (uint32_t i = 0; i < cmds->elementsCount; ++i) {
|
||
|
|
const char* id = cmds->elements[i].id;
|
||
|
|
if (!id) {
|
||
|
|
rt_kprintf("[ServoService][E] move: cmds[%d].id is null\n", (int)i);
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
ServoControl* servo = manager_.get(std::string(id));
|
||
|
|
if (!servo) {
|
||
|
|
rt_kprintf("[ServoService][E] move: servo not found, id=%s\n", id);
|
||
|
|
return -RT_ENOSYS;
|
||
|
|
}
|
||
|
|
|
||
|
|
const rt_err_t err = servo->move(cmds->elements[i].angle_rad, motion_mode);
|
||
|
|
if (err != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] move: move failed, id=%s angle=%.4f mode=%d err=%d\n",
|
||
|
|
id,
|
||
|
|
(double)cmds->elements[i].angle_rad,
|
||
|
|
(int)motion_mode,
|
||
|
|
(int)err);
|
||
|
|
return err;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (motion_mode == MotionMode::SCurve) {
|
||
|
|
manager_.wake();
|
||
|
|
}
|
||
|
|
|
||
|
|
return RT_EOK;
|
||
|
|
}
|
||
|
|
|
||
|
|
int32_t ServoServiceImpl::moveJ(const list_float_1_t* angles_rad)
|
||
|
|
{
|
||
|
|
if (init_err_ != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] moveJ: service not ready, init_err=%d\n",
|
||
|
|
(int)init_err_);
|
||
|
|
return init_err_;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!angles_rad) {
|
||
|
|
rt_kprintf("[ServoService][E] moveJ: angles_rad is null\n");
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!angles_rad->elements || angles_rad->elementsCount == 0) {
|
||
|
|
rt_kprintf("[ServoService][E] moveJ: empty list (elements=%p, count=%d)\n",
|
||
|
|
angles_rad->elements,
|
||
|
|
(int)angles_rad->elementsCount);
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
const size_t need = kServoCfg.size();
|
||
|
|
const size_t got = (size_t)angles_rad->elementsCount;
|
||
|
|
if (got != need) {
|
||
|
|
rt_kprintf("[ServoService][E] moveJ: invalid angles count, need=%d got=%d\n",
|
||
|
|
(int)need,
|
||
|
|
(int)got);
|
||
|
|
return -RT_EINVAL;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!mutex_) {
|
||
|
|
rt_kprintf("[ServoService][E] moveJ: mutex is null\n");
|
||
|
|
return -RT_ENOMEM;
|
||
|
|
}
|
||
|
|
|
||
|
|
rt_mutex_take(mutex_, RT_WAITING_FOREVER);
|
||
|
|
const MotionMode motion_mode = mode_;
|
||
|
|
rt_mutex_release(mutex_);
|
||
|
|
|
||
|
|
for (size_t i = 0; i < need; ++i) {
|
||
|
|
ServoControl* servo = manager_.get(kServoCfg[i].servo_id);
|
||
|
|
if (!servo) {
|
||
|
|
rt_kprintf("[ServoService][E] moveJ: servo not found, id=%s\n",
|
||
|
|
kServoCfg[i].servo_id.c_str());
|
||
|
|
return -RT_ENOSYS;
|
||
|
|
}
|
||
|
|
|
||
|
|
const rt_err_t err = servo->move(angles_rad->elements[i], motion_mode);
|
||
|
|
if (err != RT_EOK) {
|
||
|
|
rt_kprintf("[ServoService][E] moveJ: move failed, id=%s angle=%.4f mode=%d err=%d\n",
|
||
|
|
kServoCfg[i].servo_id.c_str(),
|
||
|
|
(double)angles_rad->elements[i],
|
||
|
|
(int)motion_mode,
|
||
|
|
(int)err);
|
||
|
|
return err;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (motion_mode == MotionMode::SCurve) {
|
||
|
|
manager_.wake();
|
||
|
|
}
|
||
|
|
|
||
|
|
return RT_EOK;
|
||
|
|
}
|