Skip to content

Example Code

Advanced Motion Package

E.g. Scara or Delta. Can be used for Kinematics up to DOF=4.

Example 1 - Startup Coordinated Motion with AMP

Startup Sequence for System to reach ability for Coordinated Movement.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
        CASE SequenceState OF
            0: // Enable Kinematic
                NoStateTasksToComplete := FALSE;

                IF DeltaKinematic.Enable() THEN
                    SequenceState := 5;
                END_IF

            5: // Wait for enabled
                IF DeltaKinematic.Enabled AND NOT DeltaKinematic.Busy THEN
                    SequenceState := 10;
                END_IF

            10: // Transition to cartesian mode - kinematic enable
                IF DeltaKinematic.BuildKinGroup() THEN
                    SequenceState := 15;
                END_IF

            15: // Transition to cartesian mode - kinematic enable
                IF DeltaKinematic.KinematicStatus = E_KinStatus.KinStatus_Ready AND NOT DeltaKinematic.Busy THEN
                    SequenceState := 20;
                END_IF

            20: // Create CM Group for Advanced Motion - Coordinated Path Motion
                IF DeltaKinematic.BuildCmGroup() THEN
                    SequenceState := 25;
                END_IF

            25: // NO Error -> Axis added to Group
                IF NOT DeltaKinematic.CmState.Error THEN
                    StateComplete();
                END_IF  
        END_CASE

Example 2 - Execute Movement Coordinated Motion with AMP

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
VAR
    _idx : DINT;
    DeltaPoints : ARRAY[0..Parameter_KinematicAmp.MaxPoints] OF ST_AmpPoint; // it is recommended to store points permanently in a recipe management system
