144 lines
4.2 KiB
C
144 lines
4.2 KiB
C
#ifndef SPEED_PLANNER_H
|
|
#define SPEED_PLANNER_H
|
|
|
|
#include <stdint.h>
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
/* 速度规划器状态枚举 */
|
|
typedef enum {
|
|
SPEED_PLANNER_IDLE = 0, /* 空闲状态 */
|
|
SPEED_PLANNER_ACCELERATING, /* 加速阶段 */
|
|
SPEED_PLANNER_CONSTANT, /* 匀速阶段 */
|
|
SPEED_PLANNER_DECELERATING, /* 减速阶段 */
|
|
SPEED_PLANNER_FINISHED /* 完成状态 */
|
|
} SpeedPlanner_State_t;
|
|
|
|
/* 速度规划器参数结构体 */
|
|
typedef struct {
|
|
float max_velocity; /* 最大速度 (m/s) */
|
|
float max_acceleration; /* 最大加速度 (m/s²) */
|
|
float max_deceleration; /* 最大减速度 (m/s²) */
|
|
float position_error_limit; /* 位置误差限制 (m) */
|
|
float velocity_tolerance; /* 速度容差，用于判断是否到达目标速度 (m/s) */
|
|
} SpeedPlanner_Param_t;
|
|
|
|
/* 速度规划器结构体 */
|
|
typedef struct {
|
|
/* 参数 */
|
|
SpeedPlanner_Param_t param;
|
|
|
|
/* 状态变量 */
|
|
SpeedPlanner_State_t state;
|
|
float current_velocity; /* 当前速度 (m/s) */
|
|
float current_position; /* 当前位置 (m) */
|
|
float target_velocity; /* 目标速度 (m/s) */
|
|
float target_position; /* 目标位置 (m) */
|
|
|
|
/* 规划变量 */
|
|
float planned_velocity; /* 规划输出速度 (m/s) */
|
|
float planned_acceleration; /* 规划输出加速度 (m/s²) */
|
|
|
|
/* 内部状态 */
|
|
float last_target_velocity; /* 上次目标速度，用于检测目标变化 */
|
|
float position_error; /* 位置误差 (目标位置 - 当前位置) */
|
|
float dt; /* 控制周期 (s) */
|
|
|
|
/* 斜坡规划状态 */
|
|
float ramp_start_velocity; /* 斜坡起始速度 */
|
|
float ramp_end_velocity; /* 斜坡结束速度 */
|
|
float ramp_duration; /* 斜坡持续时间 */
|
|
float ramp_time; /* 斜坡当前时间 */
|
|
} SpeedPlanner_t;
|
|
|
|
/**
|
|
* @brief 初始化速度规划器
|
|
* @param planner 速度规划器指针
|
|
* @param param 参数结构体指针
|
|
* @param control_freq 控制频率 (Hz)
|
|
* @return 0: 成功, -1: 失败
|
|
*/
|
|
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq);
|
|
|
|
/**
|
|
* @brief 更新当前状态
|
|
* @param planner 速度规划器指针
|
|
* @param current_velocity 当前速度 (m/s)
|
|
* @param current_position 当前位置 (m)
|
|
* @return 0: 成功, -1: 失败
|
|
*/
|
|
int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position);
|
|
|
|
/**
|
|
* @brief 设置目标速度
|
|
* @param planner 速度规划器指针
|
|
* @param target_velocity 目标速度 (m/s)
|
|
* @return 0: 成功, -1: 失败
|
|
*/
|
|
int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity);
|
|
|
|
/**
|
|
* @brief 设置目标位置
|
|
* @param planner 速度规划器指针
|
|
* @param target_position 目标位置 (m)
|
|
* @return 0: 成功, -1: 失败
|
|
*/
|
|
int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position);
|
|
|
|
/**
|
|
* @brief 执行速度规划计算
|
|
* @param planner 速度规划器指针
|
|
* @return 0: 成功, -1: 失败
|
|
*/
|
|
int8_t SpeedPlanner_Update(SpeedPlanner_t *planner);
|
|
|
|
/**
|
|
* @brief 获取规划的速度输出
|
|
* @param planner 速度规划器指针
|
|
* @return 规划的速度 (m/s)
|
|
*/
|
|
float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner);
|
|
|
|
/**
|
|
* @brief 获取规划的加速度输出
|
|
* @param planner 速度规划器指针
|
|
* @return 规划的加速度 (m/s²)
|
|
*/
|
|
float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner);
|
|
|
|
/**
|
|
* @brief 获取位置误差
|
|
* @param planner 速度规划器指针
|
|
* @return 位置误差 (m)
|
|
*/
|
|
float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner);
|
|
|
|
/**
|
|
* @brief 获取当前状态
|
|
* @param planner 速度规划器指针
|
|
* @return 当前状态
|
|
*/
|
|
SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner);
|
|
|
|
/**
|
|
* @brief 重置速度规划器
|
|
* @param planner 速度规划器指针
|
|
* @return 0: 成功, -1: 失败
|
|
*/
|
|
int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner);
|
|
|
|
/**
|
|
* @brief 检查是否需要紧急停止（位置误差过大）
|
|
* @param planner 速度规划器指针
|
|
* @return 1: 需要紧急停止, 0: 正常
|
|
*/
|
|
int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif // SPEED_PLANNER_H
|