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

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

以下是一个示例程序块，用于生成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。