西门子 1200 PLC S型速度曲线程序 FB块
博图版本v16
程序块写在FB那种
可以设置 加加速度 加速度 加减速度 减加速度减速度 减减速度 加速距离减速距离
不需要目标距离已知减速位置
以下是一个示例程序块，用于生成S型速度曲线：
// S型速度曲线程序 FB块
// 输入参数
VAR_INPUT
StartPos : REAL; // 起始位置，单位：mm
EndPos : REAL; // 目标位置，单位：mm
Accel : REAL; // 加速度，单位：mm/s^2
Decel : REAL; // 减速度，单位：mm/s^2
Jerk : REAL; // 加加速度，单位：mm/s^3
AccelDist : REAL; // 加速距离，单位：mm
DecelDist : REAL; // 减速距离，单位：mm
TotalDist : REAL; // 总距离，单位：mm
END_VAR
// 输出参数
VAR_OUTPUT
Speed : REAL; // 速度，单位：mm/s
END_VAR
// 内部参数
VAR
State : INT; // 状态机
Time : REAL; // 时间
Pos : REAL; // 位置
AccelTime : REAL; // 加速时间
DecelTime : REAL; // 减速时间
AccelSpeed : REAL; // 加速到达的最大速度
DecelSpeed : REAL; // 减速到达的最小速度
AccelDistLeft : REAL; // 剩余加速距离
DecelDistLeft : REAL; // 剩余减速距离
END_VAR
// 初始化
STATE Init:
State := 1;
Time := 0;
Pos := StartPos;
AccelDistLeft := AccelDist;
DecelDistLeft := DecelDist;
AccelSpeed := Accel;
DecelSpeed := SQR(EndPos - StartPos) * Decel / (2 * TotalDist) + Jerk * (Jerk * TotalDist * TotalDist + 2 * Accel * TotalDist) / (6 * TotalDist * (Jerk * TotalDist + Accel));
IF AccelSpeed > DecelSpeed THEN
AccelSpeed := Min(AccelSpeed, SQR(TotalDist * Decel * Jerk / Accel / (2 * Jerk * TotalDist + Accel)));
DecelSpeed := SQR(TotalDist * Accel * Jerk / Decel / (2 * Jerk * TotalDist - Decel));
END_IF
AccelTime := (AccelSpeed - Accel) / Jerk;
DecelTime := (DecelSpeed - Decel) / Jerk;
IF AccelSpeed < Accel * FB_CYCLE_TIME THEN
Speed := AccelSpeed;
State := 9;
END_IF
IF DecelSpeed < Decel * FB_CYCLE_TIME THEN
Speed := DecelSpeed;
State := 8;
END_IF
Speed := Accel;
State := 2;
END_STATE
// 加速阶段
STATE AccelPhase:
// 计算当前位置和速度
Pos := Pos + Speed * FB_CYCLE_TIME + 0.5 * Jerk * FB_CYCLE_TIME * FB_CYCLE_TIME;
Speed := Speed + Jerk * FB_CYCLE_TIME;
// 更新剩余加速距离
AccelDistLeft := AccelDistLeft - Speed * FB_CYCLE_TIME;
// 判断是否需要切换状态
IF AccelDistLeft < 0 THEN
State := 3;
Speed := AccelSpeed;
ELSEIF Speed >= AccelSpeed THEN
State := 4;
END_IF
END_STATE
// 匀速阶段
STATE ConstSpeedPhase:
// 计算当前位置和速度
Pos := Pos + Speed * FB_CYCLE_TIME;
// 判断是否需要切换状态
IF DecelDistLeft >= TotalDist - Pos - 0.5 * (Speed + DecelSpeed) / Decel * (Speed + DecelSpeed) / Decel / Decel THEN
DecelDistLeft := TotalDist - Pos - 0.5 * (DecelSpeed - Speed) / Decel * (DecelSpeed + Speed) / Decel / Decel;
END_IF
IF DecelDistLeft <= 0 THEN
State := 8;
Speed := DecelSpeed;
END_IF
END_STATE
// 减速阶段
STATE DecelPhase:
// 计算当前位置和速度
Pos := Pos + Speed * FB_CYCLE_TIME - 0.5 * Decel * FB_CYCLE_TIME * FB_CYCLE_TIME;
Speed := Speed - Decel * FB_CYCLE_TIME;
// 更新剩余减速距离
DecelDistLeft := DecelDistLeft - Speed * FB_CYCLE_TIME;
// 判断是否需要切换状态
IF DecelDistLeft < 0 THEN
State := 9;
Speed := 0;
ELSEIF Speed <= DecelSpeed THEN
State := 8;
Speed := DecelSpeed;
END_IF
END_STATE
// 完成状态
STATE Done:
Speed := 0;
END_STATE
// 主循环
CASE State OF
Init:
GOTO AccelPhase;
AccelPhase:
GOTO ConstSpeedPhase;
ConstSpeedPhase:
GOTO DecelPhase;
DecelPhase:
GOTO Done;
Done:
// do nothing
END_CASE
在该程序块中，我们使用一个状态机来控制加速、匀速和减速阶段。在每个阶段，我们根据当前速度和加速度计算出速度曲线上的下一个点，从而实现平滑的S型速度曲线。在初始化时，我们先计算出加速度和减速度达到最大最小速度所需的时间，并根据加加速度约束这些参数。然后，我们将状态切换到加速阶段，并开始计算速度曲线。
该程序块还支持以下输入参数：
- StartPos：起始位置，用于计算速度曲线的第一个点。
- EndPos：目标位置，用于计算加速距离和减速距离。
- Accel：加速度，用于计算加速时间和加速到达的最大速度。
- Decel：减速度，用于计算减速时间和减速到达的最小速度。
- Jerk：加加速度，用于限制加速度和减速度的变化率。
- AccelDist：加速距离，用于计算加速时间。
- DecelDist：减速距离，用于计算减速时间。
- TotalDist：总距离，用于计算加速和减速距离。
输出参数为速度Speed，单位为mm/s。如果程序块执行完毕，即达到Done状态，则输出的速度为0。
页:
[1]