添加速度规划
This commit is contained in:
parent
ee435d8001
commit
ec8dd40ced
@ -61,3 +61,4 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer);
|
||||
*/
|
||||
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
|
||||
float heat_increase, bool is_big);
|
||||
|
||||
|
||||
@ -1,135 +1,266 @@
|
||||
/**
|
||||
* @file speed_planner.c
|
||||
* @brief 斜坡速度规划器实现
|
||||
*/
|
||||
|
||||
#include "component/speed_planner.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
/**
|
||||
* @brief 限制函数
|
||||
*/
|
||||
static inline float clamp(float value, float min, float max) {
|
||||
if (value < min) return min;
|
||||
/* 浮点数比较容差 */
|
||||
#define FLOAT_EPSILON 1e-6f
|
||||
|
||||
/* 限制函数 */
|
||||
static float SpeedPlanner_Limit(float value, float min, float max) {
|
||||
if (value > max) return max;
|
||||
if (value < min) return min;
|
||||
return value;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化速度规划器
|
||||
*/
|
||||
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params) {
|
||||
if (planner == NULL || params == NULL) {
|
||||
/* 符号函数 */
|
||||
static float SpeedPlanner_Sign(float value) {
|
||||
if (value > FLOAT_EPSILON) return 1.0f;
|
||||
if (value < -FLOAT_EPSILON) return -1.0f;
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* 获取速度符号，用于判断加速/减速方向 */
|
||||
__attribute__((unused)) static float SpeedPlanner_GetVelocitySign(float velocity) {
|
||||
return SpeedPlanner_Sign(velocity);
|
||||
}
|
||||
|
||||
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq) {
|
||||
if (planner == NULL || param == NULL || control_freq <= 0.0f) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
memset(planner, 0, sizeof(SpeedPlanner_t));
|
||||
planner->param = *params;
|
||||
/* 参数检查 */
|
||||
if (param->max_velocity <= 0.0f || param->max_acceleration <= 0.0f ||
|
||||
param->max_deceleration <= 0.0f || param->position_error_limit <= 0.0f) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 复制参数 */
|
||||
planner->param = *param;
|
||||
|
||||
/* 计算控制周期 */
|
||||
planner->dt = 1.0f / control_freq;
|
||||
|
||||
/* 初始化状态 */
|
||||
SpeedPlanner_Reset(planner);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 重置速度规划器
|
||||
*/
|
||||
void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity) {
|
||||
int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner) {
|
||||
if (planner == NULL) {
|
||||
return;
|
||||
return -1;
|
||||
}
|
||||
|
||||
planner->current_position = current_position;
|
||||
planner->current_velocity = current_velocity;
|
||||
planner->target_position = current_position;
|
||||
planner->planned_position = current_position;
|
||||
planner->planned_velocity = current_velocity;
|
||||
planner->target_velocity = current_velocity;
|
||||
planner->state = SPEED_PLANNER_IDLE;
|
||||
planner->current_velocity = 0.0f;
|
||||
planner->current_position = 0.0f;
|
||||
planner->target_velocity = 0.0f;
|
||||
planner->target_position = 0.0f;
|
||||
planner->planned_velocity = 0.0f;
|
||||
planner->planned_acceleration = 0.0f;
|
||||
planner->last_target_velocity = 0.0f;
|
||||
planner->position_error = 0.0f;
|
||||
planner->ramp_start_velocity = 0.0f;
|
||||
planner->ramp_end_velocity = 0.0f;
|
||||
planner->ramp_duration = 0.0f;
|
||||
planner->ramp_time = 0.0f;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 更新速度规划器
|
||||
*/
|
||||
float SpeedPlanner_Update(SpeedPlanner_t *planner,
|
||||
float target_velocity,
|
||||
float current_position,
|
||||
float current_velocity,
|
||||
float dt) {
|
||||
if (planner == NULL || dt <= 0.0f) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* 更新当前状态 */
|
||||
planner->current_position = current_position;
|
||||
planner->current_velocity = current_velocity;
|
||||
planner->target_velocity = target_velocity;
|
||||
|
||||
/* 限制目标速度到最大速度范围 */
|
||||
float limited_target_velocity = clamp(target_velocity,
|
||||
-planner->param.max_velocity,
|
||||
planner->param.max_velocity);
|
||||
|
||||
/* 计算速度变化 */
|
||||
float velocity_error = limited_target_velocity - planner->planned_velocity;
|
||||
|
||||
/* 使用最大加速度限制速度变化率 */
|
||||
float max_velocity_change = planner->param.max_acceleration * dt;
|
||||
float velocity_change = clamp(velocity_error, -max_velocity_change, max_velocity_change);
|
||||
|
||||
/* 更新规划速度 */
|
||||
planner->planned_velocity += velocity_change;
|
||||
|
||||
/* 限制规划速度 */
|
||||
planner->planned_velocity = clamp(planner->planned_velocity,
|
||||
-planner->param.max_velocity,
|
||||
planner->param.max_velocity);
|
||||
|
||||
/* 更新规划位置 */
|
||||
planner->planned_position += planner->planned_velocity * dt;
|
||||
|
||||
/* 防止位移起飞：限制规划位置与当前位置的误差 */
|
||||
float position_error = planner->planned_position - current_position;
|
||||
|
||||
if (fabsf(position_error) > planner->param.max_position_error) {
|
||||
/* 位置误差过大，重新规划 */
|
||||
if (position_error > 0.0f) {
|
||||
planner->planned_position = current_position + planner->param.max_position_error;
|
||||
} else {
|
||||
planner->planned_position = current_position - planner->param.max_position_error;
|
||||
}
|
||||
|
||||
/* 根据位置误差调整速度，使位置逐渐收敛 */
|
||||
/* 使用简单的比例控制，系数可以调整 */
|
||||
float position_correction_gain = 2.0f; // 位置收敛增益
|
||||
planner->planned_velocity = (planner->planned_position - current_position) * position_correction_gain;
|
||||
|
||||
/* 再次限制速度 */
|
||||
planner->planned_velocity = clamp(planner->planned_velocity,
|
||||
-planner->param.max_velocity,
|
||||
planner->param.max_velocity);
|
||||
}
|
||||
|
||||
/* 更新目标位置（用于外部参考） */
|
||||
planner->target_position = planner->planned_position;
|
||||
|
||||
return planner->planned_velocity;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取规划后的目标位置
|
||||
*/
|
||||
float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner) {
|
||||
int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position) {
|
||||
if (planner == NULL) {
|
||||
return 0.0f;
|
||||
return -1;
|
||||
}
|
||||
return planner->planned_position;
|
||||
|
||||
planner->current_velocity = current_velocity;
|
||||
planner->current_position = current_position;
|
||||
planner->position_error = planner->target_position - planner->current_position;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取规划后的速度
|
||||
*/
|
||||
float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner) {
|
||||
int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity) {
|
||||
if (planner == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 限制目标速度在允许范围内 */
|
||||
planner->target_velocity = SpeedPlanner_Limit(target_velocity,
|
||||
-planner->param.max_velocity,
|
||||
planner->param.max_velocity);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position) {
|
||||
if (planner == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
planner->target_position = target_position;
|
||||
planner->position_error = planner->target_position - planner->current_position;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* 检查目标速度是否改变 */
|
||||
static int8_t SpeedPlanner_IsTargetChanged(SpeedPlanner_t *planner) {
|
||||
return (fabsf(planner->target_velocity - planner->last_target_velocity) > planner->param.velocity_tolerance);
|
||||
}
|
||||
|
||||
/* 开始新的斜坡规划 */
|
||||
static void SpeedPlanner_StartRamp(SpeedPlanner_t *planner) {
|
||||
planner->ramp_start_velocity = planner->current_velocity;
|
||||
planner->ramp_end_velocity = planner->target_velocity;
|
||||
|
||||
/* 计算需要的加速度和时间 */
|
||||
float velocity_diff = planner->ramp_end_velocity - planner->ramp_start_velocity;
|
||||
float required_acceleration = fabsf(velocity_diff);
|
||||
|
||||
/* 选择合适的加速度（加速或减速） */
|
||||
float max_accel = (velocity_diff >= 0) ? planner->param.max_acceleration : planner->param.max_deceleration;
|
||||
|
||||
if (required_acceleration > max_accel) {
|
||||
required_acceleration = max_accel;
|
||||
}
|
||||
|
||||
/* 计算斜坡时间 */
|
||||
if (required_acceleration > FLOAT_EPSILON) {
|
||||
planner->ramp_duration = fabsf(velocity_diff) / required_acceleration;
|
||||
} else {
|
||||
planner->ramp_duration = 0.0f;
|
||||
}
|
||||
|
||||
planner->ramp_time = 0.0f;
|
||||
|
||||
/* 设置状态 */
|
||||
if (fabsf(velocity_diff) < planner->param.velocity_tolerance) {
|
||||
planner->state = SPEED_PLANNER_FINISHED;
|
||||
} else if (velocity_diff > 0) {
|
||||
planner->state = SPEED_PLANNER_ACCELERATING;
|
||||
} else {
|
||||
planner->state = SPEED_PLANNER_DECELERATING;
|
||||
}
|
||||
}
|
||||
|
||||
/* 执行斜坡插值 */
|
||||
static float SpeedPlanner_RampInterpolation(SpeedPlanner_t *planner) {
|
||||
if (planner->ramp_duration <= FLOAT_EPSILON) {
|
||||
return planner->ramp_end_velocity;
|
||||
}
|
||||
|
||||
/* 线性插值 */
|
||||
float progress = planner->ramp_time / planner->ramp_duration;
|
||||
progress = SpeedPlanner_Limit(progress, 0.0f, 1.0f);
|
||||
|
||||
return planner->ramp_start_velocity +
|
||||
(planner->ramp_end_velocity - planner->ramp_start_velocity) * progress;
|
||||
}
|
||||
|
||||
int8_t SpeedPlanner_Update(SpeedPlanner_t *planner) {
|
||||
if (planner == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 检查位置误差是否超限 */
|
||||
if (fabsf(planner->position_error) > planner->param.position_error_limit) {
|
||||
/* 紧急停止：设置目标速度为0 */
|
||||
planner->target_velocity = 0.0f;
|
||||
}
|
||||
|
||||
/* 检查目标是否改变 */
|
||||
if (SpeedPlanner_IsTargetChanged(planner) || planner->state == SPEED_PLANNER_IDLE) {
|
||||
SpeedPlanner_StartRamp(planner);
|
||||
planner->last_target_velocity = planner->target_velocity;
|
||||
}
|
||||
|
||||
/* 根据当前状态执行规划 */
|
||||
switch (planner->state) {
|
||||
case SPEED_PLANNER_IDLE:
|
||||
planner->planned_velocity = planner->current_velocity;
|
||||
planner->planned_acceleration = 0.0f;
|
||||
break;
|
||||
|
||||
case SPEED_PLANNER_ACCELERATING:
|
||||
case SPEED_PLANNER_DECELERATING:
|
||||
/* 执行斜坡规划 */
|
||||
planner->planned_velocity = SpeedPlanner_RampInterpolation(planner);
|
||||
|
||||
/* 计算规划加速度 */
|
||||
if (planner->dt > FLOAT_EPSILON) {
|
||||
planner->planned_acceleration = (planner->planned_velocity - planner->current_velocity) / planner->dt;
|
||||
} else {
|
||||
planner->planned_acceleration = 0.0f;
|
||||
}
|
||||
|
||||
/* 限制加速度 */
|
||||
float max_accel = (planner->state == SPEED_PLANNER_ACCELERATING) ?
|
||||
planner->param.max_acceleration : planner->param.max_deceleration;
|
||||
planner->planned_acceleration = SpeedPlanner_Limit(planner->planned_acceleration, -max_accel, max_accel);
|
||||
|
||||
/* 更新斜坡时间 */
|
||||
planner->ramp_time += planner->dt;
|
||||
|
||||
/* 检查是否完成斜坡 */
|
||||
if (planner->ramp_time >= planner->ramp_duration ||
|
||||
fabsf(planner->planned_velocity - planner->ramp_end_velocity) < planner->param.velocity_tolerance) {
|
||||
planner->state = SPEED_PLANNER_FINISHED;
|
||||
planner->planned_velocity = planner->ramp_end_velocity;
|
||||
}
|
||||
break;
|
||||
|
||||
case SPEED_PLANNER_CONSTANT:
|
||||
case SPEED_PLANNER_FINISHED:
|
||||
planner->planned_velocity = planner->target_velocity;
|
||||
planner->planned_acceleration = 0.0f;
|
||||
break;
|
||||
|
||||
default:
|
||||
planner->state = SPEED_PLANNER_IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
/* 最终限制输出速度 */
|
||||
planner->planned_velocity = SpeedPlanner_Limit(planner->planned_velocity,
|
||||
-planner->param.max_velocity,
|
||||
planner->param.max_velocity);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner) {
|
||||
if (planner == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
return planner->planned_velocity;
|
||||
}
|
||||
|
||||
float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner) {
|
||||
if (planner == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
return planner->planned_acceleration;
|
||||
}
|
||||
|
||||
float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner) {
|
||||
if (planner == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
return planner->position_error;
|
||||
}
|
||||
|
||||
SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner) {
|
||||
if (planner == NULL) {
|
||||
return SPEED_PLANNER_IDLE;
|
||||
}
|
||||
return planner->state;
|
||||
}
|
||||
|
||||
int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner) {
|
||||
if (planner == NULL) {
|
||||
return 0;
|
||||
}
|
||||
return (fabsf(planner->position_error) > planner->param.position_error_limit) ? 1 : 0;
|
||||
}
|
||||
|
||||
@ -1,76 +1,143 @@
|
||||
/**
|
||||
* @file speed_planner.h
|
||||
* @brief 斜坡速度规划器
|
||||
* @details 提供最大加速度和最大速度限制,防止目标位移起飞
|
||||
*/
|
||||
|
||||
#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_position_error; /* 最大位置误差 (m), 防止位移起飞 */
|
||||
} SpeedPlanner_Params_t;
|
||||
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 {
|
||||
float current_velocity; /* 当前速度 (m/s) */
|
||||
float target_velocity; /* 目标速度 (m/s) */
|
||||
float planned_velocity; /* 规划后的速度 (m/s) */
|
||||
float current_position; /* 当前位置 (m) */
|
||||
float target_position; /* 目标位置 (m) */
|
||||
float planned_position; /* 规划后的位置 (m) */
|
||||
/* 参数 */
|
||||
SpeedPlanner_Param_t param;
|
||||
|
||||
SpeedPlanner_Params_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 params 参数结构体指针
|
||||
* @return 0:成功, -1:失败
|
||||
* @param planner 速度规划器指针
|
||||
* @param param 参数结构体指针
|
||||
* @param control_freq 控制频率 (Hz)
|
||||
* @return 0: 成功, -1: 失败
|
||||
*/
|
||||
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params);
|
||||
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 规划器结构体指针
|
||||
* @param current_position 当前位置
|
||||
* @param current_velocity 当前速度
|
||||
* @param planner 速度规划器指针
|
||||
* @return 0: 成功, -1: 失败
|
||||
*/
|
||||
void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity);
|
||||
int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner);
|
||||
|
||||
/**
|
||||
* @brief 更新速度规划器
|
||||
* @param planner 规划器结构体指针
|
||||
* @param target_velocity 目标速度 (m/s)
|
||||
* @param current_position 当前位置 (m)
|
||||
* @param current_velocity 当前速度 (m/s)
|
||||
* @param dt 时间间隔 (s)
|
||||
* @return 规划后的速度 (m/s)
|
||||
* @brief 检查是否需要紧急停止（位置误差过大）
|
||||
* @param planner 速度规划器指针
|
||||
* @return 1: 需要紧急停止, 0: 正常
|
||||
*/
|
||||
float SpeedPlanner_Update(SpeedPlanner_t *planner,
|
||||
float target_velocity,
|
||||
float current_position,
|
||||
float current_velocity,
|
||||
float dt);
|
||||
int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner);
|
||||
|
||||
/**
|
||||
* @brief 获取规划后的目标位置
|
||||
* @param planner 规划器结构体指针
|
||||
* @return 规划后的目标位置 (m)
|
||||
*/
|
||||
float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner);
|
||||
|
||||
/**
|
||||
* @brief 获取规划后的速度
|
||||
* @param planner 规划器结构体指针
|
||||
* @return 规划后的速度 (m/s)
|
||||
*/
|
||||
float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // SPEED_PLANNER_H
|
||||
|
||||
Loading…
Reference in New Issue
Block a user