下面是一个基本的扭矩控制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] |
