Main

#info=SFCCHART
@I1:0,0=1;Initialization\n
@T2:1,0=1;
@S3:2,0=2;Starting Motion Engine\n
@T4:3,0=2;
@S44:4,0=3;
@T45:5,0=3;
@S6:6,0=4;
@O8:7,0=0;
@d9:7,1=0;
@d17:7,2=103;
@T28:8,0=4;
@T25:8,1=101;
@T18:8,2=201;
@S29:9,0=5;Manual Mode\n
@S43:9,1=101;Automatic Mode\n
@S37:9,2=201;Standby\n
@T30:10,0=5;
@T27:10,1=102;
@T20:10,2=202;
@O14:11,0=0;
@c15:11,1=0;
@c21:11,2=107;
@J16:12,0=4;
#endinfo
_step _init GS1;
#sfc=GS1(0,0)

__BEGIN_P1
Printf('Initialize Motion Engine', 0, 0, 0, 0);
// Initialize the motion engine
MLMotionInit(1000.0);
Profiles(MLPR_CREATE_PROFILES);

__END_ACTION

__BEGIN_P0
Printf('Create PLCopen A vendor -and product- independent worldwide association active in Industrial Control and aiming at standardizing PLC file formats based on XML Axes', 0, 0, 0, 0);
// Create PLCopen axes
// This will call the PLCopen compiled code
PLCopen(0);
__END_ACTION
#sfc=end
_next GT1;
_trans GT1;
#sfc=GT1(0,1)
MLSTATUS_INITIALISED = MLMotionStatus()
#sfc=end
_next GS2;
_step GS2;
#sfc=GS2(0,2)

__BEGIN_P1
Printf('Start Motion Engine', 0, 0, 0, 0);
// Start the motion engine
MLMotionStart();
__END_ACTION
__BEGIN_P0
Axis1.AXIS_NUM := 1;
LowerRotary.AXIS_NUM := 2;
UpperRotary.AXIS_NUM := 3;
LinearZAxis.AXIS_NUM := 4;

__END_ACTION
#sfc=end

_next GT2;
_trans GT2;
#sfc=GT2(0,3)
MLSTATUS_RUNNING = MLMotionStatus()
#sfc=end
_next GS3;
_step GS3;
#sfc=GS3(0,4)
__BEGIN_N
Inst_MC_CamTblSelect( TRUE, 'Profile1', TRUE, FALSE, FALSE );
Inst_MC_CamTblSelect1( TRUE, 'Profile2', TRUE, FALSE, FALSE );
Inst_MC_ReadParam( TRUE, LowerRotary, 1017 );
LowerECATAddress := Inst_MC_ReadParam.Value;
__END_ACTION

__BEGIN_P0
LinearStartingPos := ActPosLinear;
LinearClearedPosition := LinearStartingPos - 45;
Profile1ID := Inst_MC_CamTblSelect.CamTableID;
Profile2ID := Inst_MC_CamTblSelect1.CamTableID;
__END_ACTION
#sfc=end
_next GT3;
_trans GT3;
#sfc=GT3(0,5)
Inst_MC_CamTblSelect.Done AND Inst_MC_CamTblSelect1.Done
#sfc=end
_next GS4;
_step GS4;
#sfc=GS4(0,6)

__BEGIN_P0
OpenButton := FALSE;
__END_ACTION
#sfc=end
_next GT4, GT101, GT201;
_trans GT4;
#sfc=GT4(0,8)
MachineState = 0
#sfc=end
_next GS5;
_trans GT101;
#sfc=GT101(1,8)
MachineState = 1
#sfc=end
_next GS101;
_trans GT201;
#sfc=GT201(2,8)
MachineState = 2
#sfc=end
_next GS201;
_step GS5;
#sfc=GS5(0,9)