END_VAR

        CASE SequenceState OF
            0: // Enable Motion Group
                IF DeltaKinematic.EnableCmGroup() THEN
                    SequenceState := 5;
                END_IF

            5: // Enabled -> Coordinated Motion State Diagram
                IF DeltaKinematic.CmState.GroupStandby THEN
                    SequenceState := 10;
                END_IF

            10: // Create Path 1
                DeltaKinematic.ClearPath();

                // Start Point
                _idx := 1;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                //DeltaPoints[1].McsPosition[E_McsAxis.Rot1] := 0; // Not used in Delta!
                DeltaPoints[_idx].Velocity := 50.0;
                DeltaPoints[_idx].Acceleration := 150.0;
                DeltaPoints[_idx].Deceleration := 25.0;
                DeltaPoints[_idx].Jerk := 1500.0;
                DeltaPoints[_idx].BufferMode := MC_BUFFER_MODE.mcAborting;
                DeltaPoints[_idx].TransitionMode := MC_TRANSITION_MODE.mcTransModeNone;
                DeltaPoints[_idx].TransitionParameter[1] := 0;
                DeltaPoints[_idx].TransitionParameter[2] := 0;
                DeltaPoints[_idx].TransitionParameterCount := 0;
                DeltaPoints[_idx].InvokeId := 10;

                // Radius Point 1
                _idx := 2;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 50;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 100;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                //DeltaPoints[1].McsPosition[E_McsAxis.Rot1] := 0; // Not used in Delta!
                DeltaPoints[_idx].Velocity := 50.0;
                DeltaPoints[_idx].Acceleration := 150.0;
                DeltaPoints[_idx].Deceleration := 25.0;
                DeltaPoints[_idx].Jerk := 1500.0;
                DeltaPoints[_idx].BufferMode := MC_BUFFER_MODE.mcBlendingPrevious;
                DeltaPoints[_idx].TransitionMode := MC_TRANSITION_MODE.mcTransModeCornerDistanceAdvanced;
                DeltaPoints[_idx].TransitionParameter[1] := 10;
                DeltaPoints[_idx].TransitionParameter[2] := 20;
                DeltaPoints[_idx].TransitionParameterCount := 2;
                DeltaPoints[_idx].InvokeId := 20;

                // Radius Point 2
                _idx := 3;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 200;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 100;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                //DeltaPoints[1].McsPosition[E_McsAxis.Rot1] := 0; // Not used in Delta!
                DeltaPoints[_idx].Velocity := 50.0;
                DeltaPoints[_idx].Acceleration := 150.0;
                DeltaPoints[_idx].Deceleration := 25.0;
                DeltaPoints[_idx].Jerk := 1500.0;
                DeltaPoints[_idx].BufferMode := MC_BUFFER_MODE.mcBlendingPrevious;
                DeltaPoints[_idx].TransitionMode := MC_TRANSITION_MODE.mcTransModeCornerDistanceAdvanced;
                DeltaPoints[_idx].TransitionParameter[1] := 10;
                DeltaPoints[_idx].TransitionParameter[2] := 20;
                DeltaPoints[_idx].TransitionParameterCount := 2;
                DeltaPoints[_idx].InvokeId := 30;

                // Final Point
                _idx := 4;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 300;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                //DeltaPoints[1].McsPosition[E_McsAxis.Rot1] := 0; // Not used in Delta!
                DeltaPoints[_idx].Velocity := 50.0;
                DeltaPoints[_idx].Acceleration := 150.0;
                DeltaPoints[_idx].Deceleration := 25.0;
                DeltaPoints[_idx].Jerk := 1500.0;
                DeltaPoints[_idx].BufferMode := MC_BUFFER_MODE.mcBuffered;
                DeltaPoints[_idx].TransitionMode := MC_TRANSITION_MODE.mcTransModeNone;
                DeltaPoints[_idx].TransitionParameter[1] := 0;
                DeltaPoints[_idx].TransitionParameter[2] := 0;
                DeltaPoints[_idx].TransitionParameterCount := 0;
                DeltaPoints[_idx].InvokeId := 40;

                // back to start circular
                _idx := 5;
                DeltaPoints[_idx].CircularMove.CircMode             := MC_CIRC_MODE.mcCircModeBorder;
                DeltaPoints[_idx].CircularMove.AuxPoint[E_McsAxis.X]    := 150;
                DeltaPoints[_idx].CircularMove.AuxPoint[E_McsAxis.Y]    := 150;
                DeltaPoints[_idx].CircularMove.AuxPoint[E_McsAxis.Z]    := -687;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X]  := 0; // End Point of Circle
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y]  := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z]  := -687;
                DeltaPoints[_idx].CircularMove.PathChoice           := MC_CIRC_PATHCHOICE.mcCircPathChoiceCounterClockwise;
                DeltaPoints[_idx].Velocity := 50.0;
                DeltaPoints[_idx].Acceleration := 150.0;
                DeltaPoints[_idx].Deceleration := 25.0;
                DeltaPoints[_idx].Jerk := 1500.0;
                DeltaPoints[_idx].BufferMode := MC_BUFFER_MODE.mcBuffered;
                DeltaPoints[_idx].TransitionMode := MC_TRANSITION_MODE.mcTransModeNone;
                DeltaPoints[_idx].TransitionParameter[1] := 0;
                DeltaPoints[_idx].TransitionParameter[2] := 0;
                DeltaPoints[_idx].TransitionParameterCount := 0;
                DeltaPoints[_idx].InvokeId := 50;

                DeltaKInematic.Points := DeltaPoints;
                DeltaKinematic.AddLinearMoveToPath(1);
                DeltaKinematic.AddLinearMoveToPath(2);
                DeltaKinematic.AddLinearMoveToPath(3);
                DeltaKinematic.AddLinearMoveToPath(4);
                DeltaKinematic.AddCircularMoveToPath(5);

                SequenceState := 20;

            20: // Execute Path 1
                IF DeltaKinematic.StartMovePath() THEN
                    SequenceState := 30;
                END_IF

            30: // Wait Moving
                IF DeltaKinematic.CmState.GroupMoving AND DeltaKinematic.busy THEN
                    SequenceState := 40;
                END_IF

            40: // Wait for Path Done   
                IF NOT DeltaKinematic.CmState.GroupMoving AND NOT DeltaKinematic.busy THEN
                    SequenceState := 10; // repeat path
                END_IF

            999: // Done
                 ;
        END_CASE

