Group Motion complete code
This page contains the complete code of Group Motion.
In GroupMotion.cpp, your group motion code should be as follows:
Copy
#include "RT_Project_01.h"
#include "AxisConfiguration.h"
#include "GroupConfiguration.h"
#include "GroupMotion.h"
VOID SetGroupStartingPosition(int Group);
VOID GetAGroupPosition(int& Group);
VOID GetAGroupVelocity(int& Group);
VOID MoveLinearAbsoluteGroup(int Group)
{
RtPrintf("Make a linear absolute group move.\n\n");
const int LENGTH = 3; //The length of the Position array.
double EndPositions[LENGTH] = { 4000, 3000, 5000 };
KsCommandStatus absoluteMove = WaitForCommand(30, TRUE, MoveLinearAbsolute(Group, LENGTH,
EndPositions, MAXIMUM_VELOCITY, MAXIMUM_ACCELERATION, MAXIMUM_DECELERATION,
MAXIMUM_JERK, mcAxisCoordSystem, mcAborting, mcNone, NULL));
if (absoluteMove.Error)
RtPrintf("MoveLinearAbsolute failed: %d\n\n", absoluteMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveLinearRelativeGroup(int Group)
{
RtPrintf("Make linear relative group move.\n\n");
const int LENGTH = 3; //The length of the Distance array.
double Distance[LENGTH] = { 5000, 6000, 7000 };
KsCommandStatus relativeMove = WaitForCommand(30, TRUE, MoveLinearRelative(Group, LENGTH,
Distance, MAXIMUM_VELOCITY, MAXIMUM_ACCELERATION, MAXIMUM_DECELERATION,
MAXIMUM_JERK, mcAxisCoordSystem, mcAborting, mcNone, NULL));
if (relativeMove.Error)
RtPrintf("MoveLinearRelative failed: %d\n\n", relativeMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveLinearAdditiveGroup(int Group)
{
RtPrintf("Make linear additive group move.\n\n");
const int LENGTH = 3; //The length of the Distance array.
double Distance[LENGTH] = { -3000, -2000, -4000 };
KsCommandStatus additiveMove = WaitForCommand(30, TRUE, MoveLinearAdditive(Group, LENGTH,
Distance, MAXIMUM_VELOCITY, MAXIMUM_ACCELERATION, MAXIMUM_DECELERATION, MAXIMUM_JERK,
mcAxisCoordSystem, mcAborting, mcNone, NULL));
if (additiveMove.Error)
RtPrintf("MoveLinearAdditive failed: %d\n\n", additiveMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveCircularAbsoluteGroup(int Group)
{
RtPrintf("Make circular absolute group move.\n\n");
const int LENGTH = 3; //The length of the Points array.
double AuxPoints[LENGTH] = { -400, 0, 0 };
double EndPoints[LENGTH] = { 0, -400, 400 };
/*We lower the velocity to 360 because the end positions are not far. If the velocity is high
and the distance is short, the axis will brake sharply. This may damage the axis.*/
KsCommandStatus absCircMove = WaitForCommand(30, TRUE, MoveCircularAbsolute(Group, mcRadius,
LENGTH, AuxPoints, EndPoints, mcShortPath, 360, 3600, 3600, 360000, mcAxisCoordSystem,
mcAborting, mcNone, NULL));
if (absCircMove.Error)
RtPrintf("MoveCircularAbsolute failed: %d\n\n", absCircMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveCircularRelativeGroup(int Group)
{
RtPrintf("Make circular relative group move.\n\n");
const int LENGTH = 3; //The length of the Points array.
double AuxPoints[LENGTH] = { 0, 0, 600 };
double EndPoints[LENGTH] = { 600, 0, 600 };
/*We lower the velocity to 360 because the distance is not far. If the velocity is high
and the distance is short, the axis will brake sharply. This may damage the axis.*/
KsCommandStatus relCircMove = WaitForCommand(30, TRUE, MoveCircularRelative(Group, mcCenter,
LENGTH, AuxPoints, EndPoints, mcShortPath, 360, 3600, 3600, 360000, mcAxisCoordSystem,
mcAborting, mcNone, NULL));
if (relCircMove.Error)
RtPrintf("MoveCircularRelative failed: %d\n\n", relCircMove.ErrorId);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveCircularAdditiveGroup(int Group)
{
RtPrintf("Make circular additive group move.\n\n");
const int LENGTH = 3; //The length of the Points array.
double AuxPoints[LENGTH] = { 0, 300, 0 };
double EndPoints[LENGTH] = { 0, 300, -600 };
/*We lower the velocity to 360 because the distance is not far. If the velocity is high
and the distance is short, the axis will brake sharply. This may damage the axis.*/
KsCommandStatus addCircMove = WaitForCommand(30, TRUE, MoveCircularAdditive(Group, mcBorder,
LENGTH, AuxPoints, EndPoints, mcShortPath, 360, 3600, 3600, 360000, mcAxisCoordSystem,
mcAborting, mcNone, NULL));
if (addCircMove.Error)
RtPrintf("MoveCircularAdditive failed: %d\n\n", addCircMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveHelicalAbsoluteGroup(int Group)
{
RtPrintf("Make helical absolute group move.\n\n");
const int LENGTH = 3; //The length of the Points array.
double AuxPoints[LENGTH] = { 0, 0, 300 };
double EndPoints[LENGTH] = { 300, 300, 0 };
/*We lower the velocity to 360 because the end positions are not far. If the velocity is high
and the distance is short, the axis will brake sharply. This may damage the axis.*/
/*When you use HelicalMove, you may need to give the move more time to complete because
drawing helices takes more time.*/
KsCommandStatus absHelMove = WaitForCommand(60, TRUE, MoveHelicalAbsolute(Group, mcRadius,
LENGTH, AuxPoints, EndPoints, mcShortPath, 60, 10, 360, 3600, 3600, 360000,
mcAxisCoordSystem, mcAborting, mcNone, NULL));
if (absHelMove.Error)
RtPrintf("MoveHelicalAbsolute failed: %d\n\n", absHelMove.ErrorId);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveHelicalRelativeGroup(int Group)
{
RtPrintf("Make helical relative group move.\n\n");
const int LENGTH = 3; //The length of the Points array.
double AuxPoints[LENGTH] = { -300, 0, 0 };
double EndPoints[LENGTH] = { 0, -300, 300 };
/*We lower the velocity to 360 because the end positions are not far. If the velocity is high
and the distance is short, the axis will brake sharply. This may damage the axis.*/
/*When you use HelicalMove, you may need to give the move more time to complete because
drawing helices takes more time.*/
KsCommandStatus relHelMove = WaitForCommand(60, TRUE, MoveHelicalRelative(Group, mcRadius,
LENGTH, AuxPoints, EndPoints, mcShortPath, 60, 10, 360, 3600, 3600, 360000,
mcAxisCoordSystem, mcAborting, mcNone, NULL));
if (relHelMove.Error)
RtPrintf("MoveHelicalRelative failed: %d\n\n", relHelMove.ErrorId);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID MoveHelicalAdditiveGroup(int Group)
{
RtPrintf("Make helical additive group move.\n\n");
const int LENGTH = 3; //The length of the Points array.
double AuxPoints[LENGTH] = { 0, 400, 0 };
double EndPoints[LENGTH] = { 400, 0, 400 };
/*We lower the velocity to 360 because the end positions are not far. If the velocity is high
and the distance is short, the axis will brake sharply. This may damage the axis.*/
/*When you use HelicalMove, you may need to give the move more time to complete because
drawing helices takes more time.*/
KsCommandStatus addHelMove = WaitForCommand(60, TRUE, MoveHelicalAdditive(Group, mcRadius,
LENGTH, AuxPoints, EndPoints, mcShortPath, 60, 10, 360, 3600, 3600, 360000,
mcAxisCoordSystem, mcAborting, mcNone, NULL));
if (addHelMove.Error)
RtPrintf("MoveHelicalAdditive failed: %d\n\n", addHelMove.ErrorId);
//Display the end positions.
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID GroupJog(int Group)
{
RtPrintf("Make a jog group move.\n\n");
const int LENGTH = 3; //The length of the Velocity array.
double Velocity[LENGTH] = { 3000, 3600, 3200 };
//Start a jog move.
KsCommandStatus jogMove = JogGroup(Group, LENGTH, Velocity, MAXIMUM_ACCELERATION,
MAXIMUM_DECELERATION, MAXIMUM_JERK, mcAxisCoordSystem);
//Let the axis moves for a while.
Sleep(5000);
GetAGroupVelocity(Group);
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID GroupInch(int Group)
{
RtPrintf("Make an inch group move.\n\n");
const int LENGTH = 3; //The length of the Velocity and Distance arrays.
double Velocity[LENGTH] = { 3000, 3600, 3200 };
double Distance[LENGTH] = { 4000, 5000, 4500 };
//Start an inch move.
KsCommandStatus inchMove = WaitForCommand(30, FALSE, InchGroup(Group, LENGTH,
Distance, Velocity, MAXIMUM_ACCELERATION, MAXIMUM_DECELERATION, MAXIMUM_JERK,
mcAxisCoordSystem));
RtPrintf("End positions:\n\n");
GetAGroupPosition(Group);
}
VOID GroupHalt(int Group)
{
//Give some time for the axis to be halted.
KsCommandStatus halt = WaitForCommand(5, FALSE, HaltGroup(Group, MAXIMUM_DECELERATION,
MAXIMUM_JERK, mcAborting));
if (halt.Error)
RtPrintf("HaltGroup failed: %d\n", halt.ErrorId);
RtPrintf("Halt Group %d.\n\n", Group);
}
VOID GroupStop(int Group)
{
//Give some time for the group to be stopped.
KsCommandStatus stop = WaitForCommand(5, FALSE, StopGroup(Group, MAXIMUM_DECELERATION,
MAXIMUM_JERK));
if (stop.Error)
RtPrintf("StopGroup failed: %d\n", stop.ErrorId);
RtPrintf("Stop Group %d.\n\n", Group);
//Get the state of the group. The group should be in the GroupStopping state.
GetAGroupState(Group);
//Release the group from the GroupStopping state.
RtPrintf("Release the group from the GroupStopping state.\n\n");
KsError error = ReleaseGroup(Group);
if (error != errNoError)
RtPrintf("ReleaseGroup failed: %x\n", error);
//Give some time for the group state to be changed, and get the state again.
Sleep(30);
GetAGroupState(Group);
}
VOID BlendSample(int Group)
{
RtPrintf("Blend multi-axis moves.\n\n");
/*Following errors affects the end positions, so the actual end positions will be
different from the commanded ones.*/
/*------Circular Absolute------*/
RtPrintf("------Circular Absolute------\n\n");
//Starting positions: (0, 0, 0).
//End positions: (0, 200, 200).
const int LENGTH = 3; //The length of the Position array.
double CircAbsAuxPoints[LENGTH] = { -200, 0, 0 };
double CircAbsEndPoints[LENGTH] = { 0, 200, 200 };
double TransitionParam = 10;
//Display the starting positions.
RtPrintf("Circular Absolute starting positions:\n\n");
GetAGroupPosition(Group);
/*We lower the velocity to 360, because the end positions are not far. If the velocity is high
and the distance is short, the axis will brake sharply. This may damage the axis.*/
KsCommandStatus absCircMove = WaitForCommand(30, TRUE, MoveCircularAbsolute(Group, mcRadius,
LENGTH, CircAbsAuxPoints, CircAbsEndPoints, mcShortPath, 360, 3600, 3600, 360000,
mcAxisCoordSystem, mcAborting, mcNone, NULL));
if (absCircMove.Error)
RtPrintf("MoveCircularAbsolute failed: %d\n\n", absCircMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("Circular Absolute end positions:\n\n");
GetAGroupPosition(Group);
/*------Linear Absolute------*/
RtPrintf("------Linear Absolute------\n\n");
//Starting positions: (0, 200, 200).
//End positions: (-600, 400, 400).
double LinearAbsEndPositions[LENGTH] = { -600, 400, 400 };
//Display the starting positions.
RtPrintf("Linear Absolute starting positions:\n\n");
GetAGroupPosition(Group);
/*When you use blending in MoveLinear functions, you must use TransitionMode and
TransitionParameter. If you set them to mcNone and NULL in a blending mode,
an error will occur.*/
KsCommandStatus absoluteMove = WaitForCommand(30, TRUE, MoveLinearAbsolute(Group, LENGTH,
LinearAbsEndPositions, 260, 2600, 2600, 260000, mcAxisCoordSystem, mcBlendingLow,
mcCornerDistance, &TransitionParam));
if (absoluteMove.Error)
RtPrintf("MoveLinearAbsolute failed: %d\n\n", absoluteMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("Linear Absolute end positions:\n\n");
GetAGroupPosition(Group);
/*------Circular Relative------*/
RtPrintf("------Circular Relative------\n\n");
//Relative uses distance, so Relative Points are distance, not positions.
//Both AuxPoints and EndPoints are counted from the starting position.
//Starting positions: (-600, 400, 400).
//Center of the circle: (-600, 400, 400) + (-300, 0, 0) = (-900, 400, 400).
//End positions: (-600, 400, 400) + (-300, 0, 300) = (-900, 400, 700).
double CircRelAuxPoints[LENGTH] = { -300, 0, 0 };
double CircRelEndPoints[LENGTH] = { -300, 0, 300 };
RtPrintf("Circular Relative starting positions:\n\n");
GetAGroupPosition(Group);
/*When you use blending in MoveCircular functions, you must use TransitionMode and
TransitionParameter, even if MoveCircular has not supported them in KINGSTAR 4.1 yet.
If you set them to mcNone and NULL in a blending mode, an error will occur.*/
KsCommandStatus relCircMove = WaitForCommand(30, TRUE, MoveCircularRelative(Group, mcCenter,
LENGTH, CircRelAuxPoints, CircRelEndPoints, mcShortPath, 300, 3000, 3000, 300000,
mcAxisCoordSystem, mcBlendingNext, mcCornerDistance, &TransitionParam));
if (relCircMove.Error)
RtPrintf("MoveCircularRelative failed: %d\n\n", relCircMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("Circular Relative end positions:\n\n");
GetAGroupPosition(Group);
/*------Linear Relative------*/
RtPrintf("------Linear Relative------\n\n");
//Starting positions: (-900, 400, 700).
//End positions: (-900, 400, 700) + (-100, -900, -400) = (-1000, -500, 300).
double Distance[LENGTH] = { -100, -900, -400 };
//Display the starting positions.
RtPrintf("Linear Relative starting positions:\n\n");
GetAGroupPosition(Group);
KsCommandStatus relativeMove = WaitForCommand(30, TRUE, MoveLinearRelative(Group, LENGTH,
Distance, 250, 2500, 2500, 250000, mcAxisCoordSystem, mcBlendingHigh,
mcCornerDistance, &TransitionParam));
if (relativeMove.Error)
RtPrintf("MoveLinearRelative failed: %d\n\n", relativeMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("Linear Relative end positions:\n\n");
GetAGroupPosition(Group);
/*------Circular Additive------*/
RtPrintf("------Circular Additive------\n\n");
//Additive uses distance, so Additive Points are distance, not positions.
//Both AuxPoints and EndPoints are counted from the starting position.
//Starting positions: (-1000, -500, 300).
//Border point of the circle: (-1000, -500, 300) + (0, 0, 600) = (-1000, -500, 900).
//End positions: (-1000, -500, 300) + (0, -300, 0) = (-1000, -800, 300).
double AuxPoints[LENGTH] = { 0, 0, 600 };
double EndPoints[LENGTH] = { 0, -300, 0 };
//Display the starting positions.
RtPrintf("Circular Additive starting positions:\n\n");
GetAGroupPosition(Group);
KsCommandStatus addCircMove = WaitForCommand(30, TRUE, MoveCircularAdditive(Group, mcBorder,
LENGTH, AuxPoints, EndPoints, mcShortPath, 280, 2800, 2800, 280000, mcAxisCoordSystem,
mcBlendingPrevious, mcCornerDistance, &TransitionParam));
if (addCircMove.Error)
RtPrintf("MoveCircularAdditive failed: %d\n\n", addCircMove.ErrorId);
//Wait a few cycles to get the correct end positions.
Sleep(5);
//Display the end positions.
RtPrintf("Circular Additive end positions:\n\n");
GetAGroupPosition(Group);
RtPrintf("------End of the blending moves------\n\n");
}
VOID SetGroupStartingPosition(int Group)
{
//Set the starting positions.
const int LENGTH = 3; //Array length. It indicates how many axes in a group.
/*Remember to change this value when you change the number of the
axes in a group.*/
double StartingPositions[LENGTH] = { 0 };
KsCommandStatus Position = WaitForCommand(5, FALSE, SetGroupPositionOffset(Group, LENGTH,
StartingPositions, FALSE, mcAxisCoordSystem, mcImmediately));
if (Position.Error)
RtPrintf("SetGroupPositionOffset failed: %d\n\n", Position.ErrorId);
//Wait a few cycles for the set position to be set.
Sleep(5);
GetAGroupPosition(Group);
}
VOID GetAGroupPosition(int& Group)
{
const int LENGTH = 3; //Array length. It indicates how many axes in a group.
/*Remember to change this value when you change the number of the
axes in a group.*/
int AxisIndex = 0; //Receive the returned value from GetGroupConfiguration.
int AxisInGroup[LENGTH] = { 0 }; /*The array that presents the indexes of the
axes in a group.*/
int OriginalIndex[LENGTH] = { 0 }; //The original indexes of the axes.
double setPosition[LENGTH] = { 0 }; //The set positions of the axes.
double actualPosition[LENGTH] = { 0 }; //The actual positions of the axes.
KsError nRet = errNoError;
//Get the set position of a group.
nRet = GetGroupPosition(Group, mcAxisCoordSystem, mcSetValue, LENGTH, setPosition);
if (nRet != errNoError)
RtPrintf("Unable to get the set position: %x\n", nRet);
//Get the actual position of a group.
nRet = GetGroupPosition(Group, mcAxisCoordSystem, mcActualValue, LENGTH, actualPosition);
if (nRet != errNoError)
RtPrintf("Unable to get the actual position: %x\n", nRet);
//Initialize AxisInGroup using sequential numbers.
for (int i = 0; i < LENGTH; i++)
AxisInGroup[i] = i;
for (int i = 0; i < LENGTH; i++)
{
//Check the axes in a group, and pass their original index to OriginalIndex.
KsError nRet = GetGroupConfiguration(Group, AxisInGroup[i], mcAxisCoordSystem, &AxisIndex);
OriginalIndex[i] = AxisIndex;
}
//Display the positions of the axes in a group.
for (int i = 0; i < LENGTH; i++)
printf("Axis %d Set position: %f, Actual position: %f\n", OriginalIndex[i],
setPosition[i], actualPosition[i]);
RtPrintf("\n");
}
VOID GetAGroupVelocity(int& Group)
{
const int LENGTH = 3; //Array length. It indicates how many axes in a group.
/*Remember to change this value when you change the number of the
axes in a group.*/
int AxisIndex = 0; //Receive the returned value from GetGroupConfiguration.
int AxisInGroup[LENGTH] = { 0 }; /*The array that presents the indexes of the
axes in a group.*/
int OriginalIndex[LENGTH] = { 0 }; //The original indexes of the axes.
double Velocity[LENGTH] = { 0 }; //The velocity of the axes.
double PathVelocity = 0; //The total amount of the velocity of the axes.
KsError nRet = errNoError;
//Get the velocity and path velocity of a group.
nRet = GetGroupVelocity(Group, mcAxisCoordSystem, mcSetValue, LENGTH, Velocity, &PathVelocity);
if (nRet != errNoError)
RtPrintf("Unable to get the velocity: %x\n", nRet);
//Initialize AxisInGroup using sequential numbers.
for (int i = 0; i < LENGTH; i++)
AxisInGroup[i] = i;
for (int i = 0; i < LENGTH; i++)
{
//Check the axes in a group, and pass their original index to OriginalIndex.
KsError nRet = GetGroupConfiguration(Group, AxisInGroup[i], mcAxisCoordSystem, &AxisIndex);
OriginalIndex[i] = AxisIndex;
}
//Display the positions of the axes in a group.
for (int i = 0; i < LENGTH; i++)
printf("Axis %d Velocity: %f\n", OriginalIndex[i],
Velocity[i]);
RtPrintf("\n");
printf("Path velocity is %f\n\n", PathVelocity);
}