2025-01-20 自用Fb_MoveFeedTP总线轴中断定长功能块

杂学阁 · 8 小时前

前言

首先，中断定长这个功能在很多小型的PLC里面都有封装现成的指令的可以使用的，如三菱的DVIT、雷赛自研平台的MC_MoveFeed、翠欧的MoveModify等等。今天来和大家分享的是我自己在用的一个EtherCAT总线伺服的中断定长功能。

正文

1. Code Show(代码分享)

1.1 自定义数据类型ParaFeedTP_DUT

TYPE ParaFeedTP_DUT :STRUCTbWindow : BOOL ;LrStartVel : LREAL ;LrStartAcc : LREAL ;LrStartDec : LREAL ;LrFeedDis : LREAL ;LrFeedVel : LREAL ;LrFeedAcc : LREAL ;LrFeedDec : LREAL ;LrJerk : LREAL ;LrFirstPos : LREAL ;LrLastPos : LREAL ;LrLossPos : LREAL ;END_STRUCTEND_TYPE

1.2 功能块Fb_MoveFeedTP

FUNCTION_BLOCK Fb_MoveFeedTPVAR_INPUT PtAxis : POINTER TO AXIS_REF_SM3 ; TriggerInputRef : REFERENCE TO TRIGGER_REF ; sParaFeedTP : ParaFeedTP_DUT ; xExecute : BOOL ;END_VARVAR_OUTPUT bDone : BOOL := FALSE ; wErrorID : WORD := 16#0000 ;END_VARVAR MoveVelocity : MC_MoveVelocity; TouchProbe : MC_TouchProbe; MoveRelative : MC_MoveRelative; Stop : MC_Stop ; LrTempPos : LREAL ; iStep : INT ;END_VAR

CASE iStep OF 0 : MoveVelocity.Execute := FALSE ; MoveRelative.Execute := FALSE ; TouchProbe.Execute := FALSE ; Stop.Execute := FALSE ; bDone := FALSE ; wErrorID := 16#0000 ; IF xExecute THEN iStep := iStep + 10 ; END_IF ; 10: IF sParaFeedTP.LrStartVel <> 0 THEN iStep := iStep + 1 ; ELSE wErrorID := 16#0001 ; iStep := 90 ; END_IF ; 11: IF sParaFeedTP.LrStartAcc <> 0 THEN iStep := iStep + 1 ; ELSE wErrorID := 16#0001 ; iStep := 90 ; END_IF ; 12: IF sParaFeedTP.LrStartDec <> 0 THEN iStep := iStep + 1 ; ELSE wErrorID := 16#0001 ; iStep := 90 ; END_IF ; 13: IF sParaFeedTP.LrFeedVel <> 0 THEN iStep := iStep + 1 ; ELSE wErrorID := 16#0001 ; iStep := 90 ; END_IF ; 14: IF sParaFeedTP.LrFeedAcc <> 0 THEN iStep := iStep + 1 ; ELSE wErrorID := 16#0001 ; iStep := 90 ; END_IF ; 15: IF sParaFeedTP.LrFeedDec <> 0 THEN iStep := iStep + 1 ; ELSE wErrorID := 16#0001 ; iStep := 90 ; END_IF ; 16: iStep := 20 ; 20: LrTempPos := PtAxis^.fActPosition ; MoveVelocity.Execute := TRUE ; TouchProbe.Execute := TRUE ; iStep := 30 ; 30: IF MoveVelocity.Error THEN wErrorID := 16#0010 ; iStep := 90 ; END_IF ; IF TouchProbe.Error THEN wErrorID := 16#0100 ; iStep := 90 ; END_IF ; IF NOT sParaFeedTP.bWindow OR (PtAxis^.iMovementType = 1) THEN IF ((PtAxis^.fActPosition - LrTempPos) > sParaFeedTP.LrLossPos) THEN Stop.Execute := TRUE ; wErrorID := 16#0110 ; iStep := 90 ; END_IF ; ELSE IF (PtAxis^.fActPosition > sParaFeedTP.LrFirstPos) AND (PtAxis^.fActPosition < sParaFeedTP.LrLastPos) THEN ; ELSE Stop.Execute := TRUE ; wErrorID := 16#0111 ; iStep := 90 ; END_IF ; END_IF ; IF TouchProbe.Done THEN MoveRelative.Execute := TRUE ; iStep := 40 ; END_IF ; 40: IF MoveRelative.Done THEN iStep := 50 ; END_IF ; IF MoveRelative.Error THEN wErrorID := 16#1000 ; iStep := 90 ; END_IF ; 50: bDone := TRUE ; IF NOT xExecute THEN iStep := 0 ; END_IF ; 90: IF Stop.Execute AND Stop.Done THEN Stop.Execute := FALSE ; END_IF ; IF NOT xExecute THEN iStep := 0 ; END_IF ;ELSE ;END_CASE;

