VAR_IN_OUT
TSMx : TSM; //轴号
END_VAR
VAR_INPUT
switch_on : BOOL:=TRUE; //使能
Fault_reset : BOOL; //复位
Start : BOOL; //启动
FWD_REV : BOOL; //正转标志（伸出）
velocity : DINT:=2000; //运行速度
acceleration : UDINT:=50000; //加速度
deceleration : UDINT:=50000; //减速度
positivelimit : BOOL; //伸出到位
nagativelimit : BOOL; //缩回到位
END_VAR
VAR_OUTPUT
Ready : BOOL; //状态读取
Error : BOOL; //故障
Error_code : UINT; //故障代码
Error_s : STRING;
END_VAR
VAR
F_TRIG_Error: F_TRIG;
F_R:INT;
END_VAR
//速度设置
IF FWD_REV THEN
F_R := -1;
ELSE
F_R := 1;
END_IF
TSMx.Target_velocity := velocity*F_R;
TSMx.Profile_acceleration := acceleration;
TSMx.Profile_deceleration := deceleration;
//自动使能
IF TSMx.Statusword.0 THEN
TSMx.Controlword.0 := switch_on;
END_IF
//启停
IF TSMx.Statusword.0 AND TSMx.Statusword.1 THEN
Ready := TRUE;
IF Start = FALSE THEN
TSMx.Controlword := 271;
ELSIF Start AND FWD_REV AND positivelimit THEN
TSMx.Controlword := 271;
ELSIF Start AND FWD_REV = FALSE AND nagativelimit THEN
TSMx.Controlword := 271;
ELSIF Start THEN
TSMx.Controlword := 15;
END_IF
ELSE
Ready := FALSE;
END_IF
//错误
Error := TSMx.Statusword.3;
Error_code := TSMx.Error_code;
IF TSMx.Statusword.3 THEN
//错误复位
TSMx.Controlword.7 := Fault_reset;
TSMx.Controlword.0 := FALSE;
CASE TSMx.Error_code OF
16#7500 :Error_s:='EtherCAT Communication Error';
16#FF01 :Error_s:='Over Current';
16#FF02 :Error_s:='Voltage high';
16#FF03 :Error_s:='Over Temperature';
16#FF04 :Error_s:='Motor open winding';
16#FF05 :Error_s:='Internal Voltage Bad';
16#FF06 :Error_s:='Position limit';
16#FF07 :Error_s:='Encoder bad';
16#FF31 :Error_s:='CW Limit';
16#FF32 :Error_s:='CCW Limit';
16#FF33 :Error_s:='CCW and CW limit';
16#FF34 :Error_s:='Current limit';
16#FF35 :Error_s:='Move When Disable';
16#FF36 :Error_s:='Voltage Low';
16#FF37 :Error_s:='Blank Q segment';
16#FF41 :Error_s:='Save Failed';
16#FF42 :Error_s:='XML read failed';
16#FFFF :Error_s:='Other Error';
ELSE
Error_s:='Reserved';
END_CASE
ELSE
Error_s:='NO ERROR';
TSMx.Controlword.7 := FALSE;
END_IF
F_TRIG_Error(CLK:= TSMx.Statusword.3, Q=> );
IF F_TRIG_Error.Q THEN
TSMx.Controlword := 270;
END_IF