Example 3 - Stop and Shutdown Coordinated Motion with AMP

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
        CASE SequenceState OF
            0:  NoStateTasksToComplete := FALSE;
                IF DeltaKinematic.Moving THEN     // Kinematic Moving
                    IF DeltaKinematic.Stop(0,0) THEN
                        SequenceState := 5;
                    END_IF
                ELSE
                    SequenceState := 10;
                END_IF

            5: // Wait for not Moving
                IF NOT DeltaKinematic.Moving THEN
                    SequenceState := 10;
                END_IF

            10: // Check CM Group Enabled/Built
                IF NOT DeltaKinematic.CmState.GroupDisabled THEN
                    IF DeltaKinematic.DisableCmGroup() THEN
                        SequenceState := 20;
                    END_IF
                ELSE
                    SequenceState := 30;
                END_IF

            20: // When Disabled - ungroup
                IF DeltaKinematic.CmState.GroupDisabled OR DeltaKinematic.CmState.GroupErrorStop THEN
                    DeltaKinematic.UngroupCmGroup();
                    SequenceState := 30;
                END_IF

            30: // Kinematic Group is Built -> Dissolve before disable
                IF DeltaKinematic.KinematicStatus = E_KinStatus.KinStatus_Ready THEN
                    DeltaKinematic.DissolveKinGroup();
                    SequenceState := 35;
                ELSE
                    SequenceState := 40;
                END_IF

            35: // Wait for Dissolved
                IF DeltaKinematic.KinematicStatus = E_KinStatus.KinStatus_Empty THEN
                    SequenceState := 40;
                END_IF

            40: // Disable ACS and MCS
                IF DeltaKinematic.Disable() THEN
                    SequenceState := 50;
                END_IF

            50: // Wait for Disabled
                IF NOT DeltaKinematic.Enabled THEN
                    SequenceState := 999;
                END_IF

            999:// Done
                StateTasksComplete := TRUE;
        END_CASE

