346 lines
9.9 KiB
XML
346 lines
9.9 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.15">
|
|
<POU Name="MAIN" Id="{56453c0e-bad0-4c10-8ff9-86e652affd20}" SpecialFunc="None">
|
|
<Declaration><![CDATA[PROGRAM MAIN
|
|
VAR
|
|
i : UINT:=0;
|
|
Nc_AxisN : ARRAY [0..1] OF AXIS_REF; // 2 axes
|
|
AxisState : ARRAY [0..1] OF UDINT;
|
|
|
|
|
|
fbPower1 : MC_Power;
|
|
fbPower4 : MC_Power;
|
|
|
|
//Left seperat to minimize the chance to hang in a loop
|
|
fbStop1 : MC_Stop;
|
|
fbStop4 : MC_Stop;
|
|
|
|
fbReset : ARRAY [0..1] OF MC_Reset;
|
|
fbGetState : Array [0..1] OF MC_ReadStatus;
|
|
|
|
fbMoveAbsolute : MC_MoveAbsolute;
|
|
fbGearIn : MC_GearIn;
|
|
fbGearOut : MC_Gearout;
|
|
|
|
//fbPower
|
|
enableAxis : BOOL;
|
|
override : LREAL := 100;
|
|
errNr1 : UDINT;
|
|
errNr2 : UDINT;
|
|
|
|
//fbGearIn
|
|
executeGearIn : BOOL;
|
|
executeGearout : BOOL;
|
|
gearedStatus : BOOL;
|
|
executeStop : BOOL;
|
|
executeReset : BOOL;
|
|
executeMove : BOOL;
|
|
|
|
//fbMove
|
|
position : LREAL; // ToDo: set to a reachable position
|
|
velocity : LREAL := 0.5; // ToDo: set velocity for move absolute
|
|
|
|
errOut : BOOL;
|
|
errorCollector : BOOL;
|
|
|
|
{attribute 'TcInitSymbol'}
|
|
bEndSwitchR AT%I* :BOOL;
|
|
bEndSwitchL AT%I* :BOOL;
|
|
bEndSwitchT AT%I* :BOOL;
|
|
bEndSwitchB AT%I* :BOOL;
|
|
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[//General Error feedback
|
|
IF errNr1 < errNr2 THEN
|
|
GVL.ErrOut := errNr2;
|
|
END_IF
|
|
IF errNr1 > errNr2 THEN
|
|
GVL.ErrOut := errNr1;
|
|
END_IF
|
|
IF errorCollector = TRUE THEN
|
|
GVL.ErrOut := 1;
|
|
errorCollector := FALSE;
|
|
END_IF
|
|
|
|
//Feedback of coupling state
|
|
IF Nc_AxisN[1].NcToPlc.CoupleState = 3 THEN
|
|
gearedStatus := TRUE;
|
|
ELSIF Nc_AxisN[1].NcToPlc.CoupleState = 0 THEN
|
|
gearedStatus := FALSE;
|
|
ELSE
|
|
gearedStatus := TRUE;
|
|
GVL.ErrOut := 1;
|
|
END_IF
|
|
|
|
|
|
|
|
executeMove := GVL.StartCmd;
|
|
|
|
|
|
enableAxis := GVL.EnableAxis;
|
|
executeGearIn := GVL.GerarInCmd;
|
|
executeGearout := GVL.GerarOutCmd;
|
|
executeStop := GVL.StopCmd;
|
|
executeReset := GVL.Reset;
|
|
GVL.GeraredStat := gearedStatus;
|
|
|
|
//Executete Reset from Python
|
|
IF executeReset = TRUE THEN
|
|
GVL.ErrOut := 0;
|
|
GVL.Done := FALSE;
|
|
GVL.EnableAxis := FALSE;
|
|
GVL.GerarInCmd := FALSE;
|
|
GVL.GerarOutCmd := FALSE;
|
|
GVL.StartCmd := FALSE;
|
|
GVL.StopCmd := FALSE;
|
|
GVL.Reset := FALSE;
|
|
END_IF
|
|
|
|
//for loop
|
|
fbPower1(
|
|
Axis := Nc_AxisN[0],
|
|
Enable := enableAxis,
|
|
Enable_Positive := bEndSwitchL,
|
|
Enable_Negative := bEndSwitchR,
|
|
Override := override,
|
|
BufferMode := ,
|
|
Options := ,
|
|
Status => ,
|
|
Busy => ,
|
|
Active => ,
|
|
Error => ,
|
|
ErrorID => errNr1 );
|
|
|
|
fbPower4(
|
|
Axis := Nc_AxisN[1],
|
|
Enable := enableAxis,
|
|
Enable_Positive := bEndSwitchR,
|
|
Enable_Negative := bEndSwitchL,
|
|
Override := override,
|
|
BufferMode := ,
|
|
Options := ,
|
|
Status => ,
|
|
Busy => ,
|
|
Active => ,
|
|
Error => ,
|
|
ErrorID => errNr2 );
|
|
|
|
fbStop1(
|
|
Axis := Nc_AxisN[0],
|
|
Execute := executeStop, // The command is executed with a positive edge.
|
|
Deceleration := , // If the value is <= 0, the deceleration
|
|
// parameterized with the last Move command is used.
|
|
Jerk := , // If the value is <= 0, the jerk parameterized
|
|
// with the last Move command is used.
|
|
Options := ,
|
|
Done => ,
|
|
Busy => ,
|
|
Active => ,
|
|
CommandAborted => ,
|
|
Error => ,
|
|
ErrorID => errNr1);
|
|
|
|
fbStop4(
|
|
Axis := Nc_AxisN[1],
|
|
Execute := executeStop, // The command is executed with a positive edge.
|
|
Deceleration := , // If the value is <= 0, the deceleration
|
|
// parameterized with the last Move command is used.
|
|
Jerk := , // If the value is <= 0, the jerk parameterized
|
|
// with the last Move command is used.
|
|
Options := ,
|
|
Done => ,
|
|
Busy => ,
|
|
Active => ,
|
|
CommandAborted => ,
|
|
Error => ,
|
|
ErrorID => errNr2);
|
|
|
|
|
|
FOR i := 0 TO 1 DO
|
|
AxisState[i] := Nc_AxisN[i].NcToPlc.CmdState;
|
|
fbReset[i](
|
|
Axis := Nc_AxisN[i],
|
|
Execute := executeReset, // The command is executed with a positive edge.
|
|
Done => ,
|
|
Busy => ,
|
|
Error => errorCollector ,
|
|
ErrorID => );
|
|
END_FOR
|
|
|
|
|
|
|
|
|
|
fbGearIn(
|
|
Master := Nc_AxisN[0],
|
|
Slave := Nc_AxisN[1],
|
|
Execute := executeGearIn,
|
|
RatioNumerator := 1,
|
|
RatioDenominator := 1,
|
|
Acceleration :=,
|
|
Deceleration :=,
|
|
Jerk :=,
|
|
BufferMode := MC_BufferMode.MC_Aborting,
|
|
Options =>,
|
|
InGear =>,
|
|
Busy =>,
|
|
Active =>,
|
|
CommandAborted =>,
|
|
Error => errorCollector,
|
|
ErrorID =>);
|
|
fbGearOut(
|
|
SLAVE := Nc_AxisN[1],
|
|
Execute := executeGearout,
|
|
Options :=,
|
|
Done =>,
|
|
Busy =>,
|
|
Error => errorCollector,
|
|
ErrorID =>);
|
|
|
|
|
|
FOR i:= 0 TO 1 DO
|
|
fbGetState[i](
|
|
Axis := Nc_AxisN[0],
|
|
Enable := TRUE,
|
|
Valid =>,
|
|
Busy =>,
|
|
Error =>,
|
|
ErrorId =>,
|
|
(* motion control statemachine states: *)
|
|
ErrorStop => GVL.Axis1ErrStoped,
|
|
Disabled => GVL.Axis1Enabled,
|
|
Stopping => GVL.Axis1Stoping,
|
|
StandStill => ,
|
|
DiscreteMotion =>GVL.Axis1moving,
|
|
ContinuousMotion => ,
|
|
SynchronizedMotion =>,
|
|
Homing =>,
|
|
(* additional status *)
|
|
ConstantVelocity =>,
|
|
Accelerating =>,
|
|
Decelerating =>,
|
|
(* status data structure *)
|
|
Status =>);
|
|
END_FOR
|
|
|
|
|
|
IF GVL.StartCmd = TRUE THEN
|
|
position := GVL.moveInterface.targetPos;
|
|
velocity := GVL.moveInterface.velocity;
|
|
//Validate input here
|
|
END_IF
|
|
fbMoveAbsolute(
|
|
Axis := Nc_AxisN[GVL.moveInterface.axisNr],
|
|
Execute := executeMove, // The command is executed with a positive edge.
|
|
Position := position,
|
|
Velocity := velocity,
|
|
Acceleration := , // If the value is 0, the standard acceleration from
|
|
// the axis configuration in the System Manager is used.
|
|
Deceleration := , // If the value is 0, the standard deceleration from
|
|
// the axis configuration in the System Manager is used.
|
|
Jerk := , // If the value is 0, the standard jerk from the axis
|
|
// configuration in the System Manager is applied.
|
|
BufferMode := MC_BufferMode.MC_Buffered,
|
|
Options := ,
|
|
Done => GVL.Done,
|
|
Busy => ,
|
|
Active => ,
|
|
CommandAborted => ,
|
|
Error => ,
|
|
ErrorId => errNr1);
|
|
|
|
|
|
|
|
//Reset Vars
|
|
IF bEndSwitchR = FALSE OR bEndSwitchL = FALSE OR bEndSwitchT = False OR bEndSwitchB = False THEN
|
|
executeStop := TRUE;
|
|
GVL.EnableAxis := FALSE;
|
|
enableAxis := FALSE;
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
executeGearIn := FALSE;
|
|
executeGearout := FALSE;
|
|
executeStop := FALSE;
|
|
executeMove := FALSE;
|
|
executeReset := FALSE;
|
|
]]></ST>
|
|
</Implementation>
|
|
<LineIds Name="MAIN">
|
|
<LineId Id="29" Count="0" />
|
|
<LineId Id="212" Count="0" />
|
|
<LineId Id="215" Count="0" />
|
|
<LineId Id="214" Count="0" />
|
|
<LineId Id="217" Count="1" />
|
|
<LineId Id="216" Count="0" />
|
|
<LineId Id="392" Count="1" />
|
|
<LineId Id="395" Count="0" />
|
|
<LineId Id="394" Count="0" />
|
|
<LineId Id="387" Count="0" />
|
|
<LineId Id="253" Count="2" />
|
|
<LineId Id="257" Count="4" />
|
|
<LineId Id="252" Count="0" />
|
|
<LineId Id="429" Count="0" />
|
|
<LineId Id="428" Count="0" />
|
|
<LineId Id="410" Count="0" />
|
|
<LineId Id="420" Count="0" />
|
|
<LineId Id="262" Count="0" />
|
|
<LineId Id="220" Count="0" />
|
|
<LineId Id="219" Count="0" />
|
|
<LineId Id="211" Count="0" />
|
|
<LineId Id="249" Count="0" />
|
|
<LineId Id="264" Count="0" />
|
|
<LineId Id="366" Count="0" />
|
|
<LineId Id="375" Count="0" />
|
|
<LineId Id="388" Count="0" />
|
|
<LineId Id="379" Count="0" />
|
|
<LineId Id="367" Count="0" />
|
|
<LineId Id="396" Count="0" />
|
|
<LineId Id="368" Count="0" />
|
|
<LineId Id="370" Count="4" />
|
|
<LineId Id="378" Count="0" />
|
|
<LineId Id="369" Count="0" />
|
|
<LineId Id="210" Count="0" />
|
|
<LineId Id="30" Count="42" />
|
|
<LineId Id="206" Count="0" />
|
|
<LineId Id="73" Count="13" />
|
|
<LineId Id="389" Count="1" />
|
|
<LineId Id="87" Count="0" />
|
|
<LineId Id="266" Count="0" />
|
|
<LineId Id="88" Count="7" />
|
|
<LineId Id="208" Count="0" />
|
|
<LineId Id="102" Count="1" />
|
|
<LineId Id="122" Count="17" />
|
|
<LineId Id="225" Count="0" />
|
|
<LineId Id="229" Count="1" />
|
|
<LineId Id="232" Count="4" />
|
|
<LineId Id="294" Count="2" />
|
|
<LineId Id="299" Count="21" />
|
|
<LineId Id="298" Count="0" />
|
|
<LineId Id="226" Count="0" />
|
|
<LineId Id="221" Count="0" />
|
|
<LineId Id="322" Count="1" />
|
|
<LineId Id="325" Count="0" />
|
|
<LineId Id="345" Count="0" />
|
|
<LineId Id="324" Count="0" />
|
|
<LineId Id="348" Count="17" />
|
|
<LineId Id="347" Count="0" />
|
|
<LineId Id="321" Count="0" />
|
|
<LineId Id="222" Count="2" />
|
|
<LineId Id="380" Count="0" />
|
|
<LineId Id="383" Count="2" />
|
|
<LineId Id="382" Count="0" />
|
|
<LineId Id="247" Count="0" />
|
|
<LineId Id="244" Count="0" />
|
|
<LineId Id="241" Count="0" />
|
|
<LineId Id="237" Count="0" />
|
|
<LineId Id="165" Count="0" />
|
|
<LineId Id="250" Count="1" />
|
|
<LineId Id="293" Count="0" />
|
|
<LineId Id="377" Count="0" />
|
|
<LineId Id="2" Count="0" />
|
|
</LineIds>
|
|
</POU>
|
|
</TcPlcObject> |