西门子 1200 PLC S型速度曲线程序 FB块

显示全部楼层 · 2023-4-4 09:05:34

[table]
[tr][td][table]
[tr][td]博图版本v16
程序块写在FB那种
可以设置加加速度加速度加减速度减加速度减速度减减速度加速距离减速距离
不需要目标距离已知减速位置[/td][/tr]
[/table][/td][/tr]
[/table]

chatGPT_小智 · 2023-4-4 09:06:37

以下是一个示例程序块，用于生成S型速度曲线：

[code]
// 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
[/code]

在该程序块中，我们使用一个状态机来控制加速、匀速和减速阶段。在每个阶段，我们根据当前速度和加速度计算出速度曲线上的下一个点，从而实现平滑的S型速度曲线。在初始化时，我们先计算出加速度和减速度达到最大最小速度所需的时间，并根据加加速度约束这些参数。然后，我们将状态切换到加速阶段，并开始计算速度曲线。

该程序块还支持以下输入参数：

- StartPos：起始位置，用于计算速度曲线的第一个点。
- EndPos：目标位置，用于计算加速距离和减速距离。
- Accel：加速度，用于计算加速时间和加速到达的最大速度。
- Decel：减速度，用于计算减速时间和减速到达的最小速度。
- Jerk：加加速度，用于限制加速度和减速度的变化率。
- AccelDist：加速距离，用于计算加速时间。
- DecelDist：减速距离，用于计算减速时间。
- TotalDist：总距离，用于计算加速和减速距离。

输出参数为速度Speed，单位为mm/s。如果程序块执行完毕，即达到Done状态，则输出的速度为0。