帮我写一个s7-1200控制V90伺服的扭矩控制FB块程序

要求：
1.以定位方式从位置1到位置2；
2. 到达位置2后以速度+扭矩方式继续下压运行，运行中的输出扭矩可设置；
3.当压到东西后，扭矩达到限定值时停止运行，并且扭矩保持5s ；
4.保持时间到后，以定位方式退回位置1；

下面是一个基本的扭矩控制FB块程序，适用于S7-1200 PLC和V90伺服。程序分为5个部分，分别用于初始化、位置定位、扭矩控制、停止并保持、退回位置1。

第一部分 - 初始化

该部分主要设置FB块所需的参数，并初始化伺服驱动器。

[code]FUNCTION_BLOCK Torque_Control_FB
VAR_INPUT
    Target_Pos : INT; // 目标位置，单位mm
    Torque_Limit : INT; // 扭矩限制，单位N.m
END_VAR

VAR_OUTPUT
    Start : BOOL; // 执行信号
    Done : BOOL; // 完成信号
END_VAR

VAR_IN_OUT
    Axis_Control : AXIS_CONTROL_TYPE := Axis_Control_Default; // 轴控制参数
    Axis_Status : AXIS_STATUS_TYPE := Axis_Status_Default; // 轴状态参数
END_VAR

VAR
    Max_Speed : REAL := 200.0; // 最大速度，单位mm/s
    Max_Acc : REAL := 100.0; // 最大加速度，单位mm/s^2
    Max_Dec : REAL := 100.0; // 最大减速度，单位mm/s^2
    Torque_Mode : INT := 23#6050; // 以速度+扭矩方式控制
    Torque_Set : INT; // 实时扭矩设定值
END_VAR

// 初始化
Axis_Control.HomeAxes(); // 执行回原点操作
Axis_Control.SetMaxSpeed(Max_Speed); // 设置最大速度
Axis_Control.SetMaxAcc(Max_Acc, Max_Dec); // 设置最大加速度和减速度
Axis_Control.SetControlMode(Torque_Mode); // 设置扭矩控制模式

Start := FALSE; // 默认不执行
Done := FALSE; // 默认未完成
Torque_Set := 0; // 初始扭矩设定值为0

END_FUNCTION_BLOCK[/code]

第二部分 - 位置定位

该部分将伺服驱动器控制到目标位置1，以定位方式运动。

[code]FUNCTION_BLOCK Torque_Control_FB_Pos1
VAR_INPUT
    Target_Pos : INT; // 目标位置，单位mm
END_VAR

VAR_OUTPUT
    Done : BOOL; // 完成信号
END_VAR

VAR_IN_OUT
    Axis_Control : AXIS_CONTROL_TYPE := Axis_Control_Default; // 轴控制参数
    Axis_Status : AXIS_STATUS_TYPE := Axis_Status_Default; // 轴状态参数
END_VAR

VAR CONSTANT
    Pos1_Pos : INT := 100; // 位置1的实际位置，单位mm
END_VAR

// 检查是否已经到达位置1
IF (Axis_Status.GetActualPos() = Pos1_Pos) THEN
    Done := TRUE; // 已经到达位置1
ELSE // 未到达位置1，继续运动
    Done := FALSE;
    Axis_Control.MoveAbsolute(Pos1_Pos, Max_Speed, Max_Acc, Max_Dec);
END_IF

END_FUNCTION_BLOCK[/code]

第三部分 - 扭矩控制

该部分将伺服驱动器从位置1控制到目标位置2，同时启用扭矩控制。

[code]FUNCTION_BLOCK Torque_Control_FB_Torque
VAR_INPUT
    Target_Pos : INT; // 目标位置，单位mm
    Torque_Limit : INT; // 扭矩限制，单位N.m
END_VAR

VAR_OUTPUT
    Done : BOOL; // 完成信号
END_VAR

VAR_IN_OUT
    Axis_Control : AXIS_CONTROL_TYPE := Axis_Control_Default; // 轴控制参数
    Axis_Status : AXIS_STATUS_TYPE := Axis_Status_Default; // 轴状态参数
END_VAR

VAR CONSTANT
    Pos2_Pos : INT := 200; // 位置2的实际位置，单位mm
END_VAR

