脉冲式电子凸轮控制算法——西门子1200——纯干货分享

>

随着现代智能装备制造领域的快速发展，机械电子化、硬件软件化成为当今工业发展趋势。传统机械凸轮结构简单可靠，但机械凸轮制造的要求和成本高，其形状和轨迹固定，设计调整困难，并且升级机械凸轮、修改凸轮曲线需要改件，改件涉及大量库存，并且周期长、难度大。为解决机械凸轮存在的问题，提出将电子技术与机械凸轮结合，通过构造凸轮曲线模拟机械凸轮的运动轨迹，达到与机械凸轮系统相同的主、从轴之间的相对运动，电子凸轮应运而生。电子凸轮具有精度高、速度快、稳定性好、成本低等优点，广泛应用在包装、印刷、木工、锂电、硅晶等先进制造行业，例如：枕式包装机刀轴与膜轴的运动、飞剪设备刀轴与材料轴的运动、电池方型卷绕机等。电子凸轮逐渐取代机械凸轮，成为工业发展的趋势，在自动化领域有重要意义。

一、直线算法分析

对于涉及到匀速运动、精度要求不高且对运动轨迹要求较少的工业场合，例如：物料处理系统中使用传送带运输物料、直线车削加工、装配线上器件的输送以及直线焊接等场合。这些场合都涉及到直线算法的应用，直线的运动状况是匀速运动，速度曲线是一条平行于X轴的直线，速度恒定，位移均匀增加。直线算法公式内容如下：

二、梯形曲线算法分析

对于不需要频繁进行加减速运动的工业场合，例如：搬运机器人在搬运过程中的平稳加减速运动、机器人在自动化生产线上的平稳运动以及在机械系统中控制运动部件动作等场合，这些场合都涉及到梯形曲线算法的应用。梯形曲线分为三个部分：加速段、匀速段、减速段，运动状况是先进行加速运动，再进行匀速运动，最后进行减速运动，速度曲线形状如梯形，不同运动状态下的加速度恒定。梯形曲线算法公式内容如下：

加速度段：

匀速度段：

减速度段：

三、三次曲线算法分析

对于不需要频繁加减速运动且速度平滑的工业场合，例如：飞剪作业、工业机器人轨迹规划以及CNC切割等场合，这些场合都涉及到三次曲线算法的应用。三次曲线的运动状况是加速均匀变化，但在起始时刻和结束时刻的加速度不连续，存在冲击现象，速度曲线形状如弓形。三次曲线算法公式内容如下：

四、五次曲线算法分析

对于一些需要频繁启停且要缓起缓停的高速运动场合，例如：机械臂快速精准的定位运动、产品装载设备、工业机器人高精度高速度运动和医疗机器人精确平稳手术作业等场合，这些场合都涉及到五次曲线算法的应用。五次曲线的运动状况是加速度没有冲击现象，曲线弯曲程度较大，五次曲线适合模拟一些复杂的运动曲线。五次曲线算法公式内容如下： 

五、S型曲线算法分析

对于一些启动和停止过程需要平滑过渡的场合，例如：数控机床的启动和停止的平滑过渡、物料输送带为避免物料的冲出与溢出的启动与停止动作、机器人关节运动等场合，这些场合都涉及到S形曲线算法的应用。S形曲线的运动状况是加减速的运动曲线平滑，没有冲击现象，速度惯性小。S形曲线算法公式内容如下：

 

 六、直接上干货——程序SCL语言

//轨迹规划

//设置曲线序号：

//直线 #Mode_Line = 10

//梯形曲线 #Mode_T3 = 20

//三次曲线 #Mode_A3 = 30

//五次曲线 #Mode_A5 = 31

// S形曲线 #Mode_S = 40

REGION 启动

//脉冲启动

#R_TRIG_Run(CLK := #inParam.Start);

IF #R_TRIG_Run.Q THEN

#Out.Busy := TRUE;

END_IF;

#R_TRIG_Init(CLK := #Out.Busy);

