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 AxisFunction
- VAR_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>
|