// 检查是否已经到达位置2
IF (Axis_Status.GetActualPos() = Pos2_Pos) THEN
    // 运行中扭矩设定值
    IF (Axis_Status.GetControlMode() = Torque_Mode) THEN
        Torque_Set := Torque_Limit; // 达到限定值后保持设定扭矩
    ELSE // 未到达位置2，继续运动
        Axis_Control.TorqueModeStart(0); // 启用扭矩模式，扭矩设定值为0
        Torque_Set := Torque_Limit; // 初始扭矩设定值为限定值
        Axis_Control.MoveAbsolute(Pos2_Pos, Max_Speed, Max_Acc, Max_Dec); // 以定位方式运动到位置2
    END_IF
ELSE // 运行中扭矩设定值随时间变化
    Done := FALSE;
    IF (Axis_Status.GetControlMode() = Torque_Mode) THEN
        // 扭矩已经达到限定值，保持5s
        IF (Axis_Status.GetTorqueValue() >= Torque_Limit) THEN
            Axis_Control.Halt(); // 停止运行
            Torque_Set := 0; // 设定扭矩值清零
            DELAY_TIME(5); // 延时5s
            Done := TRUE; // 完成信号
        ELSE // 扭矩未达到限定值，继续增加扭矩设定值
            Torque_Set := Torque_Set + 10; // 每次增加10N.m
        END_IF
    END_IF
END_IF

Axis_Control.SetTorqueValue(Torque_Set); // 实时设置扭矩设定值

END_FUNCTION_BLOCK[/code]

第四部分 - 停止并保持

该部分将伺服驱动器从位置2控制到位置1，并在达到限定扭矩后停止并保持5秒。

[code]FUNCTION_BLOCK Torque_Control_FB_Stop
VAR_INPUT
    Target_Pos : INT; // 目标位置，单位mm
    Torque_Limit : INT; // 扭矩限制，单位N.m
END_VAR

VAR_OUTPUT
    Done : BOOL; // 完成信号
END_VAR

VAR_IN_OUT
    Axis_Control : AXIS_CONTROL_TYPE := Axis_Control_Default; // 轴控制参数
    Axis_Status : AXIS_STATUS_TYPE := Axis_Status_Default; // 轴状态参数
END_VAR

VAR CONSTANT
    Pos1_Pos : INT := 100; // 位置1的实际位置，单位mm
END_VAR

// 检查是否已经到达位置1
IF (Axis_Status.GetActualPos() = Pos1_Pos) THEN
    Done := TRUE; // 已经到达位置1
ELSE // 未到达位置1，继续运动
    Done := FALSE;
    Axis_Control.MoveAbsolute(Pos1_Pos, Max_Speed, Max_Acc, Max_Dec);
    IF (Axis_Status.GetControlMode() = Torque_Mode) THEN
        // 扭矩已经达到限定值，保持5s
        IF (Axis_Status.GetTorqueValue() >= Torque_Limit) THEN
            Axis_Control.Halt(); // 停止运行
            DELAY_TIME(5); // 延时5s
        END_IF
    END_IF
END_IF

END_FUNCTION_BLOCK[/code]

第五部分 - 退回位置1

该部分将伺服驱动器从位置2控制到位置1。

[code]FUNCTION_BLOCK Torque_Control_FB_Back
VAR_INPUT
    Target_Pos : INT; // 目标位置，单位mm
END_VAR

VAR_OUTPUT
    Done : BOOL; // 完成信号
END_VAR

VAR_IN_OUT
    Axis_Control : AXIS_CONTROL_TYPE := Axis_Control_Default; // 轴控制参数
    Axis_Status : AXIS_STATUS_TYPE := Axis_Status_Default; // 轴状态参数
END_VAR

VAR CONSTANT
    Pos1_Pos : INT := 100; // 位置1的实际位置，单位mm
END_VAR

// 检查是否已经到达位置1
IF (Axis_Status.GetActualPos() = Pos1_Pos) THEN
    Done := TRUE; // 已经到达位置1
ELSE // 未到达位置1，继续运动
    Done := FALSE;
    Axis_Control.MoveAbsolute(Pos1_Pos, Max_Speed, Max_Acc, Max_Dec);
END_IF

END_FUNCTION_BLOCK[/code]