//完成

#Out.Done := NOT #Out.Busy;

//完成复位

IF #Out.Done THEN

#Out.Vel := 0;

#Out.Acc := 0;

#Out.Jerk := 0;

// RETURN;     //退出

END_IF;

END_REGION

REGION 计算轨迹参数

IF #R_TRIG_Init.Q THEN

#T := 0;                //清空计时

#Timer := 0;            //清空计时器

#Param := #inParam;     //参数初始化

CASE #Mode OF       //曲线的类型

#Mode_Line:     //直线速度

"FC_L_Init"(Param := #Param,

ArrA := #Arr,

Type => #tType,

Tff => #Tfff);

#Mode_T3:       //梯形速度曲线

"FC_T3_Init"(Param := #Param,

ArrA := #Arr,

Type => #tType,

Tff => #Tfff);

#Mode_A3:       //三次速度曲线

"FC_A3_Init"(Param := #Param,

ArrA := #Arr,

Type => #tType,

Tff => #Tfff);

#Mode_A5:       //五次速度曲线

"FC_A5_Init"(Param := #Param,

ArrA := #Arr,

Tfype => #tType,

Tfff => #Tfff);

#Mode_S:       //七段S速度曲线

"FC_S7_Init"(Param := #Param,

ArrA := #Arr,

Type => #tType,

Tff => #Tfff);

END_CASE;

END_IF;

END_REGION

REGION 轨迹规划

IF #Out.Busy THEN

#RefTime := RUNTIME(#Timer);    //耗时

#T += #RefTime;                 //执行时间

#Out.Busy := #T < #Tfff;        //执行中

CASE #Mode OF       //轨迹类型

#Mode_Line:     //直线速度

"FC_L_Port"(Param := #Param,

ArrA := #Arr,

Type := #tType,

"inT" := #T,

q := #q);

#Mode_T3:       //T形速度曲线

"FC_T3_Port"(Param := #Param,

ArrA := #Arr,

Type := #tType,

"inT" := #T,

q := #q);

#Mode_A3:       //三次速度曲线

"FC_A3_Port"(Param := #Param,

ArrA := #Arr,

Type := #tType,

"inT" := #T,

q := #q);

#Mode_A5:       //五次速度曲线

"FC_A5_Port"(Param := #Param,

ArrA := #Arr,

Type := #tType,

"inT" := #T,

q := #q);

#Mode_S:       //七段S速度曲线

"FC_S7_Port"(Param := #Param,

ArrA := #Arr,

Type := #tType,

"inT" := #T,

q := #q);

END_CASE;

//输出映射

#Out.P := #q[0];

#Out.Vel := #q[1];

#Out.Acc := #q[2];

#Out.Jerk := #q[3];

END_IF;

END_REGION

子程序：

直线：

//直线轨迹初始化

//计算执行轨迹所需要的参数

//输入参数

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

#Vm := #Param.Vel;

#Am := #Param.Acc;

#Jm := #Param.Jerk;

END_REGION

REGION 参数计算

#Pf := ABS(#P1 - #P0);   //到达目标位置所需要的距离

#Tf := #Pf / #Vm;   //到达目标位置所需要的时间

END_REGION

REGION 输出

//

#NoVar := 0;

#Tff := #Tf;

//轨迹参数映射

#ArrA[0] := #Pf;

#ArrA[1] := #Tf;

END_REGION

//直线轨迹生成

//输出对应轨迹值

//输入参数

REGION 输入

//

#P0 := #Param.P0;

#P1 := #Param.P1;

#Vm := #Param.Vel;

#Am := #Param.Acc;

#Jm := #Param.Jerk;

//轨迹参数

#Pf := #ArrA[0];

#Tf := #ArrA[1];

IF #P1 < #P0 THEN

#Vm *= -1;

END_IF;

END_REGION

REGION 生成轨迹

IF #inT < #Tf THEN

//匀速期间

#q_0 := #P0 + #Vm * #inT;   //输出位置

#q_1 := #Vm;                //输出速度

ELSE

#q_0 := #P0 + #Vm * #Tf;

#q_1 := #Vm;

END_IF;

END_REGION

REGION 输出

//轨迹

#NoVar := 0;

#q[0] := #q_0;

#q[1] := #q_1;

#q[2] := 0;

#q[3] := 0;

END_REGION

梯形曲线：

//梯形曲线初始化

//计算执行轨迹所需要的参数

//输入参数

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

#Vm := #Param.Vel;

#Am := #Param.Acc;

#Jm := #Param.Jerk;

END_REGION

REGION 参数计算

#Pf := ABS(#P1 - #P0);            //到达目标位置所需要的距离。

#Ta := #Vm / #Am;               //加速和减速需要的时间。

#Tm := (#Pf - #Am*#Ta**2)/#Vm;   //匀速度段需要的时间。

#Tf := #Tm + 2 * #Ta;             //到达终点位置所需要的时间。

//判断梯形速度曲线属于哪一种情况，梯形还是三角？

IF #Tf - 2 * #Ta > 0 THEN     //能够达到最大速度——梯形;

#Type := 1;

ELSE                     //不能达到最大速度——三角形。

#Type := 2;

END_IF;

END_REGION

REGION 输出

//

#NoVar := 0;

#Tff := #Tf;

//轨迹参数

#ArrA[0] := #Pf;

#ArrA[1] := #Tf;

#ArrA[2] := #Ta;

#ArrA[3] := #Tf;

END_REGION

//梯形曲线轨迹生成

//输出对应轨迹值

//输入参数

REGION 输入

//

#P0 := #Param.P0;

#P1 := #Param.P1;

#Vm := #Param.Vel;

#Am := #Param.Acc;

#Jm := #Param.Jerk;

//轨迹参数

#Pf := #ArrA[0];

#Tf := #ArrA[1];

#Ta := #ArrA[2];

#Tf := #ArrA[3];

#Tm := #Tf - #Ta;

IF #P1 < #P0 THEN

#Vm *= -1;

#Am *= -1;

#Pf *= -1;

END_IF;

END_REGION

REGION 生成轨迹

//轨迹类型

CASE #Type OF

1://梯形

IF #inT <= #Ta THEN      //加速段

#q_0 := #P0 + 0.5 * #Am * #inT ** 2;

#q_1 := #Am * #inT;

#q_2 := #Am / #Ta;

ELSIF #inT<=#Tm THEN   //匀速段

#q_0 := #P0 + 0.5 * #Am * #Ta ** 2 + #Vm * (#inT - #Ta);

#q_1 := #Vm;

#q_2 := 0;

ELSIF #inT<=#Tf THEN    //减速段

#q_0 := #P0 + #Pf - 0.5 * #Am * (#Tf - #inT) ** 2;

#q_1 := - #Am * (#inT - #Ta - #Tm);

#q_2 := - #Am/#Ta;

ELSE

//终点

#q_0 := #P0 + #Pf - 0.5 * #Am * (0) ** 2;

#q_1 := 0;

#q_2 := 0;

END_IF;

2://三角形

IF #inT <= #Ta THEN    //加速段

#q_0 := #P0 + 0.5 * #Am * #inT ** 2;

#q_1 := #Am * #inT;

#q_2 := #Am / #Ta;

ELSIF #inT<=#Tf THEN  //减速段

#q_0 := #P0 + #Pf - 0.5 * #Am * (#Tf - #inT) ** 2;

#q_1 := - #Am * (#inT - #Tf);

#q_2 := - #Am;

ELSE                //终点

#q_0 := #P0 + #Pf - 0.5 * #Am * (0) ** 2;

#q_1 := - 0;

#q_2 := - 0;

END_IF;

END_CASE;

END_REGION

REGION 输出

//轨迹

#NoVar := 0;

#q[0] := #q_0;

#q[1] := #q_1;

#q[2] := #q_2;

#q[3] := 0;

END_REGION

三次曲线：

//三次曲线轨迹初始化

//计算执行轨迹所需要的参数

//输入参数

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

END_REGION

REGION 参数计算

#Pf := ABS(#P1 - #P0);

#Tf := #Param.T;

#V0 := 0;

#V1 := 0;

//

#A0 := #P0;

#A1 := #V0;

#A2 := (3 * #Pf - (2 * #V0 + #V1) * #Tf) / (#Tf ** 2);

#A3 := (-2 * #Pf + (#V0 + #V1) * #Tf) / (#Tf ** 3);

END_REGION

REGION 输出

#NoVar := 0;

#Tff := #Tf;

//轨迹参数

#ArrA[0] := #Pf;

#ArrA[1] := #Tf;

#ArrA[2] := #A0;

#ArrA[3] := #A1;

#ArrA[4] := #A2;

#ArrA[5] := #A3;

END_REGION

//三次曲线轨迹生成

//输出对应轨迹值

//输入参数

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

//轨迹参数

#Pf := #ArrA[0];

#Tf := #ArrA[1];

#A0 := #ArrA[2];

#A1 := #ArrA[3];

#A2 := #ArrA[4];

#A3 := #ArrA[5];

IF #P1 < #P0 THEN

#A1 *= -1;

#A2 *= -1;

#A3 *= -1;

END_IF;

END_REGION

REGION 生成轨迹

IF #inT <= #Tf THEN

//位置

#q_0 := #A0 + #A1 * #inT + #A2 * #inT ** 2 + #A3 * #inT ** 3;

//速度

#q_1 := #A1 + 2 * #A2 * #inT + 3 * #A3 * #inT ** 2;

//加速度

#q_2 := 2 * #A2 + 6 * #A3 * #inT;

ELSE     //最后的轨迹

//位置

#q_0 := #A0 + #A1 * #Tf + #A2 * #Tf ** 2 + #A3 * #Tf ** 3;

//速度

#q_1 := #A1 + 2 * #A2 * #Tf + 3 * #A3 * #Tf ** 2;

//加速度

#q_2 := 2 * #A2 + 6 * #A3 * #Tf;

END_IF;

END_REGION

REGION 输出

//轨迹

#NoVar := 0;

#q[0] := #q_0;

#q[1] := #q_1;

#q[2] := #q_2;

#q[3] := 0;

END_REGION

五次曲线：

//五次曲线轨迹初始化

//计算执行轨迹所需要的参数

//输入参数

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

END_REGION

REGION 参数计算

#Pf := ABS(#P1 - #P0);

#Tf := #Param.T;

#V0 := 0;

#V1 := 0;

#Acc0 := 0;

#Acc1 := 0;

//

#A0 := #P0;

#A1 := #V0;

#A2 := #Acc0 / 2;

#A3 := (20 * (#Pf) - (8 * #V1 + 12 * #V0) * #Tf - (3 * #Acc0 - #Acc1) * #Tf ** 2) / (2 * #Tf ** 3);

#A4 := (-30 * (#Pf) + (14 * #V1 + 16 * #V0) * #Tf + (3 * #Acc0 - 2 * #Acc1) * #Tf ** 2) / (2 * #Tf ** 4);

#A5 := (12 * (#Pf) - (6 * #V1 + 6 * #V0) * #Tf - (#Acc0 - #Acc1) * #Tf ** 2) / (2 * #Tf ** 5);

END_REGION

REGION 输出

#NoVar := 0;

#Tfff := #Tf;

//轨迹参数

#ArrA[0] := #Pf;

#ArrA[1] := #Tfff;

#ArrA[2] := #A0;

#ArrA[3] := #A1;

#ArrA[4] := #A2;

#ArrA[5] := #A3;

#ArrA[6] := #A4;

#ArrA[7] := #A5;

END_REGION

//五次曲线轨迹生成

//输出对应轨迹值

//输入参数

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

//轨迹参数

#Pf := #ArrA[0];

#Tf := #ArrA[1];

#A0 := #ArrA[2];

#A1 := #ArrA[3];

#A2 := #ArrA[4];

#A3 := #ArrA[5];

#A4 := #ArrA[6];

#A5 := #ArrA[7];

IF #P1 < #P0 THEN

#A1 *= -1;

#A2 *= -1;

#A3 *= -1;

#A4 *= -1;

#A5 *= -1;

END_IF;

END_REGION

REGION 生成轨迹

IF #inT <= #Pf THEN

//位置

#q_0 := #A0 + #A1 * #inT + #A2 * #inT ** 2 + #A3 * #inT ** 3 + #A4 * #inT ** 4 + #A5 * #inT ** 5;

//速度

#q_1 := #A1 + 2 * #A2 * #inT + 3 * #A3 * #inT ** 2 + 4 * #A4 * #inT ** 3 + 5 * #A5 * #inT ** 4;

//加速度

#q_2 := 2 * #A2 + 6 * #A3 * #inT + 12 * #A4 * #inT ** 2 + 20 * #A5 * #inT ** 3;

//加加速度

#q_3 := 6 * #A3 + 24 * #A4 * #inT + 60 * #A5 * #inT ** 2;

ELSE          //最后的轨迹

//位置

#q_0 := #A0 + #A1 * #Pf + #A2 * #Pf ** 2 + #A3 * #Pf ** 3 + #A4 * #Pf ** 4 + #A5 * #Pf ** 5;

//速度

#q_1 := #A1 + 2 * #A2 * #Pf + 3 * #A3 * #Pf ** 2 + 4 * #A4 * #Pf ** 3 + 5 * #A5 * #Pf ** 4;

//加速度

#q_2 := 2 * #A2 + 6 * #A3 * #Pf + 12 * #A4 * #Pf ** 2 + 20 * #A5 * #Pf ** 3;

//加加速度

#q_3 := 6 * #A3 + 24 * #A4 * #Pf + 60 * #A5 * #Pf ** 2;

END_IF;

END_REGION

REGION 输出

//轨迹

#NoVar := 0;

#q[0] := #q_0;

#q[1] := #q_1;

#q[2] := #q_2;

#q[3] := #q_3;

END_REGION

S形曲线：

//S形曲线轨迹初始化

//计算执行轨迹所需要的参数

//输入参数：

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

//

#Vm := #Param.Vel;

#Am := #Param.Acc;

#Jm := #Param.Jerk;

#Pf := ABS(#P1 - #P0);

END_REGION

REGION 参数计算

//首先按最大速度vmax，来计算Ta,Tb,Tj1 Tj2, Tv

//加速轨迹

IF (#Vm - #v0) * #Jm < #Am ** 2 THEN

IF #v0 > #Vm THEN

#Tj1 := 0;

#Ta := 0;

#alima := 0;

ELSE

#Tj1 := SQRT((#Vm - #v0) / #Jm);

#Ta := 2 * #Tj1;

#alima := #Tj1 * #Jm;

END_IF;

ELSE

#Tj1 := #Am / #Jm;

#Ta := #Tj1 + (#Vm - #v0) / #Am;

#alima := #Am;

END_IF;

//减速轨迹

IF (#Vm - #v1) * #Jm < #Am ** 2 THEN

#Tj2 := SQRT((#Vm - #v1) / #Jm);

#Td := 2 * #Tj2;

#alimd := #Tj2 * #Jm;

ELSE

#Tj2 := #Am / #Jm;

#Td := #Tj2 + (#Vm - #v1) / #Am;

#alimd := #Am;

END_IF;

//匀速段的时间

#Tv := #Pf / #Vm - #Ta / 2 * (1 + #v0 / #Vm) - #Td / 2 * (1 + #v1 / #Vm);

//如果存在匀速段

IF #Tv > 0 THEN

#vlim := #Vm;

ELSE     //如果不存在匀速段，需要重新计算能否到达最大加速度

//加加速度段、减减速度段

#Tv := 0;   //无匀速时间

#amax_org := #Am;

#delta := (#Am ** 4) / (#Jm ** 2) + 2 * (#v0 ** 2 + #v1 ** 2) + #Am * (4 * #Pf - 2 * #Am / #Jm * (#v0 + #v1));

#Tj1 := #Am / #Jm;

#Ta := (#Am ** 2 / #Jm - 2 * #v0 + #delta ** 0.5) / 2 / #Am;

#Tj2 := #Am / #Jm;

#Td := (#Am ** 2 / #Jm - 2 * #v1 + #delta ** 0.5) / 2 / #Am;

#vlim := #v0 + (#Ta - #Tj1) * #alima;

WHILE #Ta < 2 * #Tj1 OR #Td < 2 * #Tj2 DO       //如果不能到达最大加速度、减少最大速度的值，直到计算出能达到的一个最大加速度值

#Am := #Am - #amax_org * 0.1;

#alima := #Am;

#alimd := #Am;

IF #Am > 0 THEN

#delta := (#Am ** 4) / (#Jm ** 2) + 2 * (#v0 ** 2 + #v1 ** 2) + #Am * (4 * #Pf - 2 * #Am / #Jm * (#v0 + #v1));

ELSE

#delta := (#Am ** 4) / (#Jm ** 2) + 2 * (#v0 ** 2 + #v1 ** 2) - #Am * (4 * #Pf - 2 * #Am / #Jm * (#v0 + #v1));

END_IF;

#Tj1 := #Am / #Jm;

#Ta := (#Am ** 2 / #Jm - 2 * #v0 + #delta ** 0.5) / 2 / #Am;

#Tj2 := #Am / #Jm;

#Td := (#Am ** 2 / #Jm - 2 * #v1 + #delta ** 0.5) / 2 / #Am;

#vlim := #v0 + (#Ta - #Tj1) * #alima;

END_WHILE;

//重新计算Ta,Td时间

IF #Ta < 0 OR #Td < 0 THEN

IF #v0 > #v1 THEN       //计算加速度、减速度段

#Ta := 0;

#Tj1 := 0;

#alima := 0;

#Td := 2 * #Pf / (#v1 + #v0);

#Tj2 := (#Jm * #Pf - (#Jm * (#Jm * #Pf ** 2 + (#v1 + #v0) ** 2 * (#v1 - #v0))) ** 0.5) / #Jm / (#v1 + #v0);

#alimd := - #Jm * #Tj2;

#vlim := #v1 - (#Td - #Tj2) * #alimd;

#alimd := - #alimd;

ELSE

#Td := 0;

#Tj2 := 0;

#Ta := 2 * #Pf / (#v1 + #v0);

#Tj1 := (#Jm * #Pf - (#Jm * (#Jm * #Pf ** 2 - (#v1 + #v0) ** 2 * (#v1 - #v0))) ** 0.5) / #Jm / (#v1 + #v0);

#alima := #Jm * #Tj1;

#vlim := #v0 + (#Ta - #Tj1) * #alima;

END_IF;

END_IF;

END_IF;

//总共的轨迹时间

#Tf := #Tv + #Ta + #Td;

//轨迹方向

IF #P0 > #P1 THEN

#Jm *= -1;

#alima *= -1;

#alimd *= -1;

#vlim *= -1;

END_IF;

END_REGION

REGION 输出

#NoVar := 0;

#Tff := #Tf;

//轨迹参数

#ArrA[0] := #Tj1;

#ArrA[1] := #Ta;

#ArrA[2] := #Tv;

#ArrA[3] := #Td;

#ArrA[4] := #Tj2;

#ArrA[5] := #Tf;

#ArrA[6] := #Jm;

#ArrA[7] := #alima;

#ArrA[8] := #alimd;

#ArrA[9] := #vlim;

END_REGION

//S形曲线轨迹生成

//输出对应轨迹值

//输入参数

REGION 输入

#P0 := #Param.P0;

#P1 := #Param.P1;

//轨迹参数

#Tj1 := #ArrA[0];

#Ta := #ArrA[1];

#Tv := #ArrA[2];

#Td := #ArrA[3];

#Tj2 := #ArrA[4];

#Tf := #ArrA[5];

#Jm := #ArrA[6];

#alima := #ArrA[7];

#alimd := #ArrA[8];

#vlim := #ArrA[9];

#T1 := #Tj1;

#T2 := #Ta - #Tj1;

#T3 := #Ta;

#T4 := #Ta + #Tv;

#T5 := #Tf - #Td + #Tj2;

#T6 := #Tf - #Tj2;

#T7 := #Tf;

END_REGION

REGION 生成轨迹

IF #inT <= #T1 THEN      //加加速段

#q_0 := #P0 + #v0 * #inT + #Jm * #inT ** 3 / 6;

#q_1 := #v0 + #Jm * #inT ** 2 / 2;

#q_2 := #Jm * #inT;

#q_3 := #Jm;

ELSIF #inT <= #T2 THEN   //加速度段

#q_0 := #P0 + #v0 * #inT + #alima / 6 * (3 * #inT ** 2 - 3 * #Tj1 * #inT + #Tj1 ** 2);

#q_1 := #v0 + #alima * (#inT - #Tj1 / 2);

#q_2 := #alima;

#q_3 := 0;

ELSIF #inT <= #T3 THEN   //加减速段

#T_org := #T3 - #inT;  //此阶段的时间

#q_0 := #P0 + (#vlim + #v0) * #T3 / 2 - #vlim * (#T_org) + #Jm * (#T_org) ** 3 / 6;

#q_1 := #vlim - #Jm * (#T_org) ** 2 / 2;

#q_2 := #Jm * (#T_org);

#q_3 := - #Jm;

ELSIF #inT <= #T4 THEN    //匀速段

#q_0 := #P0 + (#vlim + #v0) * #Ta / 2 + #vlim * (#inT - #T3);

#q_1 :=

#vlim;

#q_2 := 0;

#q_3 := 0;

ELSIF #inT <= #T5 THEN    //减加速段

#T_org := #inT - #T4;

#q_0 := #P1 - (#vlim + #v1) * #Td / 2 + #vlim * (#T_org) - #Jm * (#T_org) ** 3 / 6;

#q_1 := #vlim - #Jm * (#T_org) ** 2 / 2;

#q_2 := - #Jm * (#T_org);

#q_3 := - #Jm;

ELSIF #inT < #T6 THEN     //减速度段

#T_org := #inT - #T7 + #Td;

#q_0 := #P1 - (#vlim + #v1) * #Td / 2 + #vlim * (#T_org) - #alimd / 6 * (3 * (#T_org) ** 2 - 3 * #Tj2 * (#T_org) + #Tj2 ** 2);

#q_1 := #vlim - #alimd * (#T_org - #Tj2 / 2);

#q_2 := - #alimd;

#q_3 := 0;

ELSIF #inT <= #T7 THEN    //减减速段

#T_org := #T7 - #inT;

#q_0 := #P1 - #v1 * (#T_org) - #Jm * (#T_org) ** 3 / 6;

#q_1 := #v1 + #Jm * (#T_org) ** 2 / 2;

#q_2 := - #Jm * (#T_org);

#q_3 := #Jm;

ELSE                   //减减速段

#T_org := 0;

#q_0 := #P1 - #v1 * (#T_org) - #Jm * (#T_org) ** 3 / 6;

#q_1 := #v1 + #Jm * (#T_org) ** 2 / 2;

#q_2 := - #Jm * (#T_org);

#q_3 := #Jm;

END_IF;

END_REGION

REGION 输出

//轨迹

#NoVar := 0;

#q[0] := #q_0;

#q[1] := #q_1;

#q[2] := #q_2;

#q[3] := #q_3;

END_REGION