<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4022.10">
  <POU Name="PRG_NciOnRotation" Id="{bf43d013-1ae2-4ef0-b721-8812636d44e3}" SpecialFunc="None">
    <Declaration><![CDATA[PROGRAM PRG_NciOnRotation
VAR
	
	XposVirtual: LREAL;
	YposVirtual: LREAL;
	Radius: LREAL;
	AngleOriginal: LREAL;
	Xpos: LREAL;
	Ypos: LREAL;
	AngelDelta: LREAL;
	pai:LREAL:=3.14159;
	
	fbxtSetPointGenEnable: MC_ExtSetPointGenEnable;
	fbExtSetPointGenDisable: MC_ExtSetPointGenDisable;
	fbxtSetPointGenEnable2: MC_ExtSetPointGenEnable;
	fbExtSetPointGenDisable2: MC_ExtSetPointGenDisable;
	fbMoveAbs1,fbMoveAbs2,fbMoveAbs3:Tc2_MC2.MC_MoveAbsolute;
	fbpwoer1,fbpower2,fbpower3:Tc2_MC2.MC_Power;
	fbReset1,fbReset2,fbReset3:Tc2_MC2.mc_reset;
	fbGearin:Tc2_MC2.MC_GearIn;
	fbSetPos,fbsetpos2,fbsetpos3:Tc2_MC2.MC_SetPosition;
	istate: INT;
	SetVelo:LREAL:=40;
	rtEnable:Tc2_Standard.R_TRIG;
	_Xpos: LREAL;
	Direction: DINT;
	_Ypos: LREAL;
	Direction2: DINT;
	
	//home
	fbHomeX,fbHomeY,fbHomeR:COEDriveHome;
	homestate: INT;
	rthome:Tc2_Standard.R_TRIG;
	rtCOEHOME:Tc2_Standard.R_TRIG;
END_VAR

VAR_IN_OUT
	XVirtual: AXIS_REF;
	YVirtual: AXIS_REF;
	RotationAxis:AXIS_REF;
	X: AXIS_REF;
	Y: AXIS_REF;	
	XWPS_OFFSET:LREAL;
	YWPS_OFFSET:LREAL;
	Enable:BOOL;
END_VAR]]></Declaration>
    <Implementation>
      <ST><![CDATA[CalcPos();
Home();
X.ReadStatus();
y.ReadStatus();
RotationAxis.ReadStatus();
arrAxis_REF[3].ReadStatus();
fbpwoer1(
	Axis:=xaxis , 
	Enable:=PLCAxisEnable , 
	Enable_Positive:= TRUE, 
	Enable_Negative:=TRUE , 
	Override:=100 , 
	BufferMode:= , 
	Options:= , 
	Status=> , 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );
fbpower2(
	Axis:=yAxis , 
	Enable:=PLCAxisEnable , 
	Enable_Positive:= TRUE, 
	Enable_Negative:=TRUE , 
	Override:=100 , 
	BufferMode:= , 
	Options:= , 
	Status=> , 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );
fbpower3(
	Axis:=RotationAxis , 
	Enable:=PLCAxisEnable AND NOT RotateManual, 
	Enable_Positive:= TRUE, 
	Enable_Negative:=TRUE , 
	Override:=100 , 
	BufferMode:= , 
	Options:= , 
	Status=> , 
	Busy=> , 
	Active=> , 
	Error=> , 
	ErrorID=> );
	
IF 	xaxis.Status.ControlLoopClosed AND arrAxis_REF[3].Status.ControlLoopClosed THEN
	fbGearin(
	Master:=xaxis , 
	Slave:=arrAxis_REF[3] , 
	Execute:=TRUE , 
	RatioNumerator:=1 , 
	RatioDenominator:= 1, 
	Acceleration:= , 
	Deceleration:= , 
	Jerk:= , 
	BufferMode:= , 
	Options:= , 
	InGear=> , 
	Busy=> , 
	Active=> , 
	CommandAborted=> , 
	Error=> , 
	ErrorID=> );
END_IF

IF NOT PLCAxisEnable OR NOT  Enable THEN
	
	istate:=0;
END_IF
	
rtEnable(CLK:=Enable AND arrAxis_REF[3].Status.Coupled , Q=> );
IF rtEnable.Q THEN
	istate:=-10;
END_IF

CASE istate OF 

	-10:
	fbExtSetPointGenDisable(Axis:=x , Execute:=FALSE  );
	fbExtSetPointGenDisable2(Axis:=y , Execute:=FALSE  );
	fbxtSetPointGenEnable(Axis:=x , Execute:=FALSE  );
	fbxtSetPointGenEnable2(Axis:=y , Execute:=FALSE  );
	fbMoveAbs1(Axis:=X , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
	fbMoveAbs2(Axis:=y , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
	fbSetPos(Axis:=XVirtual , Execute:=FALSE , Position:=X.NcToPlc.SetPos  );
	fbSetPos2(Axis:=yVirtual , Execute:=FALSE , Position:=y.NcToPlc.SetPos  );	
	fbSetPos3(Axis:= RotationAxis, Execute:=FALSE , Position:=0  );
	IF X.NcToPlc.SetVelo =0 AND  Y.NcToPlc.SetVelo =0 THEN
			istate:=-5;
	END_IF
	-5:
		fbSetPos(Axis:=XVirtual , Execute:=TRUE , Position:=X.NcToPlc.SetPos  );
		fbSetPos2(Axis:=yVirtual , Execute:=TRUE , Position:=y.NcToPlc.SetPos  );	
			fbSetPos3(Axis:=RotationAxis , Execute:=TRUE , Position:=0 );		
		IF ABS(XVirtual.NcToPlc.SetPos - X.NcToPlc.SetPos)<0.001 AND  ABS(yVirtual.NcToPlc.SetPos - y.NcToPlc.SetPos)<0.001 
			AND ABS(RotationAxis.NcToPlc.SetPos-0)<0.05 THEN
			istate :=10;
		END_IF

	10:
	 IF ABS(Xpos - x.NcToPlc.SetPos)<0.01 AND  ABS(Ypos - y.NcToPlc.SetPos)<0.01 THEN
		 istate:=20;
	 END_IF
	(*
	fbMoveAbs1(Axis:=X , Execute:=TRUE , Position:=Xpos , Velocity:=SetVelo );
	fbMoveAbs2(Axis:=y , Execute:=TRUE , Position:=ypos, Velocity:=SetVelo );	
	IF fbMoveAbs1.Done AND fbMoveAbs2.Done THEN
		istate:=20;
		fbMoveAbs1(Axis:=X , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
		fbMoveAbs2(Axis:=y , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
	END_IF
	*)
	20:
		fbxtSetPointGenEnable(Axis:=X , Execute:=TRUE  );
		fbxtSetPointGenEnable2(Axis:=Y , Execute:=TRUE  );
		IF  fbxtSetPointGenEnable.Enabled AND fbxtSetPointGenEnable2.Enabled THEN
			istate:=100;
		END_IF
	100:
	
IF Xpos > _Xpos THEN
	Direction:=1;
ELSIF  Xpos < _Xpos THEN
	Direction:=-1;
ELSE
	Direction:=0;
END_IF
IF Ypos > _Ypos THEN
	Direction2:=1;
ELSIF  Ypos < _Ypos THEN
	Direction2:=-1;
ELSE
	Direction2:=0;
END_IF
			fbxtSetPointGenEnable(Axis:=X , Execute:=TRUE  );
		fbxtSetPointGenEnable2(Axis:=Y , Execute:=TRUE  );
		MC_ExtSetPointGenFeed(
	Xpos,//rExGenPostion , (*位置*)
	0,// Motion[1].AXIS.NcToPlc.SetVelo,//rspeedgen,//Motion[1].AXIS.NcToPlc.SetVelo, (*速度*)
	0,//Motion[1].AXIS.NcToPlc.SetAcc, (*加速度*)
    1, (*方向*)
	X );
	MC_ExtSetPointGenFeed(
	Ypos,//rExGenPostion , (*位置*)
	0,// Motion[1].AXIS.NcToPlc.SetVelo,//rspeedgen,//Motion[1].AXIS.NcToPlc.SetVelo, (*速度*)
	0,//Motion[1].AXIS.NcToPlc.SetAcc, (*加速度*)
    1, (*方向*)
	Y );
	_Xpos:=Xpos;
	_Ypos:=Ypos;		
END_CASE
// NOT Enable OR NOT PLCAxisEnable AND X.NcToPlc.SetVelo =0
 //NOT Enable OR NOT PLCAxisEnable AND  Y.NcToPlc.SetVelo =0 
	fbExtSetPointGenDisable(Axis:=x , Execute:=NOT Enable OR NOT PLCAxisEnable AND X.NcToPlc.SetVelo =0  );
	fbExtSetPointGenDisable2(Axis:=y , Execute:=NOT Enable OR NOT PLCAxisEnable AND  Y.NcToPlc.SetVelo =0   );
		
fbReset1(
	Axis:=x , 
	Execute:=PLCReset AND x.Status.Error , 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );
fbReset2(
	Axis:=y , 
	Execute:=PLCReset AND y.Status.Error , 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );
fbReset3(
	Axis:=RotationAxis , 
	Execute:=PLCReset AND RotationAxis.Status.Error , 
	Done=> , 
	Busy=> , 
	Error=> , 
	ErrorID=> );
bAxisError:= arrAxis_REF[1].Status.Error OR arrAxis_REF[2].Status.Error  OR arrAxis_REF[3].Status.Error 
			OR x.Status.Error OR y.Status.Error OR RotationAxis.Status.Error;
bAllCoupled:=bAllHomed AND fbxtSetPointGenEnable.Enabled AND fbxtSetPointGenEnable2.Enabled 
 AND arrAxis_REF[3].Status.ControlLoopClosed AND NOT bAxisError; ]]></ST>
    </Implementation>
    <Action Name="CalcPos" Id="{ede94f25-0049-4813-8d68-6c067bb15149}">
      <Implementation>
        <ST><![CDATA[XposVirtual:=XVirtual.NcToPlc.ActPos-XWPS_OFFSET;
YposVirtual:=YVirtual.NcToPlc.ActPos-YWPS_OFFSET;

Radius:=SQRT(XposVirtual*XposVirtual+YposVirtual*YposVirtual);
IF XposVirtual>0 AND YposVirtual>0  THEN
	AngleOriginal:=ATAN(YposVirtual/XposVirtual);
ELSIF  XposVirtual< 0 AND YposVirtual>0 THEN
	AngleOriginal:=pai+ATAN(YposVirtual/XposVirtual);
ELSIF  XposVirtual> 0 AND YposVirtual<0 THEN
	AngleOriginal:=ATAN(YposVirtual/XposVirtual);
ELSIF  XposVirtual<0 AND YposVirtual<0 THEN
	AngleOriginal:=-1*pai+ATAN(YposVirtual/XposVirtual);
ELSIF  YposVirtual>0 AND XposVirtual=0 THEN
	AngleOriginal:=0.5*pai;
ELSIF  YposVirtual<0 AND XposVirtual=0 THEN
	AngleOriginal:=-0.5*pai;
ELSIF  YposVirtual=0 AND XposVirtual>0 THEN
	AngleOriginal:=0;
ELSIF  YposVirtual=0 AND XposVirtual<0 THEN
	AngleOriginal:=pai;
ELSIF  YposVirtual=0 AND XposVirtual=0 THEN
	AngleOriginal:=0;
	
END_IF
AngelDelta:=RotationAxis.NcToPlc.ModuloActPos*pai/180;
Xpos:=Radius*COS(AngleOriginal+AngelDelta);
Ypos:=1*Radius*SIN(AngleOriginal+AngelDelta);]]></ST>
      </Implementation>
    </Action>
    <Action Name="Home" Id="{656c62bc-dc48-43bd-b5f1-16b7865386a7}">
      <Implementation>
        <ST><![CDATA[Tc2_System.MEMCPY(ADR(arrNetId),ADR(AmsNetId),10);
sNetId:='';
FOR I:=1 TO 6 DO 
	sNetId:=Tc2_Standard.CONCAT(sNetId,BYTE_TO_STRING(arrNetId[I])  );
	IF i<6 THEN
	sNetId:=Tc2_Standard.CONCAT(sNetId,'.'  );
	END_IF

END_FOR
fbHomeX(
	AXIS:=x, 
	PowerOn := PLCAxisEnable , 
	FB_Enable:=bHomecmd , 
	FB_sNetId:=sNetId , 
	FB_nSlaveAddr:=1001 , 
	FB_home_mode:= 34, 
	FB_home_highspeed:=50000 , 
	FB_home_lowspeed:=400000 , 
	FB_Done=> , 
	fb_home_active=> , 
	Status=> );
fbHomeY(
	AXIS:=y, 
	PowerOn := PLCAxisEnable , 
	FB_Enable:=bHomecmd , 
	FB_sNetId:=sNetId , 
	FB_nSlaveAddr:= 1002, 
	FB_home_mode:=34 , 
	FB_home_highspeed:= 5000, 
	FB_home_lowspeed:= 30000, 
	FB_Done=> , 
	fb_home_active=> , 
	Status=> );
	
	fbHomer(
	AXIS:=RotationAxis, 
	PowerOn := PLCAxisEnable , 
	FB_Enable:=bHomecmd , 
	FB_sNetId:=sNetId , 
	FB_nSlaveAddr:= 1003, 
	FB_home_mode:=34 , 
	FB_home_highspeed:= 100000, 
	FB_home_lowspeed:= 100000, 
	FB_Done=> , 
	fb_home_active=> , 
	Status=> );
	_bAllHomed:=x.Status.Homed AND y.Status.Homed AND RotationAxis.Status.Homed;
rthome(CLK:=_bAllHomed , Q=> );
rtCOEHOME(CLK:=bHomecmd , Q=> );
IF rtCOEHOME.Q THEN
		homestate:=0;
END_IF
IF rthome.Q THEN
	homestate:=10;
	bAllHomed:=FALSE;
	fbMoveAbs1(Axis:=X , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
	fbMoveAbs2(Axis:=y , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
	fbMoveAbs3(Axis:=RotationAxis , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
	fbSetPos(Axis:=X , Execute:=FALSE , Position:=0  );
	fbSetPos2(Axis:=y , Execute:=FALSE , Position:=0 );	
	fbSetPos3(Axis:=RotationAxis , Execute:=FALSE , Position:=0 );
END_IF	
	CASE homestate OF
	10:
	fbMoveAbs1(Axis:=X , Execute:=TRUE , Position:=XHomeOffSet+XWPS_OFFSET , Velocity:=SetVelo );
	fbMoveAbs2(Axis:=y , Execute:=TRUE , Position:=yHomeOffSet+YWPS_OFFSET, Velocity:=SetVelo );
	fbMoveAbs3(Axis:=RotationAxis , Execute:=TRUE , Position:=rHomeOffSet , Velocity:=SetVelo );
	IF fbMoveAbs1.Done AND fbMoveAbs2.Done AND fbMoveAbs3.Done THEN
		homestate:=20;
	END_IF
	20:
		fbSetPos(Axis:=X , Execute:=TRUE , Position:=0  );
	fbSetPos2(Axis:=y , Execute:=TRUE , Position:=0 );	
	fbSetPos3(Axis:=RotationAxis , Execute:=TRUE , Position:=0 );
	IF fbSetPos.Done AND fbSetPos2.Done AND fbSetPos3.Done THEN
		bAllHomed:=TRUE;
		homestate:=0;
		fbMoveAbs1(Axis:=X , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
		fbMoveAbs2(Axis:=y , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
		fbMoveAbs3(Axis:=RotationAxis , Execute:=FALSE , Position:=0 , Velocity:=SetVelo );
		fbSetPos(Axis:=X , Execute:=FALSE , Position:=0  );
		fbSetPos2(Axis:=y , Execute:=FALSE , Position:=0 );	
		fbSetPos3(Axis:=RotationAxis , Execute:=FALSE , Position:=0 );
	END_IF
	
	END_CASE]]></ST>
      </Implementation>
    </Action>
    <LineIds Name="PRG_NciOnRotation">
      <LineId Id="3" Count="0" />
      <LineId Id="194" Count="0" />
      <LineId Id="4" Count="1" />
      <LineId Id="195" Count="0" />
      <LineId Id="6" Count="71" />
      <LineId Id="313" Count="0" />
      <LineId Id="78" Count="74" />
      <LineId Id="219" Count="0" />
      <LineId Id="153" Count="1" />
      <LineId Id="220" Count="0" />
      <LineId Id="199" Count="5" />
      <LineId Id="2" Count="0" />
      <LineId Id="206" Count="5" />
      <LineId Id="205" Count="0" />
      <LineId Id="213" Count="5" />
      <LineId Id="212" Count="0" />
      <LineId Id="266" Count="1" />
      <LineId Id="264" Count="1" />
    </LineIds>
    <LineIds Name="PRG_NciOnRotation.CalcPos">
      <LineId Id="2" Count="25" />
      <LineId Id="1" Count="0" />
    </LineIds>
    <LineIds Name="PRG_NciOnRotation.Home">
      <LineId Id="26" Count="7" />
      <LineId Id="25" Count="0" />
      <LineId Id="2" Count="1" />
      <LineId Id="24" Count="0" />
      <LineId Id="4" Count="7" />
      <LineId Id="1" Count="0" />
      <LineId Id="13" Count="1" />
      <LineId Id="23" Count="0" />
      <LineId Id="15" Count="7" />
      <LineId Id="12" Count="0" />
      <LineId Id="35" Count="0" />
      <LineId Id="37" Count="10" />
      <LineId Id="36" Count="0" />
      <LineId Id="34" Count="0" />
      <LineId Id="48" Count="0" />
      <LineId Id="84" Count="3" />
      <LineId Id="52" Count="0" />
      <LineId Id="54" Count="0" />
      <LineId Id="67" Count="0" />
      <LineId Id="56" Count="2" />
      <LineId Id="69" Count="1" />
      <LineId Id="68" Count="0" />
      <LineId Id="53" Count="0" />
      <LineId Id="49" Count="1" />
      <LineId Id="63" Count="1" />
      <LineId Id="62" Count="0" />
      <LineId Id="59" Count="0" />
      <LineId Id="65" Count="1" />
      <LineId Id="71" Count="0" />
      <LineId Id="73" Count="1" />
      <LineId Id="72" Count="0" />
      <LineId Id="75" Count="0" />
      <LineId Id="60" Count="0" />
      <LineId Id="77" Count="5" />
      <LineId Id="76" Count="0" />
      <LineId Id="83" Count="0" />
      <LineId Id="61" Count="0" />
      <LineId Id="51" Count="0" />
    </LineIds>
  </POU>
</TcPlcObject>