总体程序

将上述5个部分组合起来，即可实现从位置1到位置2的扭矩控制运动。

[code]FUNCTION_BLOCK Torque_Control_FB_Main
VAR_INPUT
    Target_Pos : INT; // 目标位置，单位mm
    Torque_Limit : INT; // 扭矩限制，单位N.m
END_VAR

VAR_OUTPUT
    Done : BOOL; // 完成信号
END_VAR

VAR_IN_OUT
    Axis_Control : AXIS_CONTROL_TYPE := Axis_Control_Default; // 轴控制参数
    Axis_Status : AXIS_STATUS_TYPE := Axis_Status_Default; // 轴状态参数
    Sub_FB_Pos1 : Torque_Control_FB_Pos1; // 位置定位子程序
    Sub_FB_Torque : Torque_Control_FB_Torque; // 扭矩控制子程序
    Sub_FB_Stop : Torque_Control_FB_Stop; // 停止并保持子程序
    Sub_FB_Back : Torque_Control_FB_Back; // 退回位置1子程序
END_VAR

VAR CONSTANT
    Run_State_Idle : INT := 0; // 空闲状态
    Run_State_Pos1 : INT := 1; // 位置1状态
    Run_State_Torque : INT := 2; // 扭矩控制状态
    Run_State_Stop : INT := 3; // 停止并保持状态
    Run_State_Back : INT := 4; // 退回位置1状态
END_VAR

VAR
    Run_State : INT := Run_State_Idle; // 当前运行状态
END_VAR

CASE Run_State OF
    Run_State_Idle: // 空闲状态
        Done := TRUE; // 完成信号
        IF (Start) THEN
            // 进入位置1状态
            Sub_FB_Pos1.Target_Pos := 100; // 设置目标位置为位置1
            Sub_FB_Pos1(); // 执行位置定位子程序
            Run_State := Run_State_Pos1;
        END_IF
    Run_State_Pos1: // 位置1状态
        Sub_FB_Pos1.Target_Pos := 100; // 设置目标位置为位置1
        Sub_FB_Pos1(); // 执行位置定位子程序
        IF (Sub_FB_Pos1.Done) THEN
            // 进入扭矩控制状态
            Sub_FB_Torque.Target_Pos := 200; // 设置目标位置为位置2
            Sub_FB_Torque.Torque_Limit := Torque_Limit; // 设置扭矩限制值
            Sub_FB_Torque(); // 执行扭矩控制子程序
            Run_State := Run_State_Torque;
        END_IF
    Run_State_Torque: // 扭矩控制状态
        Sub_FB_Torque.Target_Pos := 200; // 设置目标位置为位置2
        Sub_FB_Torque.Torque_Limit := Torque_Limit; // 设置扭矩限制值
        Sub_FB_Torque(); // 执行扭矩控制子程序
        IF (Sub_FB_Torque.Done) THEN
            // 进入停止并保持状态
            Sub_FB_Stop.Target_Pos := 200; // 设置目标位置为位置2
            Sub_FB_Stop.Torque_Limit := Torque_Limit; // 设置扭矩限制值
            Sub_FB_Stop(); // 执行停止并保持子程序
            Run_State := Run_State_Stop;
        END_IF
    Run_State_Stop: // 停止并保持状态
        Sub_FB_Stop.Target_Pos := 200; // 设置目标位置为位置2
        Sub_FB_Stop.Torque_Limit := Torque_Limit; // 设置扭矩限制值
        Sub_FB_Stop(); // 执行停止并保持子程序
        IF (Sub_FB_Stop.Done) THEN
            // 进入退回位置1状态
            Sub_FB_Back.Target_Pos := 100; // 设置目标位置为位置1
            Sub_FB_Back(); // 执行退回位置1子程序
            Run_State := Run_State_Back;
        END_IF
    Run_State_Back: // 退回位置1状态
        Sub_FB_Back.Target_Pos := 100; // 设置目标位置为位置1
        Sub_FB_Back(); // 执行退回位置1子程序
        IF (Sub_FB_Back.Done) THEN
            // 进入空闲状态
            Done := TRUE; // 完成信号
            Run_State := Run_State_Idle;
        END_IF
END_CASE

END_FUNCTION_BLOCK[/code]