84 lines
1.9 KiB
C++
84 lines
1.9 KiB
C++
//
|
||
// Created by Administrator on 2026/1/7.
|
||
//
|
||
|
||
#ifndef SERVO_MANAGER_H
|
||
#define SERVO_MANAGER_H
|
||
|
||
extern "C" {
|
||
#include <rtthread.h>
|
||
#include <rtdevice.h>
|
||
}
|
||
|
||
#include <stdint.h>
|
||
#include <stddef.h>
|
||
#include <vector>
|
||
|
||
class ServoManager
|
||
{
|
||
public:
|
||
struct ServoConfig
|
||
{
|
||
const char* servo_id; // 唯一ID
|
||
const char* pwm_dev_name;
|
||
int pwm_channel;
|
||
|
||
// ns
|
||
rt_uint32_t period_ns;
|
||
rt_uint32_t min_pulse_ns;
|
||
rt_uint32_t max_pulse_ns;
|
||
|
||
float min_angle_deg;
|
||
float max_angle_deg;
|
||
float home_angle_deg;
|
||
};
|
||
|
||
struct ServoCmd
|
||
{
|
||
const char* id;
|
||
float angle_deg;
|
||
};
|
||
|
||
public:
|
||
|
||
explicit ServoManager(const std::vector<ServoConfig> &cfg);
|
||
~ServoManager();
|
||
|
||
size_t count() const { return _cfg.size(); }
|
||
|
||
rt_err_t init(bool enable_after_init = true, bool go_home = true);
|
||
|
||
rt_err_t enable(const char* servo_id, bool on);
|
||
rt_err_t setAngle(const char* servo_id, float angle_deg);
|
||
|
||
// 全部设置：两种形态（vector / 指针）
|
||
rt_err_t setAllAngles(const std::vector<float>& angles_deg);
|
||
rt_err_t setAllAngles(const float* angles_deg, size_t n);
|
||
|
||
// 批量设置：两种形态（vector / 指针）
|
||
rt_err_t setAngles(const std::vector<ServoCmd>& cmds);
|
||
rt_err_t setAngles(const ServoCmd* cmds, size_t n);
|
||
|
||
const char* idAt(size_t order) const;
|
||
int orderOf(const char* servo_id) const;
|
||
|
||
private:
|
||
rt_err_t enableNoLock(size_t order, bool on);
|
||
rt_err_t setAngleNoLock(size_t order, float angle_deg);
|
||
|
||
int findOrderByIdNoLock(const char* servo_id) const;
|
||
|
||
static float clampf(float v, float lo, float hi);
|
||
static rt_uint32_t angleToPulseNs(const ServoConfig& c, float angle_deg);
|
||
|
||
private:
|
||
std::vector<ServoConfig> _cfg;
|
||
std::vector<rt_device_pwm*> _pwm_dev;
|
||
std::vector<rt_uint32_t> _last_pulse;
|
||
|
||
rt_mutex_t _mutex;
|
||
};
|
||
|
||
#endif // SERVO_MANAGER_H
|
||
|