514 lines
10 KiB
C
514 lines
10 KiB
C
|
||
#include "curves.h"
|
||
|
||
//--------------------------------------------------
|
||
#include "trigger.h"
|
||
#include <math.h>
|
||
|
||
//--------------------------------------------------
|
||
|
||
#if (1) // T型速度曲线加减速（四点式，三段）
|
||
|
||
//--------------------------------------------------
|
||
|
||
// 计算加减速需要的位移
|
||
// 参数: pps1 速度1
|
||
// pps2 速度2
|
||
// ppss 加速度
|
||
// 返回:
|
||
// 位移值
|
||
u32 TTypeCalcDisplacement(float pps1, float pps2, float ppss)
|
||
{
|
||
// 公式: d=(v2+v1)*(v2-v1)/(2a)
|
||
// v1 = pps1
|
||
// v2 = pps2
|
||
// a = ppss
|
||
float temp;
|
||
|
||
if (ppss == 0) // 加速度为0，匀速直线运动
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
if (pps1 < pps2)
|
||
{
|
||
temp = (pps2-pps1);
|
||
}
|
||
else
|
||
{
|
||
temp = (pps1-pps2);
|
||
}
|
||
temp *= (pps1+pps2);
|
||
temp /= ppss;
|
||
temp /= 2;
|
||
return (temp+0.5f);
|
||
}
|
||
|
||
//--------------------------------------------------
|
||
|
||
// 功能：计算某位移下速度变化需要加速度
|
||
// 参数：
|
||
// disp 位移
|
||
// pps1 速度1
|
||
// pps2 速度2
|
||
// 返回: 加速度
|
||
|
||
float TTypeCalcAddPPSS(u32 disp, float pps1, float pps2)
|
||
{
|
||
float temp;
|
||
// 公式: a=(v2+v1)*(v2-v1)/(2*d)
|
||
// v1 = pps1
|
||
// v2 = pps2
|
||
// d = disp
|
||
|
||
if (disp == 0 || pps1 == pps2)
|
||
{
|
||
return 0;
|
||
}
|
||
if (pps1 < pps2)
|
||
{
|
||
temp = (pps2-pps1);
|
||
}
|
||
else
|
||
{
|
||
temp = (pps1-pps2);
|
||
}
|
||
temp *= (pps1+pps2);
|
||
temp /= disp;
|
||
temp /= 2;
|
||
return (temp);
|
||
}
|
||
|
||
//--------------------------------------------------
|
||
|
||
// 计算某位移和某加速度下，最终的速度值
|
||
// 参数：
|
||
// disp 位移
|
||
// pps1 速度1
|
||
// ppss 加速度
|
||
//
|
||
// 返回:
|
||
// 速度值
|
||
|
||
float TTypeCalcEndSpd(u32 disp, float pps1, float ppss)
|
||
{
|
||
float temp;
|
||
// 公式: a=(v2+v1)*(v2-v1)/(2*d)
|
||
// v1 = pps1
|
||
// d = disp
|
||
// 最终 pps2 = sqrt(2*disp*ppss + pps1*pps1)
|
||
|
||
temp = 2.0f*disp;
|
||
temp *= ppss;
|
||
temp += pps1*pps1;
|
||
if (temp > 0)
|
||
{
|
||
temp = sqrt(temp);
|
||
}
|
||
else
|
||
{
|
||
temp = 0;
|
||
}
|
||
|
||
return (temp);
|
||
}
|
||
|
||
//--------------------------------------------------
|
||
// 计算运动时间
|
||
float TTypeCalcRunTime(u32 disp, u32 startpps, u32 stoppps, u32 runpps, u32 addppss)
|
||
{
|
||
TTypeCtrl tctrl;
|
||
float runtime = 0;
|
||
float time = 0;
|
||
|
||
memset(&tctrl, 0, sizeof(TTypeCtrl));
|
||
|
||
// 计算中间变量和结果
|
||
tctrl.movement = disp;
|
||
tctrl.begPPS = startpps; // 本段初始速度
|
||
tctrl.endPPS = stoppps; // 本段结束速度
|
||
tctrl.runPPS = runpps; // 本段运行速度
|
||
tctrl.maxPPSS = addppss; // 最大的加速度
|
||
TTypeCalcRunParas(&tctrl); // 计算加减速参数
|
||
|
||
{
|
||
s32 newPPSS1, newPPSS2;
|
||
|
||
newPPSS1 = tctrl.newAddPPSS;
|
||
newPPSS2 = tctrl.newAddPPSS;
|
||
|
||
if (tctrl.begPPS > tctrl.newRunPPS)
|
||
{
|
||
newPPSS1 *= -1;
|
||
}
|
||
if (tctrl.endPPS > tctrl.newRunPPS)
|
||
{
|
||
newPPSS2 *= -1;
|
||
}
|
||
|
||
runtime = 0;
|
||
if (tctrl.mvmtseg[0] != 0 && newPPSS1 != 0)
|
||
{
|
||
time = (int)tctrl.newRunPPS - (int)tctrl.begPPS;
|
||
time /= newPPSS1;
|
||
runtime += time;
|
||
}
|
||
|
||
if (tctrl.mvmtseg[1] != 0 && tctrl.newRunPPS != 0)
|
||
{
|
||
time = tctrl.mvmtseg[1];
|
||
time /= (int)tctrl.newRunPPS;
|
||
runtime += time;
|
||
}
|
||
|
||
if (tctrl.mvmtseg[2] != 0 && newPPSS2 != 0)
|
||
{
|
||
time = (int)tctrl.newRunPPS - (int)tctrl.endPPS;
|
||
time /= newPPSS2;
|
||
runtime += time;
|
||
}
|
||
// printf("run time = %.6f\r\n", runtime);
|
||
}
|
||
|
||
return runtime;
|
||
}
|
||
|
||
//--------------------------------------------------
|
||
|
||
// 梯形速度计算
|
||
int TTypeCalcRunParas(TTypeCtrl * pTTypeCtrl)
|
||
{
|
||
#define TCDEBUG_OUT 0 // 输出调试信息
|
||
|
||
int rslt;
|
||
// 已知量
|
||
// 条件，当前位移段的运动不能换向（速度在一个区间，不会过零）
|
||
|
||
// 用到的变量
|
||
u32 movement; // 本段位移量
|
||
u32 begPPS, endPPS, runPPS;
|
||
u32 addPPSS; // 加速度
|
||
|
||
movement = pTTypeCtrl->movement;
|
||
begPPS = pTTypeCtrl->begPPS;
|
||
endPPS = pTTypeCtrl->endPPS;
|
||
runPPS = pTTypeCtrl->runPPS;
|
||
addPPSS = pTTypeCtrl->maxPPSS;
|
||
|
||
// 保证运行速度大于最低速度
|
||
if (runPPS < begPPS || runPPS < endPPS)
|
||
{
|
||
runPPS = begPPS > endPPS ? begPPS:endPPS;
|
||
}
|
||
|
||
rslt = 0;
|
||
// 1. 判断是否达到最大速度，并计算各个段的位移
|
||
{
|
||
u32 mvmt1, mvmt2, mvmt3;
|
||
|
||
mvmt1 = TTypeCalcDisplacement(begPPS, runPPS, addPPSS);
|
||
mvmt3 = TTypeCalcDisplacement(endPPS, runPPS, addPPSS);
|
||
|
||
if (mvmt1 + mvmt3 >= movement) // 不能达到运行速度
|
||
{
|
||
float tmpv;
|
||
tmpv = movement * 2;
|
||
tmpv = sqrt((tmpv*addPPSS + begPPS*begPPS + endPPS*endPPS)/2.0); // 能够达到的峰值速度
|
||
runPPS = (tmpv);
|
||
#if (TCDEBUG_OUT == 1)
|
||
printf("TTypeCalcRunParas runPPS=%d\r\n", runPPS);
|
||
#endif
|
||
if (runPPS < begPPS || runPPS < endPPS) // 如果重新计算的速度小于启动停止速度
|
||
{
|
||
u32 tpmvmt;
|
||
tpmvmt = TTypeCalcDisplacement(begPPS, endPPS, addPPSS); // 直接变速最小位移
|
||
if (tpmvmt > movement) // 所需要位移太大，不足以完成加减速，规划失败
|
||
{
|
||
addPPSS = TTypeCalcAddPPSS(movement, begPPS, endPPS); // 重新计算加速度
|
||
#if (TCDEBUG_OUT == 1)
|
||
printf("TTypeCalcRunParas addPPSS=%d\r\n", addPPSS);
|
||
#endif
|
||
if (begPPS <= endPPS)
|
||
{
|
||
mvmt1 = movement;
|
||
mvmt2 = 0;
|
||
mvmt3 = 0;
|
||
}
|
||
else
|
||
{
|
||
mvmt1 = 0;
|
||
mvmt2 = 0;
|
||
mvmt3 = movement;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (begPPS <= endPPS)
|
||
{
|
||
mvmt1 = tpmvmt;
|
||
mvmt2 = movement - tpmvmt;
|
||
mvmt3 = 0;
|
||
}
|
||
else
|
||
{
|
||
mvmt1 = 0;
|
||
mvmt2 = movement - tpmvmt;
|
||
mvmt3 = tpmvmt;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
mvmt1 = TTypeCalcDisplacement(begPPS, runPPS, addPPSS);
|
||
mvmt3 = TTypeCalcDisplacement(endPPS, runPPS, addPPSS); // 重新计算位移
|
||
|
||
if (mvmt1 + mvmt3 > movement)
|
||
{
|
||
mvmt2 = 0;
|
||
mvmt3 = movement - mvmt1;
|
||
}
|
||
else
|
||
{
|
||
mvmt2 = movement - mvmt1 - mvmt3;
|
||
}
|
||
}
|
||
}
|
||
else // 可以达到运行速度
|
||
{
|
||
mvmt2 = movement - mvmt1 - mvmt3;
|
||
}
|
||
|
||
pTTypeCtrl->mvmtseg[0] = mvmt1; // 第一段
|
||
pTTypeCtrl->mvmtseg[1] = mvmt2; // 第二段
|
||
pTTypeCtrl->mvmtseg[2] = mvmt3; // 第三段
|
||
pTTypeCtrl->newRunPPS = runPPS;
|
||
pTTypeCtrl->newAddPPSS = addPPSS;
|
||
}
|
||
#if (TCDEBUG_OUT == 1)
|
||
printf("TTypeCalcRunParas: seg[0]=%d, seg[2]=%d, seg[1]=%d, newRunPPS[1]=%.1f, newAddPPSS=%.1f\r\n",
|
||
pTTypeCtrl->mvmtseg[0], pTTypeCtrl->mvmtseg[2], pTTypeCtrl->mvmtseg[1], pTTypeCtrl->newRunPPS, pTTypeCtrl->newAddPPSS);
|
||
#endif
|
||
|
||
return rslt;
|
||
}
|
||
//--------------------------------------------
|
||
// 计算分频系数表
|
||
int TTypeCalcDivTab(u32 fin, TTypeCtrl * pTTypeCtrl, u32 * pFidvTab, u32 * pTablen)
|
||
{
|
||
#define TDEBUG_OUT 0 // 输出调试信息
|
||
|
||
#if (TDEBUG_OUT == 1)
|
||
u32 time, tptm, lstm;
|
||
#endif
|
||
int rslt;
|
||
int totalclk = 0; // 运行时间（单位是1个时间片）
|
||
u32 movement;
|
||
|
||
if (fin <= 0 || pTTypeCtrl == NULL || pFidvTab == NULL || pTablen == NULL)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
#if (TDEBUG_OUT == 1)
|
||
time = GetUsSoftTimer();
|
||
lstm = time;
|
||
#endif
|
||
|
||
// 计算运动参数
|
||
TTypeCalcRunParas(pTTypeCtrl);
|
||
|
||
*pTablen = 0;
|
||
rslt = 0;
|
||
// 计算分频系数，生成每个脉冲的分频系数表
|
||
{
|
||
u32 i;
|
||
float begPPS; // 本段初始速度
|
||
float endPPS; // 本段结束速度
|
||
float runPPS; // 本段运行速度
|
||
float newPPSS1, newPPSS2;
|
||
|
||
float pps; // 速度值
|
||
float add;
|
||
float rpps1, rpps2; // 单精度能够显著提高运算速度
|
||
int runclk;
|
||
int coef;
|
||
u32 * pBuf;
|
||
|
||
movement = pTTypeCtrl->movement;
|
||
begPPS = pTTypeCtrl->begPPS;
|
||
endPPS = pTTypeCtrl->endPPS;
|
||
runPPS = pTTypeCtrl->newRunPPS;
|
||
newPPSS1 = pTTypeCtrl->newAddPPSS;
|
||
newPPSS2 = pTTypeCtrl->newAddPPSS;
|
||
|
||
if (begPPS > runPPS)
|
||
{
|
||
newPPSS1 *= -1;
|
||
}
|
||
if (endPPS > runPPS)
|
||
{
|
||
newPPSS2 *= -1;
|
||
}
|
||
|
||
#if (TDEBUG_OUT == 1)
|
||
lstm = GetUsSoftTimer();
|
||
#endif
|
||
pps = begPPS;
|
||
rpps1 = pps;
|
||
runclk = 0;
|
||
add = newPPSS1;
|
||
add /= fin;
|
||
pBuf = &(pFidvTab[0]);
|
||
for (i = 0; i < pTTypeCtrl->mvmtseg[0]; i++) // 第一段
|
||
{
|
||
if (pps > runPPS)
|
||
{
|
||
pps = runPPS;
|
||
}
|
||
if (newPPSS1 < 0 && pps < endPPS)
|
||
{
|
||
pps = endPPS;
|
||
}
|
||
coef = fin / pps; // 根据速度值得到系数
|
||
*pBuf++ = coef; // 记录分频系数
|
||
runclk += coef; // 根据分频系数得到下一个脉冲产生的时间点
|
||
pps = begPPS + runclk * add; // 根据速度规划，计算下一个脉冲产生时刻的速度值
|
||
}
|
||
rpps1 = pps;
|
||
totalclk += runclk;
|
||
|
||
#if (TDEBUG_OUT == 1)
|
||
tptm = GetUsSoftTimer();
|
||
printf("TCDiv seg1 tm=%uus\r\n", tptm-lstm);
|
||
lstm = tptm;
|
||
#endif
|
||
// 倒序产生第三段
|
||
pps = endPPS;
|
||
rpps2 = pps;
|
||
runclk = 0;
|
||
add = newPPSS2;
|
||
add /= fin;
|
||
pBuf = &(pFidvTab[movement - 1]);
|
||
for (i = 0; i < pTTypeCtrl->mvmtseg[2]; i++)
|
||
{
|
||
if (pps > runPPS)
|
||
{
|
||
pps = runPPS;
|
||
}
|
||
if (newPPSS2 < 0 && pps < endPPS)
|
||
{
|
||
pps = endPPS;
|
||
}
|
||
coef = fin / pps; // 根据速度值得到系数
|
||
*pBuf-- = coef; // 记录分频系数
|
||
runclk += coef; // 根据分频系数得到下一个脉冲产生的时间点
|
||
pps = endPPS + runclk * add; // 根据速度规划，计算下一个脉冲产生时刻的速度值
|
||
}
|
||
rpps2 = pps;
|
||
totalclk += runclk;
|
||
#if (TDEBUG_OUT == 1)
|
||
tptm = GetUsSoftTimer();
|
||
printf("TCDiv seg3 tm=%uus\r\n", tptm-lstm);
|
||
lstm = tptm;
|
||
#endif
|
||
// 匀速段
|
||
pps = (rpps1 + rpps2) / 2;
|
||
if (pps > runPPS)
|
||
{
|
||
pps = runPPS;
|
||
}
|
||
if (newPPSS2 < 0 && pps < endPPS)
|
||
{
|
||
pps = endPPS;
|
||
}
|
||
runclk = 0;
|
||
pBuf = &(pFidvTab[pTTypeCtrl->mvmtseg[0]]);
|
||
coef = fin / pps; // 根据速度值得到系数
|
||
for (i = 0; i < pTTypeCtrl->mvmtseg[1]; i++)
|
||
{
|
||
*pBuf++ = coef; // 记录分频系数
|
||
}
|
||
runclk = pTTypeCtrl->mvmtseg[1] * coef;
|
||
totalclk += runclk;
|
||
#if (TDEBUG_OUT == 1)
|
||
tptm = GetUsSoftTimer();
|
||
printf("TCDiv seg2 tm=%uus\r\n", tptm-lstm);
|
||
lstm = tptm;
|
||
#endif
|
||
}
|
||
|
||
if (rslt < 0)
|
||
{
|
||
rslt++;
|
||
}
|
||
else
|
||
{
|
||
*pTablen = movement;
|
||
rslt = totalclk;
|
||
}
|
||
#if (TDEBUG_OUT == 1)
|
||
tptm = GetUsSoftTimer();
|
||
printf("TCDiv tm=%uus\r\n", tptm-time);
|
||
#endif
|
||
|
||
return rslt;
|
||
}
|
||
|
||
//--------------------------------------------
|
||
|
||
#endif
|
||
|
||
//----------------------------------------------------------------------------------------
|
||
|
||
//----------------------------------------------------------------------------------------
|
||
|
||
#if (0) // S型速度曲线加减速（七段）
|
||
|
||
|
||
typedef struct
|
||
{
|
||
// 输入参数
|
||
u32 movement; // 本段位移量
|
||
|
||
float begPPS; // 本段初始速度
|
||
float endPPS; // 本段结束速度
|
||
float runPPS; // 本段运行速度（v）
|
||
float maxPPSS; // 最大的加速度（a）
|
||
float maxPPSSS; // 最大的加加速度（j）
|
||
|
||
// 计算中间变量和结果
|
||
u32 timeseg[7]; // 七段时间长度, 单位为1个时间片
|
||
u32 mvmtseg[7]; // 七段位移, 分段的位移量 (如果位移量不为零, 说明存在这个段)
|
||
float newRunPPS; // 新的运行速度
|
||
|
||
}STypeCtrl;
|
||
|
||
// 计算位移量
|
||
u32 STypeCalcDisplacement(float pps1, float pps2, float ppss);
|
||
|
||
// 计算加速度
|
||
float STypeCalcAddPPSS(u32 disp, float pps1, float pps2);
|
||
|
||
// 计算运动结束的速度
|
||
float STypeCalcEndSpd(u32 disp, float pps1, float ppss);
|
||
|
||
// 计算运动的时间
|
||
float STypeCalcRunTime(u32 disp, u32 startpps, u32 stoppps, u32 runpps, u32 addppss);
|
||
|
||
// 计算运动相关参数
|
||
int STypeCalcRunParas(STypeCtrl * pSTypeCtrl);
|
||
|
||
// 计算生成分频系数表
|
||
int STypeCalcDivTab(u32 fin, STypeCtrl * pSTypeCtrl, u32 * pFidvTab, u32 * pTablen);
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
#endif
|