Example 4 - Execute Movement Coordinated Motion with AMP and switch Path during motion

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
        CASE SequenceState OF
            0: // Enable Motion Group
                IF DeltaKinematic.EnableCmGroup() THEN
                    SequenceState := 5;
                END_IF

            5: // Enabled -> Coordinated Motion State Diagram
                IF DeltaKinematic.CmState.GroupStandby THEN
                    SequenceState := 10;
                END_IF

            10: // Create Path 1
                DeltaKinematic.ClearPath();

                // Start Point
                _idx                                       := 1;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                DeltaPoints[_idx].Velocity                 := 50.0;
                DeltaPoints[_idx].Acceleration             := 150.0;
                DeltaPoints[_idx].Deceleration             := 25.0;
                DeltaPoints[_idx].Jerk                     := 1500.0;
                DeltaPoints[_idx].BufferMode               := MC_BUFFER_MODE.mcAborting;
                DeltaPoints[_idx].TransitionMode           := MC_TRANSITION_MODE.mcTransModeNone;
                DeltaPoints[_idx].TransitionParameter[1]   := 0;
                DeltaPoints[_idx].TransitionParameter[2]   := 0;
                DeltaPoints[_idx].TransitionParameterCount := 0;
                DeltaPoints[_idx].InvokeId                 := 10;

                // Radius Point 1
                _idx                                       := 2;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 50;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                DeltaPoints[_idx].Velocity                 := 50.0;
                DeltaPoints[_idx].Acceleration             := 150.0;
                DeltaPoints[_idx].Deceleration             := 25.0;
                DeltaPoints[_idx].Jerk                     := 1500.0;
                DeltaPoints[_idx].BufferMode               := MC_BUFFER_MODE.mcBlendingPrevious;
                DeltaPoints[_idx].TransitionMode           := MC_TRANSITION_MODE.mcTransModeCornerDistanceAdvanced;
                DeltaPoints[_idx].TransitionParameter[1]   := 10;
                DeltaPoints[_idx].TransitionParameter[2]   := 20;
                DeltaPoints[_idx].TransitionParameterCount := 2;
                DeltaPoints[_idx].InvokeId                 := 20;

                // Radius Point 2
                _idx                                       := 3;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 200;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                DeltaPoints[_idx].Velocity                 := 50.0;
                DeltaPoints[_idx].Acceleration             := 150.0;
                DeltaPoints[_idx].Deceleration             := 25.0;
                DeltaPoints[_idx].Jerk                     := 1500.0;
                DeltaPoints[_idx].BufferMode               := MC_BUFFER_MODE.mcBlendingPrevious;
                DeltaPoints[_idx].TransitionMode           := MC_TRANSITION_MODE.mcTransModeCornerDistanceAdvanced;
                DeltaPoints[_idx].TransitionParameter[1]   := 10;
                DeltaPoints[_idx].TransitionParameter[2]   := 20;
                DeltaPoints[_idx].TransitionParameterCount := 2;
                DeltaPoints[_idx].InvokeId                 := 30;

                // Final Point
                _idx                                       := 4;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := 300;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                DeltaPoints[_idx].Velocity                 := 50.0;
                DeltaPoints[_idx].Acceleration             := 150.0;
                DeltaPoints[_idx].Deceleration             := 25.0;
                DeltaPoints[_idx].Jerk                     := 1500.0;
                DeltaPoints[_idx].BufferMode               := MC_BUFFER_MODE.mcBlendingPrevious;
                DeltaPoints[_idx].TransitionMode           := MC_TRANSITION_MODE.mcTransModeCornerDistanceAdvanced;
                DeltaPoints[_idx].TransitionParameter[1]   := 10;
                DeltaPoints[_idx].TransitionParameter[2]   := 20;
                DeltaPoints[_idx].TransitionParameterCount := 2;
                DeltaPoints[_idx].InvokeId                 := 40;

                // Build Path
                DeltaKInematic.Points := DeltaPoints;
                DeltaKinematic.AddLinearMoveToPath(1);
                DeltaKinematic.AddLinearMoveToPath(2);
                DeltaKinematic.AddLinearMoveToPath(3);
                DeltaKinematic.AddLinearMoveToPath(4);
                SequenceState := 20;

            20: // Execute Path 1
                IF DeltaKinematic.StartMovePath(ChangePath := TRUE) THEN
                    SequenceState := 30;
                END_IF

            30: // Wait Moving
                IF DeltaKinematic.CmState.GroupMoving AND DeltaKinematic.busy THEN
                    SequenceState := 40;
                END_IF

            40: // Wait for Path Done   
                IF NOT DeltaKinematic.CmState.GroupMoving AND NOT DeltaKinematic.busy THEN
                    SequenceState := 10; // repeat path
                END_IF

                IF Interrupt AND DeltaKinematic.CmState.McToPlcCm.InvokeId = 30 THEN
                    Interrupt     := FALSE;
                    SequenceState := 50;
                END_IF

            50: //
                DeltaKinematic.ClearPath();

                // Redirect
                _idx                                       := 5;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := -55;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                DeltaPoints[_idx].Velocity                 := 80.0;
                DeltaPoints[_idx].Acceleration             := 150.0;
                DeltaPoints[_idx].Deceleration             := 25.0;
                DeltaPoints[_idx].Jerk                     := 1500.0;
                DeltaPoints[_idx].BufferMode               := MC_BUFFER_MODE.mcAborting;
                DeltaPoints[_idx].TransitionMode           := MC_TRANSITION_MODE.mcTransModeNone;
                DeltaPoints[_idx].TransitionParameter[1]   := 0;
                DeltaPoints[_idx].TransitionParameter[2]   := 0;
                DeltaPoints[_idx].TransitionParameterCount := 0;
                DeltaPoints[_idx].InvokeId                 := 50;

                // Redirect
                _idx                                       := 6;
                DeltaPoints[_idx].McsPosition[E_McsAxis.X] := -60;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Y] := 0;
                DeltaPoints[_idx].McsPosition[E_McsAxis.Z] := -687;
                DeltaPoints[_idx].Velocity                 := 5.0;
                DeltaPoints[_idx].Acceleration             := 150.0;
                DeltaPoints[_idx].Deceleration             := 25.0;
                DeltaPoints[_idx].Jerk                     := 1500.0;
                DeltaPoints[_idx].BufferMode               := MC_BUFFER_MODE.mcBlendingPrevious;
                DeltaPoints[_idx].TransitionMode           := MC_TRANSITION_MODE.mcTransModeCornerDistanceAdvanced;
                DeltaPoints[_idx].TransitionParameter[1]   := 10;
                DeltaPoints[_idx].TransitionParameter[2]   := 20;
                DeltaPoints[_idx].TransitionParameterCount := 2;
                DeltaPoints[_idx].InvokeId                 := 60;

                // Build Path
                DeltaKInematic.Points := DeltaPoints;
                DeltaKinematic.AddLinearMoveToPath(5);
                DeltaKinematic.AddLinearMoveToPath(6);
                SequenceState := 20;

            999: // Done
                ;
        END_CASE

