67 lines
1.4 KiB
C++
67 lines
1.4 KiB
C++
//
|
||
// Created by Administrator on 2026/4/22.
|
||
//
|
||
|
||
#ifndef SERVO_CONTROL_H
|
||
#define SERVO_CONTROL_H
|
||
|
||
#include <stdint.h>
|
||
#include <string>
|
||
|
||
#include "common/type/servo_types.h"
|
||
#include "common/curve/include/s_curve.h"
|
||
#include "servo_manager/include/servo_driver.h"
|
||
#include <rtthread.h>
|
||
|
||
|
||
class ServoControl
|
||
{
|
||
public:
|
||
explicit ServoControl(const ServoDriver::Config& cfg);
|
||
~ServoControl();
|
||
|
||
rt_err_t init(bool enable_after_init = true, bool go_home = true);
|
||
|
||
rt_err_t move(float angle_rad, MotionMode mode);
|
||
rt_err_t stop();
|
||
|
||
void setConstraints(double max_velocity_rad,
|
||
double max_acceleration_rad,
|
||
double max_jerk_rad);
|
||
|
||
void update(rt_tick_t now_tick);
|
||
|
||
const std::string& id() const;
|
||
|
||
float currentAngle() const;
|
||
float targetAngle() const;
|
||
float currentVelocity() const;
|
||
|
||
bool isActive() const;
|
||
|
||
private:
|
||
void sampleStateNoLock(rt_tick_t now_tick);
|
||
double ticksToSeconds(rt_tick_t delta_ticks) const;
|
||
|
||
private:
|
||
// 角度变化阈值，rad
|
||
static constexpr float ESP = 1e-4f;
|
||
ServoDriver driver_;
|
||
cmvr::SCurve curve_;
|
||
|
||
float current_angle_rad_;
|
||
float current_velocity_rad_per_sec_;
|
||
float target_angle_rad_;
|
||
|
||
rt_tick_t start_tick_;
|
||
uint32_t generation_;
|
||
|
||
bool active_;
|
||
bool dirty_;
|
||
|
||
cmvr::SCurveProfile profile_;
|
||
|
||
rt_mutex_t mutex_;
|
||
};
|
||
|
||
#endif // SERVO_CONTROL_H
|