| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537 | <?xml version="1.0" encoding="utf-8"?><TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.12">  <POU Name="AxisFunction" Id="{23fbbc65-50c0-4a15-b3af-7ce3a033266f}" SpecialFunc="None">    <Declaration><![CDATA[FUNCTION_BLOCK AxisFunctionVAR_INPUT	AxisControl				         :DUT_AxisControl; //命令的触发END_VAR                              VAR_IN_OUT RETAIN                    	AxisData				         :DUT_AxisData;  //位置/速度数据END_VAR                              VAR_OUTPUT                           	AxisState				         :DUT_AxisState; //轴反馈数据状态END_VAR                              VAR 	Axis					         :AXIS_REF;//定义轴	Axis_Power				         :MC_Power;	Axis_Reset				         :MC_Reset;	Axis_Jog				         :MC_Jog;	Axis_Home				         :MC_Home;	Axis_MoveRelative		         :MC_MoveRelative;	Axis_MoveVelocity		         :MC_MoveVelocity;	Axis_MoveAbsolute		         :MC_MoveAbsolute;	Axis_TorqueControl		         :MC_TorqueControl;	Axis_Stop				         :MC_Stop;	AxisActPos				         :MC_ReadActualPosition;	AxisActVel				         :MC_ReadActualVelocity;	AxisError				         :MC_ReadAxisError;    T 								 :ARRAY[0..31]OF TON;  	P								 :ARRAY[0..31]OF BOOL;	RisingEdge						 :ARRAY[0..31]OF R_TRIG;	MCWritePosBiasPersistent		 :MC_WriteParameterPersistent;	Istep: INT;	tPos: LREAL;	AxisDisabled: BOOL;		hTorqueControlEx: BOOL;		hStopEx: BOOL;	hMoveRelativeEx: BOOL;		Axis_FinishHoming: MC_FinishHoming;	Axis_StepReferencePulse: MC_StepReferencePulse;	Axis_StepAbsoluteSwitch: MC_StepAbsoluteSwitch;	StepAbsoluteSwitch_Ref_Signal			:MC_Ref_Signal_Ref;	mcHomingParameter					:MC_HomingParameter;	mcPositiveDirection: MC_Home_Direction;	MCSwitchMode: MC_Switch_Mode;END_VAR                              ]]></Declaration>    <Implementation>      <ST>< OR 			 (NOT AxisControl.TPMode AND AxisControl.RobotToCmdHome) AND 			  NOT Axis.Status.DriveDeviceError AND 		      Axis.Status.ControlLoopClosed AND Axis.Status.NotMoving,PT:=T#3S);	RisingEdge[0](CLk:=T[0].Q);	IF 	 RisingEdge[0].Q THEN		IF AxisControl.HomeType = 1 THEN			Istep			:= 100;			END_IF;		IF  AxisControl.HomeType= 2 THEN			Istep			:= 200;		END_IF	END_IF			//=============================04.Axis TroqueControl======================	RisingEdge[1](CLk:=(AxisControl.TPMode AND AxisControl.PcToCmdTroqueControl)OR 				       (NOT AxisControl.TPMode AND AxisControl.RobotToCmdTroqueControl)OR  					   (0<Istep AND Istep<900 AND hTorqueControlEx)				   AND  				       NOT Axis.Status.DriveDeviceError AND Axis.Status.ControlLoopClosed                  );	Axis_TorqueControl.Execute	:= RisingEdge[1].Q;		//=============================05.Axis Veliocity==========================	Axis_MoveVelocity.Velocity:=AxisData.vel[AxisData.No];	RisingEdge[2](CLk:=(AxisControl.TPMode AND AxisControl.PcToCmdVeliocity)OR 		         (NOT AxisControl.TPMode AND AxisControl.RobotToCmdVeliocity) AND		          NOT Axis.Status.DriveDeviceError AND 		          Axis.Status.ControlLoopClosed AND Axis.Status.NotMoving);	Axis_MoveVelocity.Execute	:= RisingEdge[2].Q;		//=============================06.Axis MoveAbsolute=======================	Axis_MoveAbsolute.Velocity:=AxisData.vel[AxisData.No];	Axis_MoveAbsolute.Position:=AxisData.Pos[AxisData.No]+AxisData.PositionBias;	RisingEdge[3](CLk:=((AxisControl.TPMode AND AxisControl.PcToCmdMoveAbsolute)OR 				 (NOT AxisControl.TPMode AND AxisControl.RobotToCmdMoveAbsolute )) AND				  NOT Axis.Status.DriveDeviceError AND 		          Axis.Status.ControlLoopClosed AND Axis.Status.NotMoving);	Axis_MoveAbsolute.Execute	:= RisingEdge[3].Q;		//=============================07.Axis MoveRelative=======================		RisingEdge[4](CLk:=(AxisControl.TPMode AND AxisControl.PcToCmdMoveRelative)OR 				        (NOT AxisControl.TPMode AND AxisControl.RobotToCmdMoveRelative )OR				        (0<Istep AND Istep<900 AND hMoveRelativeEx )					AND NOT Axis.Status.DriveDeviceError AND Axis.Status.ControlLoopClosed);	Axis_MoveRelative.Execute	:= RisingEdge[4].Q;			//=============================08.Axis Stop===============================	Axis_Stop.Execute:= (AxisControl.TPMode AND AxisControl.PcToCmdStop)OR							(NOT AxisControl.TPMode AND AxisControl.RobotToCmdStop)OR							Axis.Status.Error OR 							Axis.Status.DriveDeviceError OR hStopEx;																						//=============================09.Axis Reset==============================	Axis_Reset.Execute:= (AxisControl.TPMode AND AxisControl.PcToCmdReset)OR 						 (NOT AxisControl.TPMode AND AxisControl.RobotToCmdReset);		//=============================10.Axis Statue=============================	AxisState.ErrorID:=AxisError.AxisErrorID;	AxisState.Enabled:=Axis.Status.ControlLoopClosed;	AxisState.Busy:=Axis.Status.Moving;	AxisState.Done:=Axis.Status.StandStill AND Axis.Status.ControlLoopClosed AND	                NOT Axis.Status.DriveDeviceError AND NOT Axis.Status.Error AND					Axis.Status.NotMoving;	AxisState.Error:=NOT Axis.Status.DriveDeviceError AND NOT Axis.Status.Error AND					 AxisError.Error;		//=============================12.Axis DataSave===========================	T[1](IN:=AxisControl.DataSave AND NOT AxisControl.TPMode AND Axis.Status.ControlLoopClosed 								,PT:=T#3S);	RisingEdge[5](CLk:=T[1].Q);	IF T[1].Q THEN	AxisData.Pos[AxisData.No]:=AxisState.Pos;	AxisData.Vel[AxisData.No]:=AxisState.vel;	END_IF		AxisActPos.Enable:=1;		AxisActVel.Enable:=1;		AxisError.Enable:=1;		//=============================13.Home Mode===================================	T[10](IN:=T[11].Q AND T[13].Q AND T[14].Q AND AxisState.Enabled,PT:=T#10S);	T[11](IN:=ABS( AxisState.Pos-tPos)<=0.001,PT:=T#200MS);	IF T[12].Q THEN	   tPos:=AxisState.Pos;	END_IF	T[12](IN:=NOT p[12],PT:=T#200MS);	p[12]:=T[12].Q;	T[13](IN:=ABS(AxisState.vel)<=0.1,PT:=T#200MS);	T[14](IN:=ABS(AxisData.ActTorque-Axis_TorqueControl.Torque*10)<=50,PT:=T#200MS);	T[15](IN:=Axis.Status.Moving,PT:=T#500MS);		T[16](IN:=Axis.Status.NotMoving,PT:=T#500MS);				CASE Istep OF		100:		Axis_FinishHoming.Execute:=FALSE;		Axis_MoveRelative.Velocity:=10;		Axis_MoveRelative.Distance:=30.0;		hMoveRelativeEx:=TRUE;		IF AxisState.Enabled AND Axis_MoveRelative.Done		THEN			hMoveRelativeEx:=FALSE;			Istep:=101;		END_IF		101:		Axis_StepAbsoluteSwitch.Execute:=TRUE;		IF Axis_StepAbsoluteSwitch.Done THEN			Istep:=103;		END_IF				102:		Axis_StepReferencePulse.Execute:=TRUE;		IF Axis_StepReferencePulse.Done THEN			Istep:=103;		END_IF				103:		Axis_StepAbsoluteSwitch.Execute:=FALSE;		Axis_FinishHoming.Execute:=TRUE;		IF Axis_FinishHoming.Done THEN			Axis_FinishHoming.Execute:=FALSE;			Istep:=999;		END_IF				200:		Axis_TorqueControl.Torque:=AxisData.hTorque;		Axis_TorqueControl.TorqueRamp:=AxisData.hTorqueRamp;		Axis_TorqueControl.VelocityLimitHigh:=AxisData.hTorqueVelLimitHigh;		Axis_TorqueControl.VelocityLimitLow:=AxisData.hTorqueVelVelocityLimitLow;		hTorqueControlEx:=TRUE;		IF T[15].Q THEN			Istep:=201;		END_IF		201:		IF T[16].Q AND T[10].Q		THEN		hTorqueControlEx:=FALSE;		hStopEx:= TRUE;			Istep:=202;		END_IF		202:		hStopEx:= FALSE;		Axis_MoveRelative.Velocity:=10;		Axis_MoveRelative.Distance:=-10.0;		IF Axis_Stop.Done  THEN			Istep:=203;		END_IF		203:		hMoveRelativeEx:=TRUE;		IF AxisState.Enabled AND Axis_MoveRelative.Done		THEN			Istep:=204;		END_IF		204:		hMoveRelativeEx:=FALSE;		IF AxisState.Enabled AND T[16].Q THEN			Istep:=205;		END_IF		205:		AxisData.PositionBias:=AxisActPos.Position;		IF AxisData.PositionBias=AxisActPos.Position THEN			Istep:=206;		END_IF		//Istep:=206;		206:		IF AxisState.Enabled THEN			Istep:=999;		END_IF	END_CASE	IF Istep=0 OR Istep=999 OR (Istep<0 AND Istep >999 )		THEN		hMoveRelativeEx:=FALSE;		hTorqueControlEx:=FALSE;		hStopEx:= FALSE;		Axis_StepAbsoluteSwitch.Execute:=FALSE;		Axis_StepReferencePulse.Execute:=FALSE;		Axis_FinishHoming.Execute:=FALSE;	END_IF	//=============================14.Axis Execute============================	AxisState.vel:=AxisActVel.ActualVelocity;	AxisState.Pos:=AxisActPos.Position-AxisData.PositionBias;	//AxisState.Pos:=AxisActPos.Position;		Axis.ReadStatus();		Axis_Power(Axis:=Axis,Enable:= , Enable_Positive:=TRUE,Enable_Negative	:=TRUE,Override:=100.0, Status=>,);		Axis_Jog(Axis:=Axis,JogForward :=, JogBackwards	:=,Velocity	:=AxisData.Vel[31],Acceleration	:=,Deceleration	:=,Jerk	:=,Mode	:=MC_JOGMODE_CONTINOUS,Done=>,Busy	=>,Active=>,CommandAborted=>,Error=>,ErrorID=>);		Axis_Home(Axis:=Axis, Execute:= ,Position:=0-AxisState.Pos,HomingMode:= ,BufferMode:= , Options:= ,bCalibrationCam:= ,Done=> , Busy=>, Active=> ,CommandAborted=> ,Error=> , ErrorID=> );		Axis_TorqueControl(Axis:=Axis,Execute:= , ContinuousUpdate:= , Relative:= , Torque:= , TorqueRamp:= , VelocityLimitHigh:= , VelocityLimitLow:= , BufferMode:= , Options:= , InTorque=> , Busy=> ,Active=> , CommandAborted=> , Error=> , ErrorId=> );		Axis_MoveVelocity(Axis:=Axis,Execute:=,Velocity	:=,Acceleration	:=,Deceleration	:=,Jerk:=,Direction:=,BufferMode:=,Options:=,InVelocity	=>,Busy	=>,Active	=>,CommandAborted=>,Error=>, ErrorID=>);		Axis_MoveRelative(Axis:=Axis ,Execute:=, Distance:=, Velocity:=, Acceleration:=, Deceleration:=, Jerk:=, BufferMode:=, Options:=, Done=>,Busy=>,Active=>,CommandAborted=>,Error=>,ErrorID=>);		Axis_Stop( Axis:=Axis, Execute:= , Deceleration:= , Jerk:= ,Options:= , Done=> ,Busy=> ,Active=> ,CommandAborted=> ,Error=> , ErrorID=> );		Axis_Reset(Axis:=Axis, Execute:=,Done=>,Busy=>,Error=>,ErrorID=>);		Axis_MoveAbsolute( Axis:=Axis,Execute:=,Position:=,Velocity:=,Acceleration:=,Deceleration:=,Jerk:=,BufferMode:=,Options:=,Done=>,Busy=>,Active=>,CommandAborted=>,Error	=>, ErrorID=>);		AxisActPos(Axis:=Axis ,Enable:= , Valid=> , Busy=> , Error=> , ErrorID=> , Position=> );		AxisActVel(Axis:=Axis , Enable:= , Valid=> , Busy=> , Error=> , ErrorID=> , ActualVelocity=>AxisState.vel );		AxisError(Axis:= Axis,Enable:= , Valid=> ,Busy=> ,Error=> ,ErrorID=> ,AxisErrorID=> );		MCWritePosBiasPersistent(Axis:=Axis, Execute:= , ParameterNumber:=MC_AxisParameter.AxisEncoderOffset,Value:=AxisData.PositionBias,Done=> , Busy=> , Error=> , ErrorID=> );		Axis_StepAbsoluteSwitch.ReferenceSignal.Level:=AxisControl.HomeSW;	Axis_StepAbsoluteSwitch.Options.DisableDriveAccess:=TRUE;	Axis_StepReferencePulse.Options.DisableDriveAccess:=TRUE;	Axis_FinishHoming.Options.DisableDriveAccess:=TRUE;		Axis_StepAbsoluteSwitch(	Axis:=Axis , 	Parameter:=mcHomingParameter , 	Execute:= , 	Direction:=mcPositiveDirection , 	SwitchMode:= MCSwitchMode, 	ReferenceSignal:= , 	Velocity:=50 , 	Acceleration:= , 	Deceleration:= , 	Jerk:= , 	SetPosition:= , 	TimeLimit:= , 	DistanceLimit:= , 	TorqueLimit:=100 , 	PositiveLimitSwitch:= , 	NegativeLimitSwitch:= , 	BufferMode:= , 	Options:= , 	Done=> , 	Busy=> , 	Active=> , 	CommandAborted=> , 	Error=> , 	ErrorID=> );		  	Axis_StepReferencePulse(	Axis:=Axis , 	Parameter:=mcHomingParameter , 	Execute:= , 	Direction:=mcPositiveDirection , 	ReferenceSignal:= , 	Velocity:=100 , 	Acceleration:= , 	Deceleration:= , 	Jerk:= , 	SetPosition:= , 	TimeLimit:= , 	DistanceLimit:= , 	TorqueLimit:=100 , 	BufferMode:= , 	Options:= , 	Done=> , 	Busy=> , 	Active=> , 	CommandAborted=> , 	Error=> , 	ErrorID=> );							Axis_FinishHoming(	Axis:=Axis , 	Parameter:=mcHomingParameter , 	Execute:= , 	Distance:=0-AxisActPos.Position , 	Velocity:=100 , 	Acceleration:= , 	Deceleration:= , 	Jerk:= , 	BufferMode:= , 	Options:= , 	Done=> , 	Busy=> , 	CommandAborted=> , 	Error=> , 	ErrorID=> );									]]></ST>    </Implementation>    <LineIds Name="AxisFunction">      <LineId Id="4143" Count="5" />      <LineId Id="4366" Count="0" />      <LineId Id="4365" Count="0" />      <LineId Id="4150" Count="0" />      <LineId Id="4152" Count="4" />      <LineId Id="4158" Count="7" />      <LineId Id="4167" Count="1" />      <LineId Id="4170" Count="9" />      <LineId Id="4183" Count="3" />      <LineId Id="4371" Count="0" />      <LineId Id="4647" Count="1" />      <LineId Id="4718" Count="0" />      <LineId Id="4717" Count="0" />      <LineId Id="4716" Count="0" />      <LineId Id="4711" Count="0" />      <LineId Id="4196" Count="0" />      <LineId Id="4719" Count="0" />      <LineId Id="4197" Count="0" />      <LineId Id="4199" Count="7" />      <LineId Id="4208" Count="8" />      <LineId Id="4652" Count="0" />      <LineId Id="4220" Count="1" />      <LineId Id="4729" Count="0" />      <LineId Id="4727" Count="0" />      <LineId Id="4655" Count="0" />      <LineId Id="4224" Count="1" />      <LineId Id="4662" Count="0" />      <LineId Id="4708" Count="2" />      <LineId Id="4725" Count="0" />      <LineId Id="4722" Count="2" />      <LineId Id="4233" Count="0" />      <LineId Id="4235" Count="3" />      <LineId Id="4240" Count="8" />      <LineId Id="4251" Count="0" />      <LineId Id="4253" Count="8" />      <LineId Id="4263" Count="0" />      <LineId Id="4299" Count="1" />      <LineId Id="4634" Count="0" />      <LineId Id="4768" Count="0" />      <LineId Id="4767" Count="0" />      <LineId Id="4763" Count="3" />      <LineId Id="4758" Count="2" />      <LineId Id="4769" Count="0" />      <LineId Id="4761" Count="0" />      <LineId Id="4757" Count="0" />      <LineId Id="4641" Count="1" />      <LineId Id="4638" Count="0" />      <LineId Id="4301" Count="0" />      <LineId Id="5019" Count="0" />      <LineId Id="5037" Count="0" />      <LineId Id="5021" Count="1" />      <LineId Id="5025" Count="0" />      <LineId Id="5032" Count="1" />      <LineId Id="5035" Count="0" />      <LineId Id="5034" Count="0" />      <LineId Id="5018" Count="0" />      <LineId Id="4302" Count="0" />      <LineId Id="4305" Count="0" />      <LineId Id="4349" Count="0" />      <LineId Id="4945" Count="1" />      <LineId Id="4940" Count="1" />      <LineId Id="4947" Count="2" />      <LineId Id="4942" Count="1" />      <LineId Id="4950" Count="0" />      <LineId Id="5036" Count="0" />      <LineId Id="4954" Count="1" />      <LineId Id="4959" Count="0" />      <LineId Id="4956" Count="0" />      <LineId Id="4952" Count="0" />      <LineId Id="4944" Count="0" />      <LineId Id="4306" Count="0" />      <LineId Id="4308" Count="3" />      <LineId Id="4720" Count="0" />      <LineId Id="4313" Count="2" />      <LineId Id="5093" Count="5" />      <LineId Id="4319" Count="0" />      <LineId Id="4665" Count="0" />      <LineId Id="4745" Count="0" />      <LineId Id="4753" Count="1" />      <LineId Id="4667" Count="2" />      <LineId Id="4323" Count="0" />      <LineId Id="4326" Count="1" />      <LineId Id="4329" Count="0" />      <LineId Id="4331" Count="0" />      <LineId Id="4330" Count="0" />      <LineId Id="4332" Count="0" />      <LineId Id="4771" Count="0" />      <LineId Id="4334" Count="0" />      <LineId Id="4336" Count="2" />      <LineId Id="4742" Count="2" />      <LineId Id="4342" Count="0" />      <LineId Id="4741" Count="0" />      <LineId Id="4343" Count="0" />      <LineId Id="4346" Count="2" />      <LineId Id="4303" Count="0" />      <LineId Id="4264" Count="0" />      <LineId Id="4732" Count="0" />      <LineId Id="4734" Count="2" />      <LineId Id="4961" Count="1" />      <LineId Id="4960" Count="0" />      <LineId Id="4733" Count="0" />      <LineId Id="4265" Count="0" />      <LineId Id="4738" Count="2" />      <LineId Id="4748" Count="0" />      <LineId Id="4297" Count="0" />      <LineId Id="4538" Count="0" />      <LineId Id="4389" Count="0" />      <LineId Id="4539" Count="0" />      <LineId Id="4396" Count="0" />      <LineId Id="4540" Count="0" />      <LineId Id="4411" Count="0" />      <LineId Id="4541" Count="0" />      <LineId Id="4424" Count="0" />      <LineId Id="4542" Count="0" />      <LineId Id="4440" Count="0" />      <LineId Id="4543" Count="0" />      <LineId Id="4455" Count="0" />      <LineId Id="4544" Count="0" />      <LineId Id="4470" Count="0" />      <LineId Id="4545" Count="0" />      <LineId Id="4484" Count="0" />      <LineId Id="4546" Count="0" />      <LineId Id="4490" Count="0" />      <LineId Id="4547" Count="0" />      <LineId Id="4505" Count="0" />      <LineId Id="4548" Count="0" />      <LineId Id="4516" Count="0" />      <LineId Id="4549" Count="0" />      <LineId Id="4523" Count="0" />      <LineId Id="4550" Count="0" />      <LineId Id="4530" Count="0" />      <LineId Id="4515" Count="0" />      <LineId Id="5010" Count="1" />      <LineId Id="5013" Count="1" />      <LineId Id="5012" Count="0" />      <LineId Id="4911" Count="23" />      <LineId Id="4277" Count="0" />      <LineId Id="4827" Count="1" />      <LineId Id="4831" Count="0" />      <LineId Id="4895" Count="0" />      <LineId Id="4894" Count="0" />      <LineId Id="4893" Count="0" />      <LineId Id="4892" Count="0" />      <LineId Id="4891" Count="0" />      <LineId Id="4890" Count="0" />      <LineId Id="4838" Count="1" />      <LineId Id="4877" Count="12" />      <LineId Id="4280" Count="0" />      <LineId Id="4354" Count="0" />      <LineId Id="4282" Count="3" />      <LineId Id="4896" Count="14" />      <LineId Id="4286" Count="8" />      <LineId Id="633" Count="0" />    </LineIds>  </POU></TcPlcObject>
 |