81 lines
2.1 KiB
C
81 lines
2.1 KiB
C
#include "trapezoidal_profile.h"
|
|
/* USER INCLUDE BEGIN */
|
|
|
|
/* USER INCLUDE END */
|
|
|
|
|
|
/* --- 梯形速度规划器相关定义与函数 --- */
|
|
|
|
// 1. 规划器初始化函数
|
|
void Profile_Init(TrapezoidalProfile *p, float start, float target, float v_max, float acc) {
|
|
p->start_pos = start;
|
|
p->target_pos = target;
|
|
float dist = target - start;
|
|
p->direction = (dist >= 0.0f) ? 1 : -1;
|
|
dist = fabsf(dist);
|
|
|
|
p->max_vel = v_max;
|
|
p->accel = acc;
|
|
p->elapsed_time = 0.0f;
|
|
|
|
p->t_acc = v_max / acc;
|
|
p->d_acc = 0.5f * acc * p->t_acc * p->t_acc;
|
|
|
|
if (dist < 2.0f * p->d_acc) { // 走不到最大速度就得减速 (三角形曲线)
|
|
p->d_acc = dist / 2.0f;
|
|
p->t_acc = sqrtf(2.0f * p->d_acc / acc);
|
|
p->t_total = 2.0f * p->t_acc;
|
|
p->d_cruise = 0.0f;
|
|
p->max_vel = acc * p->t_acc;
|
|
} else { // 标准梯形曲线
|
|
p->d_cruise = dist - 2.0f * p->d_acc;
|
|
p->t_total = 2.0f * p->t_acc + (p->d_cruise / v_max);
|
|
}
|
|
}
|
|
|
|
// 2. 规划器实时更新函数
|
|
void Profile_Update(TrapezoidalProfile *p, float dt) {
|
|
p->elapsed_time += dt;
|
|
float t = p->elapsed_time;
|
|
float v = 0.0f, s = 0.0f;
|
|
|
|
if (t < p->t_acc) {
|
|
// 加速段
|
|
v = p->accel * t;
|
|
s = 0.5f * p->accel * t * t;
|
|
}
|
|
else if (t < (p->t_total - p->t_acc)) {
|
|
// 匀速段
|
|
v = p->max_vel;
|
|
s = p->d_acc + p->max_vel * (t - p->t_acc);
|
|
}
|
|
else if (t < p->t_total) {
|
|
// 减速段
|
|
float t_dec = t - (p->t_total - p->t_acc);
|
|
v = p->max_vel - p->accel * t_dec;
|
|
s = p->d_acc + p->d_cruise + (p->max_vel * t_dec - 0.5f * p->accel * t_dec * t_dec);
|
|
}
|
|
else {
|
|
// 到达终点
|
|
p->current_pos = p->target_pos;
|
|
p->current_vel = 0.0f;
|
|
return;
|
|
}
|
|
|
|
p->current_vel = v * (float)p->direction;
|
|
p->current_pos = p->start_pos + s * (float)p->direction;
|
|
}
|
|
/* -------------------------------------------------------- */
|
|
|
|
|
|
/* USER DEFINE BEGIN */
|
|
|
|
/* USER DEFINE END */
|
|
|
|
|
|
|
|
|
|
/* USER FUNCTION BEGIN */
|
|
|
|
/* USER FUNCTION END */
|