Example Scope for AMP

Camming

Can be used for all Kinematic Systems.

Example 1 - Startup Coordinated Motion with Camming

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
            CASE SequenceState OF
                0: // Enable Kinematic
                    IF RobotKinematic.Enable() THEN
                        SequenceState := 5;
                    END_IF

                5: // Wait for enabled
                    IF RobotKinematic.Enabled AND NOT RobotKinematic.Busy THEN
                        IF _Recipe.MoveAcsPoints THEN // Execute from here if ACS movements should be done
                            // Done
                        ELSE
                            SequenceState := 10;
                        END_IF

                    END_IF

                10: // Transition to cartesian mode - kinematic enable
                    IF RobotKinematic.BuildKinGroup() THEN
                        SequenceState := 15;
                    END_IF

                15: // Transition to cartesian mode - kinematic enable
                    IF RobotKinematic.KinematicStatus = E_KinStatus.KinStatus_Ready AND NOT RobotKinematic.Busy THEN
                        // Done
                    END_IF  
            END_CASE

Example 2 - Execute Movement Coordinated Motion with Camming

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
VAR
    _TempPointVelocity           : ARRAY[1..KINEMATIC_CONSTANTS.DEGREES_OF_FREEDOM_MAX] OF LREAL; 
    _TempPointAcceleration       : ARRAY[1..KINEMATIC_CONSTANTS.DEGREES_OF_FREEDOM_MAX] OF LREAL; 
    _TempPointJerk               : ARRAY[1..KINEMATIC_CONSTANTS.DEGREES_OF_FREEDOM_MAX] OF LREAL; 
