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 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 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 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;