MoveVelocity( Axis := PtAxis^, Execute := , Velocity := sParaFeedTP.LrStartVel, Acceleration := sParaFeedTP.LrStartAcc, Deceleration := sParaFeedTP.LrStartDec, Jerk := sParaFeedTP.LrJerk, Direction := , BufferMode := , InVelocity => , Busy => , Active => , CommandAborted => , Error => , ErrorID => ); TouchProbe( Axis := PtAxis^, TriggerInput := TriggerInputRef, Execute := , WindowOnly := sParaFeedTP.bWindow, FirstPosition := sParaFeedTP.LrFirstPos, LastPosition := sParaFeedTP.LrLastPos, Done => , Busy => , Error => , ErrorID => , RecordedPosition => , CommandAborted => ); MoveRelative( Axis := PtAxis^, Execute := , Distance := sParaFeedTP.LrFeedDis, Velocity := sParaFeedTP.LrFeedVel, Acceleration := sParaFeedTP.LrFeedAcc, Deceleration := sParaFeedTP.LrFeedDec, Jerk := sParaFeedTP.LrJerk, BufferMode := MC_BUFFER_MODE.Aborting, Done => , Busy => , Active => , CommandAborted => , Error => , ErrorID => );

Stop( Axis := PtAxis^, Execute := , Deceleration := sParaFeedTP.LrFeedDec * 10.0, Jerk := sParaFeedTP.LrJerk * 10.0, Done => , Busy => , Error => , ErrorID => );

1.3 测试程序Pro_Motion

PROGRAM Pro_MotionVAR Power : MC_Power; Stop : MC_Stop ; Reset : MC_Reset ; MoveFeedTP : Fb_MoveFeedTP; RefTrigger : TRIGGER_REF ; SetPos : MC_SetPosition ; iMStep : INT ; bEnable : BOOL; bStart : BOOL; bStop : BOOL; bReset : BOOL;END_VAR

Power( Axis:= AxisX, Enable:= TRUE, bRegulatorOn:= , bDriveStart:= TRUE, Status=> , bRegulatorRealState=> , bDriveStartRealState=> , Busy=> , Error=> , ErrorID=> );

Stop( Axis := AxisX, Execute := , Deceleration := 100000.0, Jerk := 100000.0, Done => , Busy => , Error => , ErrorID => );

Reset( Axis := AxisX, Execute := , Done => , Busy => , Error => , ErrorID => ); MoveFeedTP( PtAxis := ADR(AxisX), TriggerInputRef := RefTrigger, sParaFeedTP := , xExecute := , bDone => , wErrorID => ); SetPos( Axis := AxisX, Execute := , Position := 0.0, Mode := , Done => , Busy => , Error => , ErrorID => ); IF NOT bEnable THEN iMStep := 0 ;END_IF ; IF bStop THEN iMStep := 100 ;END_IF CASE iMStep OF 0 : IF bEnable THEN Power.bRegulatorOn := bEnable ; ELSE Power.bRegulatorOn := FALSE ; END_IF ; IF Power.Status THEN iMStep := 10 ; END_IF ; 10: RefTrigger.iTriggerNumber := 1 ; IF bStart THEN MoveFeedTP.xExecute := TRUE ; END_IF ; IF MoveFeedTP.bDone THEN MoveFeedTP.xExecute := FALSE ; bStart := FALSE ; iMStep := 20 ; END_IF ; IF MoveFeedTP.wErrorID <> 0 THEN iMStep := 90 ; END_IF ; 20: SetPos.Execute := TRUE ; IF SetPos.Done THEN SetPos.Execute := FALSE ; iMStep := 0 ; END_IF ; 90: IF bReset THEN MoveFeedTP.xExecute := FALSE ; bStart := FALSE ; bReset := FALSE ; iMStep := 0 ; END_IF ; 100: bStart := FALSE ; MoveFeedTP.xExecute := FALSE ; Stop.Execute := TRUE ; IF Stop.Done AND NOT bStop THEN Stop.Execute := FALSE ; iMStep := 0 ; END_IF ;ELSE ;END_CASE ;

2. Hardware Config(硬件配置)

测试平台：雷赛MC508 + 雷赛L6N

2.1 接口定义

*DI4/DI5 可复用通用输入 DI，当 DI4、DI5 默认无分配功能，作探针使用

2.2 接口回路

3. Show (效果曲线)

3.1 中断理想曲线

3.2 启动速度过大，导致位置反揪

3.3 超位置停止

更多关于探针的使用可自行观看雷赛L7N伺服手册有详细说明。

核心原理就是利用伺服的锁存功能，去实现看到某一个外部型号后，在以编码器当前的位置去进行相对运动。（现在市面上有不少伺服都有这样子的功能，如台达的A2-M里面的Capture功能。）

结尾