END_VAR

            CASE SequenceState OF
                0: // Add Point 1-4
                    IF  RobotKinematic.CamAddPointToPath(   MotionFunctionFirstPoint    := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            MotionFunctionThisPoint     := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            PointNumber                 := 2, 
                                                            TotalPointsInTable          := 5, // Minimum of 2 (Start Point + End Point), Start Point is automatically set by FB internally
                                                            PointVelocity               := _TempPointVelocity,      // at this Point in Cam Table
                                                            PointAcceleration           := _TempPointAcceleration,  // at this Point in Cam Table
                                                            PointJerk                   := _TempPointJerk,          // at this Point in Cam Table
                                                            PointIndex                  := 1)                       // Index of Point in structure of ST_BasePoint
                        AND
                        RobotKinematic.CamAddPointToPath(   MotionFunctionFirstPoint    := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            MotionFunctionThisPoint     := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            PointNumber                 := 3, 
                                                            TotalPointsInTable          := 5, // Minimum of 2 (Start Point + End Point), Start Point is automatically set by FB internally
                                                            PointVelocity               := _TempPointVelocity,      // at this Point in Cam Table
                                                            PointAcceleration           := _TempPointAcceleration,  // at this Point in Cam Table
                                                            PointJerk                   := _TempPointJerk,          // at this Point in Cam Table
                                                            PointIndex                  := 2)                       // Index of Point in structure of ST_BasePoint  
                        AND
                        RobotKinematic.CamAddPointToPath(   MotionFunctionFirstPoint    := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            MotionFunctionThisPoint     := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            PointNumber                 := 4, 
                                                            TotalPointsInTable          := 5, // Minimum of 2 (Start Point + End Point), Start Point is automatically set by FB internally
                                                            PointVelocity               := _TempPointVelocity,      // at this Point in Cam Table
                                                            PointAcceleration           := _TempPointAcceleration,  // at this Point in Cam Table
                                                            PointJerk                   := _TempPointJerk,          // at this Point in Cam Table
                                                            PointIndex                  := 3)                       // Index of Point in structure of ST_BasePoint      
                        AND
                        RobotKinematic.CamAddPointToPath(   MotionFunctionFirstPoint    := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            MotionFunctionThisPoint     := MC_MotionFunctionType.MOTIONFUNCTYPE_POLYNOM5, 
                                                            PointNumber                 := 5, 
                                                            TotalPointsInTable          := 5, // Minimum of 2 (Start Point + End Point), Start Point is automatically set by FB internally
                                                            PointVelocity               := _TempPointVelocity,      // at this Point in Cam Table
                                                            PointAcceleration           := _TempPointAcceleration,  // at this Point in Cam Table
                                                            PointJerk                   := _TempPointJerk,          // at this Point in Cam Table
                                                            PointIndex                  := 4)                       // Index of Point in structure of ST_BasePoint  
                    THEN
                        SequenceState := 10;    
                    END_IF

                10: // Check for Points added successfully 
                    IF NOT RobotKinematic.Error AND NOT RobotKinematic.Busy THEN
                        SequenceState := 20;
                    END_IF

                20: // Cam In
                    IF RobotKinematic.CamIn() THEN
                        SequenceState := 30;
                    END_IF

                30: // Check Cammed In
                    IF RobotKinematic.CammingActive AND NOT RobotKinematic.Error AND NOT RobotKinematic.Busy THEN
                        SequenceState := 40;
                    END_IF

                40: // Start move Kinematic
                    IF RobotKinematic.CamStartMovePath( TargetMoveVelocity      := Parameter_Kinematic.CamDefaultMoveVelocity, // Move Velocity for Cam Table Master !!!Needs to consider all Slave dynamics, all Slaves try to follow Master setpoints!!!
                                                        TargetMoveAcceleration  := Parameter_Kinematic.CamDefaultMoveAcceleration, // Move Acceleration for Cam Table Master !!!Needs to consider all Slave dynamics, all Slaves try to follow Master setpoints!!!
                                                        TargetMoveDeceleration  := Parameter_Kinematic.CamDefaultMoveDeceleration, // Move Deceleration for Cam Table Master !!!Needs to consider all Slave dynamics, all Slaves try to follow Master setpoints!!!
                                                        TargetMoveJerk          := Parameter_Kinematic.CamDefaultMoveJerk ) 
                    THEN
                        SequenceState := 50; 
                    END_IF

                50: // Kinematic Moving or Done 
                    IF RobotKinematic.Moving OR ( NOT RobotKinematic.Error AND NOT RobotKinematic.Busy AND NOT RobotKinematic.Moving ) THEN
                        SequenceState := 60;
                    END_IF

                60: // Move Complete    
                    IF NOT RobotKinematic.Error AND NOT RobotKinematic.Busy AND NOT RobotKinematic.Moving THEN
                        SequenceState := 70;
                    END_IF

                70: // Cam Out
                    IF RobotKinematic.CamOut() THEN
                        SequenceState := 80;
                    END_IF

                80: // Check Cammed Out
                    IF NOT RobotKinematic.CammingActive AND NOT RobotKinematic.Error AND NOT RobotKinematic.Busy THEN
                        SequenceState := 90;
                    END_IF

                90: // Clear Path
                    IF RobotKinematic.CamClearPath() THEN
                        SequenceState := 0; // Repeat Path
                    END_IF

                999: // Done
                     ;
            END_CASE
Note

Point Management is done via recipe. Not visible in this example.

Example 3 - Stop and Shutdown Coordinated Motion with Camming

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
            CASE SequenceState OF
                0:  IF RobotKinematic.Moving THEN     // Kinematic Moving
                        IF RobotKinematic.Stop(0,0) THEN
                            SequenceState := 5;
                        END_IF
                    ELSE
                        SequenceState := 10;
                    END_IF

                5: // Wait for not Moving
                    IF NOT RobotKinematic.Moving AND NOT RobotKinematic.Busy THEN
                        SequenceState := 10;
                    END_IF

                10: // Check for system Cammed 
                    IF RobotKinematic.CammingActive THEN
                        IF RobotKinematic.CamOut() THEN
                            SequenceState := 20;
                        END_IF
                    ELSE
                        SequenceState := 20;
                    END_IF

                20: // Check for system Cammed Out
                    IF NOT RobotKinematic.CammingActive AND NOT RobotKinematic.Error AND NOT RobotKinematic.Busy THEN
                        SequenceState := 30;
                    END_IF

                30: // Disable ACS and MCS
                    IF RobotKinematic.Disable() THEN
                        SequenceState := 40;
                    END_IF

                40: // Wait for Disabled
                    IF NOT RobotKinematic.Enabled THEN
                        SequenceState := 999;
                    END_IF

                999:// Done
            END_CASE

Example Scope for Camming