55 lines
1.1 KiB
C++
55 lines
1.1 KiB
C++
//
|
|
// Created by Administrator on 2026/4/22.
|
|
//
|
|
|
|
#ifndef SERVO_MANAGER_H
|
|
#define SERVO_MANAGER_H
|
|
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "servo_manager/include/servo_control.h"
|
|
#include "servo_manager/include/servo_driver.h"
|
|
|
|
#include <rtthread.h>
|
|
|
|
class ServoManager
|
|
{
|
|
public:
|
|
explicit ServoManager(const std::vector<ServoDriver::Config>& cfg);
|
|
~ServoManager();
|
|
|
|
rt_err_t init(bool enable_after_init = true, bool go_home = true);
|
|
|
|
ServoControl* get(const std::string& servo_id);
|
|
const ServoControl* get(const std::string& servo_id) const;
|
|
|
|
size_t count() const;
|
|
|
|
rt_err_t start();
|
|
void stop();
|
|
|
|
void setUpdatePeriodMs(rt_uint32_t update_period_ms);
|
|
void wake();
|
|
|
|
private:
|
|
static void threadEntry(void* parameter);
|
|
void run();
|
|
|
|
int findIndexByIdNoLock(const std::string& servo_id) const;
|
|
|
|
private:
|
|
std::vector<ServoControl> controls_;
|
|
|
|
rt_mutex_t mutex_;
|
|
rt_sem_t wake_sem_;
|
|
rt_thread_t thread_;
|
|
|
|
rt_uint32_t update_period_ms_{10};
|
|
volatile bool running_;
|
|
};
|
|
|
|
#endif // SERVO_MANAGER_H
|