172 lines
4.2 KiB
C
172 lines
4.2 KiB
C
#include "s_curve.h"
|
|
|
|
// s曲线加速度各段参数定义
|
|
// 起始速度
|
|
#define F0 0.0f
|
|
|
|
// 加加速度与减减速度
|
|
#define FAA 0.0f
|
|
#define FRR 0.0f
|
|
|
|
// 加速段三个时间
|
|
#define TAA 0.1f
|
|
#define TUA 0.2f
|
|
#define TRA 0.1f
|
|
|
|
// 匀速段
|
|
#define TUU 5.0f
|
|
|
|
// 减速段
|
|
#define TAR 0.1f
|
|
#define TUR 0.2f
|
|
#define TRR 0.1f
|
|
|
|
// 表格长度
|
|
#define S_CURVE_TABLE_LEN 400
|
|
int16_t s_curve_table[S_CURVE_TABLE_LEN] = {0};
|
|
|
|
static int16_t s_curve_func(s_curve_t *s, float32 t, float32 *freq, float32 *acc)
|
|
{
|
|
// 辅助常数项
|
|
float32 A, B, C, D, E, F;
|
|
// 表达式中间值
|
|
float32 f3, f4, f5;
|
|
// 加速段，匀速段，减速段时间
|
|
float32 Ta, Tu, Tr;
|
|
// 减加速与加减速段斜率
|
|
float32 fra, far;
|
|
// 起始频率与加加速频率，减减速频率
|
|
float32 f0, faa, frr;
|
|
float32 taa, tua, tra, tuu, tar, tur, trr;
|
|
|
|
// 获取参数
|
|
faa = s->faa;
|
|
frr = s->frr;
|
|
|
|
taa = s->taa;
|
|
tua = s->tua;
|
|
tra = s->tra;
|
|
tuu = s->tuu;
|
|
tar = s->tar;
|
|
tur = s->tur;
|
|
trr = s->trr;
|
|
|
|
f0 = s->f0;
|
|
|
|
fra = faa * taa / tra;
|
|
far = frr * trr / tar;
|
|
|
|
Ta = taa + tua + tra;
|
|
Tu = tuu;
|
|
Tr = tar + tur + trr;
|
|
|
|
A = f0;
|
|
B = f0 - 0.5 * faa * taa * taa;
|
|
C = f0 + 0.5 * faa * taa * taa + faa * taa * tua + 0.5 * fra * (taa + tua) * (taa + tua) - fra * Ta * (taa + tua);
|
|
|
|
// f1 = f0 + 0.5 * faa * taa * taa;
|
|
// f2 = f0 + 0.5 * faa * taa * taa + faa * taa * tua;
|
|
f3 = 0.5 * fra * Ta * Ta + C;
|
|
|
|
D = f3 - 0.5 * far * (Ta + Tu) * (Ta + Tu);
|
|
f4 = -far * 0.5 * (Ta + Tu + tar) * (Ta + Tu + tar) + far * (Ta + Tu) * (Ta + Tu + tar) + D;
|
|
|
|
E = f4 + far * tar * (Ta + Tu + tar);
|
|
f5 = -far * tar * (Ta + Tu + Tr - trr) + E;
|
|
|
|
F = f5 + frr * (Ta + Tu + Tr) * (Ta + Tu + Tr - trr) - 0.5 * frr * (Ta + Tu + Tr - trr) * (Ta + Tu + Tr - trr);
|
|
|
|
// 如果时间点在全行程规定的时间段内
|
|
if ((t >= 0) && (t <= Ta + Tu + Tr))
|
|
{
|
|
// 加加速段
|
|
if ((t >= 0) && (t <= taa))
|
|
{
|
|
*freq = 0.5 * faa * t * t + A;
|
|
*acc = faa * t;
|
|
}
|
|
// 匀加速段
|
|
else if ((t >= taa) && (t <= taa + tua))
|
|
{
|
|
*freq = faa * taa * t + B;
|
|
*acc = faa * taa;
|
|
}
|
|
// 加减速段
|
|
else if ((t >= taa + tua) && (t <= taa + tua + tra))
|
|
{
|
|
*freq = -0.5 * fra * t * t + fra * Ta * t + C;
|
|
*acc = -fra * t + fra * Ta;
|
|
}
|
|
// 匀速段
|
|
else if ((t >= Ta) && (t <= Ta + tuu))
|
|
{
|
|
*freq = f3;
|
|
*acc = 0;
|
|
}
|
|
// 加减速段
|
|
else if ((t >= Ta + Tu) && (t <= Ta + Tu + tar))
|
|
{
|
|
*freq = -0.5 * far * t * t + far * (Ta + Tu) * t + D;
|
|
*acc = -far * t + far * (Ta + Tu);
|
|
}
|
|
// 匀减速
|
|
else if ((t >= Ta + Tu + tar) && (t <= Ta + Tu + tar + tur))
|
|
{
|
|
*freq = -far * tar * t + E;
|
|
*acc = -far * tar;
|
|
}
|
|
// 减减速
|
|
else if ((t >= Ta + Tu + Tr - trr) && (t <= Ta + Tu + Tr))
|
|
{
|
|
*freq = 0.5 * frr * t * t - frr * (Ta + Tu + Tr) * t + F;
|
|
*acc = frr * t - frr * (Ta + Tu + Tr);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
// S型曲线初始化
|
|
void s_curve_table_gen(uint16_t tmin, uint16_t tmax)
|
|
{
|
|
uint16_t i, tint;
|
|
float32 ti;
|
|
float32 freq;
|
|
float32 acc;
|
|
float32 fi;
|
|
s_curve_t s;
|
|
osel_memset((uint8_t *)&s, 0, sizeof(s_curve_t));
|
|
|
|
s.f0 = F0;
|
|
s.taa = TAA;
|
|
s.tua = TUA;
|
|
s.tra = TRA;
|
|
s.tuu = TUU;
|
|
s.tar = TAR;
|
|
s.tur = TUR;
|
|
s.trr = TRR;
|
|
|
|
// 根据约束条件求出加加速段与减减速段斜率
|
|
s.faa = 2.0 / (s.taa * (s.taa + s.tra + 2 * s.tua));
|
|
s.frr = 2.0 / (s.trr * (s.tar + s.trr + 2 * s.tur));
|
|
|
|
for (i = 0; i < S_CURVE_TABLE_LEN; i++)
|
|
{
|
|
// 求出每个时间点对应的频率以及加速度
|
|
fi = i * (TAA + TUA + TRA + TUU + TAR + TUR + TRR) / S_CURVE_TABLE_LEN;
|
|
s_curve_func(&s, fi, &freq, &acc);
|
|
|
|
// 根据最大与最小装载值确定定时器实际值
|
|
ti = tmax - (tmax - tmin) * freq;
|
|
// 转换为整数值
|
|
tint = (uint16_t)ti;
|
|
|
|
// 存入s曲线表
|
|
s_curve_table[i] = tint;
|
|
}
|
|
}
|