62 lines
1.2 KiB
C++
62 lines
1.2 KiB
C++
//
|
|
// Created by Administrator on 2026/4/22.
|
|
//
|
|
|
|
#ifndef RTTHREAD_SERVO_DRIVER_H
|
|
#define RTTHREAD_SERVO_DRIVER_H
|
|
|
|
|
|
#include <rtdevice.h>
|
|
#include <rtthread.h>
|
|
|
|
#include <stdint.h>
|
|
#include <string>
|
|
|
|
class ServoDriver
|
|
{
|
|
public:
|
|
struct Config
|
|
{
|
|
std::string servo_id;
|
|
std::string pwm_dev_name;
|
|
int pwm_channel;
|
|
|
|
rt_uint32_t period_ns;
|
|
rt_uint32_t min_pulse_ns;
|
|
rt_uint32_t max_pulse_ns;
|
|
|
|
float min_angle_rad;
|
|
float max_angle_rad;
|
|
float home_angle_rad;
|
|
};
|
|
|
|
public:
|
|
explicit ServoDriver(const Config& cfg);
|
|
~ServoDriver();
|
|
|
|
rt_err_t init(bool enable_after_init = true, bool go_home = true);
|
|
|
|
rt_err_t enable(bool on);
|
|
rt_err_t setAngle(float angle_rad);
|
|
|
|
const std::string& id() const;
|
|
const Config& config() const;
|
|
|
|
private:
|
|
rt_err_t enableNoLock(bool on);
|
|
rt_err_t setAngleNoLock(float angle_rad);
|
|
|
|
static float clampf(float v, float lo, float hi);
|
|
static rt_uint32_t angleToPulseNs(const Config& cfg, float angle_rad);
|
|
|
|
private:
|
|
Config cfg_;
|
|
rt_device_pwm* pwm_dev_;
|
|
rt_uint32_t last_pulse_;
|
|
bool inited_;
|
|
|
|
rt_mutex_t mutex_;
|
|
};
|
|
|
|
#endif //RTTHREAD_SERVO_DRIVER_H
|