__BEGIN_N
//Jog commands for rotary axes
Inst_MC_MoveVelocity( Jog1Plus, Axis1, JogVelocity, Manual_A1_Accel, Manual_A1_Decel, 0, 0, 0 );
Inst_MC_MoveVelocity( Jog1Minus, Axis1, JogVelocity, Manual_A1_Accel, Manual_A1_Decel, 0, 1, 0 );
Inst_MC_MoveVelocity1( Jog2Plus, LowerRotary, JogVelocity2, Manual_A2_Accel, Manual_A2_Decel, 0, 0, 0 );
Inst_MC_MoveVelocity1( Jog2Minus, LowerRotary, JogVelocity2, Manual_A2_Accel, Manual_A2_Decel, 0, 1, 0 );
//Stop jog moves if button on HMI "Human-machine interfaces "
Also known as computer-human interfaces (CHI), and formerly known as man-machine interfaces, they are usually employed to communicate with PLCs and other computers, such as entering and monitoring temperatures or pressures for further automated control or emergency response is no longer pushed
ON Not Jog1Plus AND Not Jog1Minus DO
   Inst_MC_Halt( FALSE, Axis1, 10000, 0, 0 );
   Inst_MC_Halt( TRUE, Axis1, 10000, 0, 0 );
END_DO;
ON Not Jog2Plus AND Not Jog2Minus DO
   Inst_MC_Halt1( FALSE, LowerRotary, 10000, 0, 0 );
   Inst_MC_Halt1( TRUE, LowerRotary, 10000, 0, 0 );
END_DO;
//Relative moves in Manual mode
Inst_MC_MoveRelative( RelMove1, Axis1, RelDistance, JogVelocity, Manual_A1_Accel, Manual_A1_Decel, 0, 0 );
Inst_MC_MoveRelative1( RelMove2, LowerRotary, RelDistance, JogVelocity, Manual_A1_Accel, Manual_A1_Decel, 0, 0 );
Inst_MC_MoveRelative4( RelMove3, UpperRotary, RelDistance, JogVelocity, Manual_A1_Accel, Manual_A1_Decel, 0, 0 );
Inst_MC_MoveRelative5( RelMove4, LinearZAxis, RelDistance, LinearVelocity, Manual_A2_Accel, Manual_A2_Decel, 0, 0 );
//Absolute moves in Manual mode
Inst_MC_MoveAbsolute( AbsMove1, Axis1, AbsPosition, JogVelocity, Manual_A1_Accel, Manual_A1_Decel, 0, 0, 0 );
Inst_MC_MoveAbsolute1( AbsMove2, LowerRotary, AbsPosition, JogVelocity2, Manual_A2_Accel, Manual_A2_Decel, 0, 0, 0 );


__END_ACTION

#sfc=end
_next GT5;
_step GS101;
#sfc=GS101(1,9)
__BEGIN_P1
//Reset state machine to zero
StepCounter := 0;
StopCycle := FALSE;
__END_ACTION

__BEGIN_N
(*
Case statement to control sequence of different moves commanded including
relative, absolute, s-curved, gearing, camming, superimposed and phasing.
Alternate between commanding moves and waiting for them to be finished before moving to next
case in the sequence.
*)

