88 lines
1.9 KiB
C++
88 lines
1.9 KiB
C++
//
|
|
// Created by Administrator on 2026/4/22.
|
|
//
|
|
|
|
#ifndef SERVO_CONTROL_H
|
|
#define SERVO_CONTROL_H
|
|
|
|
#include <stdint.h>
|
|
#include <string>
|
|
|
|
#include <rtthread.h>
|
|
|
|
#include "common/curve/include/s_curve.h"
|
|
#include "common/type/servo_types.h"
|
|
#include "planner/s_curve_planner/include/s_curve_position_planner.h"
|
|
#include "servo_manager/include/servo_driver.h"
|
|
|
|
struct ServoConfig;
|
|
|
|
class ServoControl
|
|
{
|
|
public:
|
|
struct Config
|
|
{
|
|
double max_velocity_rad{6.28};
|
|
double max_acceleration_rad{100.0};
|
|
double max_jerk_rad{500.0};
|
|
double position_gain{8.0};
|
|
};
|
|
|
|
explicit ServoControl(const ServoConfig& 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 setPositionGain(double position_gain);
|
|
|
|
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:
|
|
static constexpr float ESP = 1e-4f;
|
|
|
|
std::string id_;
|
|
float limit_min_angle_rad_;
|
|
float limit_max_angle_rad_;
|
|
float home_angle_rad_;
|
|
Config cfg_;
|
|
ServoDriver driver_;
|
|
cmvr::SCurve curve_;
|
|
cmvr::SCurvePositionPlanner1D follow_planner_;
|
|
|
|
float current_angle_rad_;
|
|
float current_velocity_rad_per_sec_;
|
|
float target_angle_rad_;
|
|
|
|
rt_tick_t start_tick_;
|
|
rt_tick_t last_update_tick_;
|
|
uint32_t generation_;
|
|
MotionMode active_mode_;
|
|
|
|
bool active_;
|
|
bool dirty_;
|
|
|
|
cmvr::SCurveProfile profile_;
|
|
|
|
rt_mutex_t mutex_;
|
|
};
|
|
|
|
#endif // SERVO_CONTROL_H
|