<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4022.10">
  <POU Name="COEDriveHome" Id="{660721ab-7ff8-47ea-bace-a45a12882612}" SpecialFunc="None">
    <Declaration><![CDATA[FUNCTION_BLOCK COEDriveHome
VAR_IN_OUT
	AXIS:AXIS_REF;
	PowerOn: BOOL;
END_VAR

VAR_INPUT
	FB_Enable:BOOL;
	FB_sNetId: T_AmsNetId;
	FB_nSlaveAddr:UINT;
	FB_home_mode :SINT:=1;
	FB_home_highspeed: DINT:=1048576;
	FB_home_lowspeed: DINT:=1048576;
END_VAR
VAR_OUTPUT
	FB_Done:BOOL;
	fb_home_active:BOOL;
	Status:BOOL;
END_VAR
VAR
	FB_MC_Reset: MC_Reset;
	FB_Reset: BOOL;
	TON_1:TON;
	TON_2:TON;
	read_sdo:BOOL;
	FB_MC_Power: MC_Power;
	FB_EcCoESdoRead_6060: FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6060: FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6098: FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6098: FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6099_1: FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6099_1: FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6099_2: FB_EcCoESdoRead;
	FB_EcCoESdoWrite_6099_2: FB_EcCoESdoWrite;
	FB_EcCoESdoRead_6041: FB_EcCoESdoRead;
	servo_mode: INT:=8;
	servo_mode_displsy: INT;
	home_mode_displsy: INT;
	home_highspeed_displsy: DINT;
	home_lowspeed_displsy: DINT;
	sv_statusword: DINT;

	R_TRIG_yanshi: R_TRIG;
	oldExecute: BOOL;
	istate: INT;
	fb_resrt: BOOL;
	ao:BOOL;
	fbhome:Tc2_MC2.MC_Home;
END_VAR
]]></Declaration>
    <Implementation>
      <ST><![CDATA[///////使能（用于使能轴和触发回零）
(*FB_MC_Power(
	Axis:=AXIS, 
	Enable:= PowerOn , 
	Enable_Positive:=PowerOn ,//AND FB_POWER_ONOFF , 
	Enable_Negative:=PowerOn ,//,AND FB_POWER_ONOFF, 
	Override:= , 
	BufferMode:= , 
	Options:= , 
	Status=> Status, 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );*)

/////////轴复位	（主要用来清除本地控制偏差）
	FB_MC_Reset(
	Axis:=AXIS , 
	Execute:=FB_Reset , 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );
////////////用来循环触发读写SDO	
TON_1(IN:= , PT:=T#10MS , Q=> , ET=> );
TON_2(IN:= , PT:=T#10MS , Q=> , ET=> );
IF  NOT  ton_1.IN=TRUE   THEN
                   ton_1.IN:=TRUE;
                  read_sdo:=TRUE;
   END_IF
IF  ton_1.Q   THEN
            ton_1.IN:=FALSE;
             read_sdo:=FALSE;
      END_IF
	  
(*写入6060*)
  FB_EcCoESdoWrite_6060(
	sNetId:= FB_sNetId ,                //'169.254.105.252.3.1'
	nSlaveAddr:= FB_nSlaveAddr ,       //1001
	nSubIndex:= 0, 
	nIndex:=16#6060 , 
	pSrcBuf:= ADR( servo_mode ), 
	cbBufLen:= 1 , 
	bExecute:= servo_mode <> servo_mode_displsy  AND read_sdo  , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
	
(*读取6060*)
FB_EcCoESdoRead_6060(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr ,
	nSubIndex:= 0,
	nIndex:=16#6060 ,
	pDstBuf:=ADR( servo_mode_displsy  ) ,
	cbBufLen:=1 ,
	bExecute:= read_sdo ,
	tTimeout:= ,
	bBusy=> ,
	bError=> ,
	nErrId=> );
	
	(*写入6098*)
  FB_EcCoESdoWrite_6098(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr ,
	nSubIndex:= 0, 
	nIndex:=16#6098 , 
	pSrcBuf:= ADR( FB_home_mode), 
	cbBufLen:= 1 , 
	bExecute:= FB_home_mode <> home_mode_displsy AND read_sdo    , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
	
(*读取6098*)
FB_EcCoESdoRead_6098(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr ,
	nSubIndex:= 0,
	nIndex:=16#6098 ,
	pDstBuf:=ADR( home_mode_displsy  ) ,
	cbBufLen:=1 ,
	bExecute:= read_sdo  ,
	tTimeout:= ,
	bBusy=> ,
	bError=> ,
	nErrId=> );
	
		(*写入6099_1*)
  FB_EcCoESdoWrite_6099_1(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr , 
	nSubIndex:= 1, 
	nIndex:=16#6099 , 
	pSrcBuf:= ADR( FB_home_highspeed), 
	cbBufLen:= 4 , 
	bExecute:= FB_home_highspeed <> home_highspeed_displsy AND read_sdo     , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
	
(*读取6099_1*)
FB_EcCoESdoRead_6099_1(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr ,
	nSubIndex:= 1,
	nIndex:=16#6099 ,
	pDstBuf:=ADR( home_highspeed_displsy  ) ,
	cbBufLen:=4 ,
	bExecute:=  read_sdo  ,
	tTimeout:= ,
	bBusy=> ,
	bError=> ,
	nErrId=> );
	
		(*写入6099_2*)
  FB_EcCoESdoWrite_6099_2(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr ,
	nSubIndex:= 2, 
	nIndex:=16#6099 , 
	pSrcBuf:= ADR( FB_home_lowspeed), 
	cbBufLen:= 4 , 
	bExecute:= FB_home_lowspeed <> home_lowspeed_displsy  AND read_sdo    , 
	tTimeout:= , 
	bBusy=> , 
	bError=> , 
	nErrId=> );
	
(*读取6099_2*)
FB_EcCoESdoRead_6099_2(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr ,
	nSubIndex:= 2,
	nIndex:=16#6099 ,
	pDstBuf:=ADR( home_lowspeed_displsy  ) ,
	cbBufLen:=4 ,
	bExecute:= read_sdo   ,
	tTimeout:= ,
	bBusy=> ,
	bError=> ,
	nErrId=> );

	(*读取6041*)
FB_EcCoESdoRead_6041(
	sNetId:=FB_sNetId ,
	nSlaveAddr:=FB_nSlaveAddr ,
	nSubIndex:= 0,
	nIndex:=16#6041 ,
	pDstBuf:=ADR( sv_statusword ) ,
	cbBufLen:=2 ,
	bExecute:=  read_sdo  ,
	tTimeout:= ,
	bBusy=> ,
	bError=> ,
	nErrId=> );	 
	

///////开始程序
R_TRIG_yanshi(CLK:=FB_Enable AND (FB_Enable <> oldExecute) AND istate = 0 , Q=> );	
IF  R_TRIG_yanshi.Q  THEN 
	iState := 5;
	oldExecute := FB_Enable;	
 	fbhome(Axis:=AXIS , Execute:=FALSE , Position:= 0, HomingMode:=MC_ResetCalibration);
  	fbhome(Axis:=AXIS , Execute:=TRUE , Position:= 0, HomingMode:=MC_ResetCalibration);
 
	
ELSIF FB_Enable = FALSE  THEN
	oldExecute := FB_Enable;
	ao:=FALSE;
	FB_Done:=FALSE;
	PowerOn:=FALSE;
	FB_Reset:=FALSE;
	istate:=0;
	fb_home_active:=FALSE;
	
END_IF

CASE istate  OF
	
5:
     FB_Reset:=TRUE;
	istate:=7;
7:
IF  FB_MC_Reset.Done  THEN
		istate:=10;
		FB_Reset:=FALSE;
END_IF
10://////切换模式
   ao:=TRUE;
   fb_home_active:=TRUE;
    PowerOn:=FALSE;
      FB_resrt :=FALSE;
       servo_mode:=6 ;
 		fbhome(Axis:=AXIS , Execute:=FALSE , Position:= 0, HomingMode:=MC_ResetCalibration);
 
	   fb_home_active:=TRUE;
    PowerOn:=FALSE;
      FB_resrt :=FALSE;
       servo_mode:=6 ;
	   
(*power*)
IF  servo_mode_displsy=6  THEN
	istate:=11;     
END_IF
11:
  PowerOn:=TRUE;
  IF NOT  sv_statusword.12 THEN
	  istate:=12;
  END_IF
  12:
(*qingpiancha  sv_statusword.15  AND*)
IF    sv_statusword.12  THEN
     FB_Reset:=TRUE;
END_IF
IF  FB_MC_Reset.Done  THEN
 servo_mode:=8 ;
 FB_Done:=TRUE;
 istate:=13;
 	fbhome(Axis:=AXIS , Execute:=FALSE , Position:= 0, HomingMode:=MC_ForceCalibration);
  END_IF
13:
	 FB_Reset:=FALSE;
	fbhome(Axis:=AXIS , Execute:=TRUE , Position:= 0, HomingMode:=MC_ForceCalibration);
 

	
	END_CASE
	]]></ST>
    </Implementation>
    <LineIds Name="COEDriveHome">
      <LineId Id="3" Count="166" />
      <LineId Id="369" Count="0" />
      <LineId Id="322" Count="0" />
      <LineId Id="370" Count="0" />
      <LineId Id="321" Count="0" />
      <LineId Id="170" Count="7" />
      <LineId Id="320" Count="0" />
      <LineId Id="178" Count="2" />
      <LineId Id="428" Count="1" />
      <LineId Id="431" Count="2" />
      <LineId Id="430" Count="0" />
      <LineId Id="434" Count="0" />
      <LineId Id="436" Count="0" />
      <LineId Id="435" Count="0" />
      <LineId Id="181" Count="5" />
      <LineId Id="368" Count="0" />
      <LineId Id="336" Count="0" />
      <LineId Id="351" Count="4" />
      <LineId Id="188" Count="5" />
      <LineId Id="374" Count="3" />
      <LineId Id="194" Count="7" />
      <LineId Id="427" Count="0" />
      <LineId Id="202" Count="2" />
      <LineId Id="373" Count="0" />
      <LineId Id="304" Count="1" />
      <LineId Id="303" Count="0" />
      <LineId Id="205" Count="0" />
      <LineId Id="2" Count="0" />
    </LineIds>
  </POU>
</TcPlcObject>