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
|