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
|
|||
|
|
|