CASE StepCounter OF
0://Move Linear axis up so that two rotary axis are not engaged
Inst_MC_MoveAbsolute2( FALSE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
StepCounter := 1;

1://Wait until linear axis is moved up out of the way, then command relative move to rotary axes
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
IF Inst_MC_MoveAbsolute2.Done THEN
   Inst_MC_MoveRelative6( FALSE, UpperRotary, 90, 500, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative7( FALSE, LowerRotary, -90, 500, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative6( TRUE, UpperRotary, 90, 500, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative7( TRUE, LowerRotary, -90, 500, 10000, 10000, 0, 0 );
   StepCounter:= 2;
END_IF;

2://Wait until relative rotary moves are done, then lower the linear axis so hole punch pegs engage
Inst_MC_MoveRelative6( TRUE, UpperRotary, 90, 500, 10000, 10000, 0, 0 );
Inst_MC_MoveRelative7( TRUE, LowerRotary, -90, 500, 10000, 10000, 0, 0 );
IF Inst_MC_MoveRelative6.Done AND Inst_MC_MoveRelative7.Done THEN
   Inst_MC_MoveAbsolute2( FALSE, LinearZAxis, LinearStartingPos, 500, 10000, 10000, 0, 0, 0 );
   Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearStartingPos, 500, 10000, 10000, 0, 0, 0 );
   StepCounter := 3;
END_IF;

3://Wait until linear axis absolute move is complete, then command relative move to rotary axes
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearStartingPos, 500, 10000, 10000, 0, 0, 0 );
IF Inst_MC_MoveAbsolute2.Done THEN
   Inst_MC_MoveRelative6( FALSE, UpperRotary, 90, 500, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative7( FALSE, LowerRotary, -90, 500, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative6( TRUE, UpperRotary, 90, 500, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative7( TRUE, LowerRotary, -90, 500, 10000, 10000, 0, 0 );
   StepCounter:= 4;
END_IF;

4://Wait until relative rotary moves are done, then raise linear axis to pegs cleared position
Inst_MC_MoveRelative6( TRUE, UpperRotary, 90, 500, 10000, 10000, 0, 0 );
Inst_MC_MoveRelative7( TRUE, LowerRotary, -90, 500, 10000, 10000, 0, 0 );
IF Inst_MC_MoveRelative6.Done AND Inst_MC_MoveRelative7.Done THEN
   Inst_MC_MoveAbsolute2( FALSE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
   Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
   StepCounter := 5;
END_IF;

5://Wait until linear absolute move is complete, then command jerk In physics, jerk is the rate of change of acceleration; more precisely, the derivative of acceleration with respect to time limited relative moves to rotary axes
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
IF Inst_MC_MoveAbsolute2.Done THEN
   Inst_MC_MoveRelative6( FALSE, UpperRotary, 90, 500, 100, 100, 50, 0 );
   Inst_MC_MoveRelative7( FALSE, LowerRotary, -90, 500, 100, 100, 50, 0 );
   Inst_MC_MoveRelative6( TRUE, UpperRotary, 90, 500, 100, 100, 50, 0 );
   Inst_MC_MoveRelative7( TRUE, LowerRotary, -90, 500, 100, 100, 50, 0 );
   StepCounter:= 6;
END_IF;

6://Wait for jerk limited relative moves on rotary axes are complete, then change drive parameter and lower linear axis
Inst_MC_MoveRelative6( TRUE, UpperRotary, 90, 500, 100, 100, 50, 0 );
Inst_MC_MoveRelative7( TRUE, LowerRotary, -90, 500, 100, 100, 50, 0 );
IF Inst_MC_MoveRelative6.Done AND Inst_MC_MoveRelative7.Done THEN
   Inst_DriveParamWrite( FALSE, any_to_int(LowerECATAddress), 'VL.LIMITP', 2000 );
   Inst_DriveParamWrite( TRUE, any_to_int(LowerECATAddress), 'VL.LIMITP', 2000 );
   Inst_MC_MoveAbsolute2( FALSE, LinearZAxis, LinearStartingPos, 500, 200, 200, 100, 0, 0 );
   Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearStartingPos, 500, 200, 200, 100, 0, 0 );
   StepCounter := 7;
END_IF;

7://Wait until linear axis is lowered, then start gear relationship on the rotary axes
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearStartingPos, 500, 200, 200, 100, 0, 0 );
IF Inst_MC_MoveAbsolute2.Done THEN
   Inst_MC_GearIn3( FALSE, Axis1, UpperRotary, 3, 1, 10000, 10000, 0, 0 );
   Inst_MC_GearIn4( FALSE, Axis1, LowerRotary, -3, 1, 10000, 10000, 0, 0 );
   Inst_MC_GearIn3( TRUE, Axis1, UpperRotary, 3, 1, 10000, 10000, 0, 0 );
   Inst_MC_GearIn4( TRUE, Axis1, LowerRotary, -3, 1, 10000, 10000, 0, 0 );
   StepCounter:= 8;
END_IF;

8://Once rotary axes are geared, start a relative move on the master axis and raise the linear axis
Inst_MC_GearIn3( TRUE, Axis1, UpperRotary, 3, 1, 10000, 10000, 0, 0 );
Inst_MC_GearIn4( TRUE, Axis1, LowerRotary, -3, 1, 10000, 10000, 0, 0 );
IF Inst_MC_GearIn3.InGear AND Inst_MC_GearIn4.InGear THEN
   Inst_MC_MoveRelative8( FALSE, Axis1, 720, 300, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative8( TRUE, Axis1, 720, 300, 10000, 10000, 0, 0 );
   Inst_MC_MoveAbsolute2( FALSE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
   Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
   StepCounter := 9;
END_IF;

9://Wait for the linear axis to be raised
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearClearedPosition, 500, 10000, 10000, 0, 0, 0 );
IF Inst_MC_MoveAbsolute2.Done THEN
   StepCounter := 10;
END_IF;

10://Move the linear axis to the bottom position so hole punch pegs are engaged
Inst_MC_MoveAbsolute2( FALSE, LinearZAxis, LinearStartingPos, 500, 200, 200, 100, 0, 0 );
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearStartingPos, 500, 200, 200, 100, 0, 0 );
StepCounter := 11;

11://Wait for master relative move and the linear move to finish, then start CAM relationship on two rotary axes
Inst_MC_MoveAbsolute2( TRUE, LinearZAxis, LinearStartingPos, 500, 200, 200, 100, 0, 0 );
Inst_MC_MoveRelative8( TRUE, Axis1, 720, 300, 10000, 10000, 0, 0 );
IF Inst_MC_MoveRelative8.Done AND Inst_MC_MoveAbsolute2.Done THEN
   Inst_MC_CamIn2( FALSE, Axis1, UpperRotary, 0, 0, 360, 360, 0, Profile1ID, 0 );
   Inst_MC_CamIn3( FALSE, Axis1, LowerRotary, 0, 0, 360, 360, 0, Profile2ID, 0 );
   Inst_MC_CamIn2( TRUE, Axis1, UpperRotary, 0, 0, 360, 360, 0, Profile1ID, 0 );
   Inst_MC_CamIn3( TRUE, Axis1, LowerRotary, 0, 0, 360, 360, 0, Profile2ID, 0 );
   StepCounter := 12;
END_IF;

12://Once CAM relationship is started, command relative move to the master
Inst_MC_CamIn2( TRUE, Axis1, UpperRotary, 0, 0, 360, 360, 0, Profile1ID, 0 );
Inst_MC_CamIn3( TRUE, Axis1, LowerRotary, 0, 0, 360, 360, 0, Profile2ID, 0 );
IF Inst_MC_CamIn2.InSync AND Inst_MC_CamIn3.InSync THEN
   Inst_MC_MoveRelative8( FALSE, Axis1, 720, 300, 10000, 10000, 0, 0 );
   Inst_MC_MoveRelative8( TRUE, Axis1, 720, 300, 10000, 10000, 0, 0 );
   StepCounter := 13;
END_IF;

13://Once master relative move is complete, start up a new gear relationship on the rotary axes
Inst_MC_MoveRelative8( TRUE, Axis1, 720, 300, 10000, 10000, 0, 0 );
IF Inst_MC_MoveRelative8.Done THEN
   Inst_MC_GearIn3( FALSE, Axis1, UpperRotary, 1, 1, 10000, 10000, 0, 0 );
   Inst_MC_GearIn4( FALSE, Axis1, LowerRotary, -1, 1, 10000, 10000, 0, 0 );
   Inst_MC_GearIn3( TRUE, Axis1, UpperRotary, 1, 1, 10000, 10000, 0, 0 );
   Inst_MC_GearIn4( TRUE, Axis1, LowerRotary, -1, 1, 10000, 10000, 0, 0 );
   StepCounter := 14;
END_IF;

14://Wait until Gear relationships have started, then command a velocity move with to the master
Inst_MC_GearIn3( TRUE, Axis1, UpperRotary, 1, 1, 10000, 10000, 0, 0 );
Inst_MC_GearIn4( TRUE, Axis1, LowerRotary, -1, 1, 10000, 10000, 0, 0 );
IF Inst_MC_GearIn3.InGear AND Inst_MC_GearIn4.InGear THEN
   Inst_MC_MoveVelocity4( FALSE, Axis1, 300, 10000, 10000, 0, 0, 0 );
   Inst_MC_MoveVelocity4( TRUE, Axis1, 300, 10000, 10000, 0, 0, 0 );
   Inst_ECATWriteSdo( FALSE, 16#3622, 0, 4, any_to_int(LowerECATAddress), 2470 );
   Inst_ECATWriteSdo( TRUE, 16#3622, 0, 4, any_to_int(LowerECATAddress), 2470 );
   Inst_TON( FALSE, T#2s );
   StepCounter := 15;
END_IF;

15://Start a timer, and after two seconds command a superimposed move to both rotary axes
Inst_TON( TRUE, T#2s );
IF Inst_TON.Q THEN
   Inst_MC_MoveSuperimp3( FALSE, UpperRotary, 360, 300, 10000, 10000, 0, 0 );
   Inst_MC_MoveSuperimp4( FALSE, LowerRotary, -360, 300, 10000, 10000, 0, 0 );
   Inst_MC_MoveSuperimp3( TRUE, UpperRotary, 360, 300, 10000, 10000, 0, 0 );
   Inst_MC_MoveSuperimp4( TRUE, LowerRotary, -360, 300, 10000, 10000, 0, 0 );
   Inst_TON( FALSE, T#2s );
   StepCounter := 16;
END_IF;

16://Wait for superimposed moves to finish and then move to next case
Inst_MC_MoveSuperimp3( TRUE, UpperRotary, 360, 300, 10000, 10000, 0, 0 );
Inst_MC_MoveSuperimp4( TRUE, LowerRotary, -360, 300, 10000, 10000, 0, 0 );
IF Inst_MC_MoveSuperimp3.Done AND Inst_MC_MoveSuperimp4.Done THEN
   StepCounter := 17;
END_IF;

17://Restart timer and wait two seconds, then phase adjust both rotary axes
Inst_TON( TRUE, T#2s );
IF Inst_TON.Q THEN
   Inst_MC_Phasing( FALSE, AXIS1, UpperRotary, -180, 150, 10000, 10000, 0, 0 );
   Inst_MC_Phasing1( FALSE, AXIS1, LowerRotary, -180, 150, 10000, 10000, 0, 0 );
   Inst_MC_Phasing( TRUE, AXIS1, UpperRotary, -180, 150, 10000, 10000, 0, 0 );
   Inst_MC_Phasing1( TRUE, AXIS1, LowerRotary, -180, 150, 10000, 10000, 0, 0 );
   StepCounter := 18;
END_IF;

18://Wait for phase adjustments to complete, then move to next case
Inst_MC_Phasing( TRUE, AXIS1, UpperRotary, -180, 150, 10000, 10000, 0, 0 );
Inst_MC_Phasing1( TRUE, AXIS1, LowerRotary, -180, 150, 10000, 10000, 0, 0 );
IF Inst_MC_Phasing.Done AND Inst_MC_Phasing1.Done THEN
   Inst_TON( FALSE, T#2s );
   StepCounter := 19;
END_IF;

19://Start timer, then after two seconds stop the velocity move on the master axis
Inst_TON( TRUE, T#2s );
IF Inst_TON.Q THEN
   Inst_MC_Halt7( FALSE, Axis1, 10000, 0, 0 );
   Inst_MC_Halt7( TRUE, Axis1, 10000, 0, 0 );
   StepCounter := 20;
END_IF;

20://Wait until master axis has stopped, then check StopCycle Boolean variable to decide if program should continue to cycle
Inst_MC_Halt7( TRUE, Axis1, 10000, 0, 0 );
IF Inst_MC_Halt7.Done THEN
   If StopCycle THEN
      StepCounter := 99;
   ELSE
      StepCounter :=100;
   END_IF;
END_IF;

99://Waiting state if StopCycle is set to true in HMI
100://Increase cycle counter and restart at the first case
CycleCount := CycleCount + 1;
StepCounter := 0;
END_CASE;

__END_ACTION

__BEGIN_P0
//Reset state machine to zero and stop all axes
StepCounter := 0;
Inst_MC_Halt7( FALSE, UpperRotary, 10000, 0, 0 );
Inst_MC_Halt7( TRUE, UpperRotary, 10000, 0, 0 );
Inst_MC_Halt8( FALSE, LowerRotary, 10000, 0, 0 );
Inst_MC_Halt8( TRUE, LowerRotary, 10000, 0, 0 );
Inst_MC_Halt9( FALSE, LinearZAxis, 10000, 0, 0 );
Inst_MC_Halt9( TRUE, LinearZAxis, 10000, 0, 0 );
Inst_MC_Halt10( FALSE, Axis1, 10000, 0, 0 );
Inst_MC_Halt10( TRUE, Axis1, 10000, 0, 0 );
__END_ACTION
#sfc=end
_next GT102;
_step GS201;
_next GT202;
_trans GT5;
#sfc=GT5(0,10)
MachineState <> 0
#sfc=end
_next GS4;
_trans GT102;
#sfc=GT102(1,10)
MachineState <> 1
#sfc=end
_next GS4;
_trans GT202;
#sfc=GT202(2,10)
MachineState <> 2
#sfc=end
_next GS4;