internal static bool GenerateInstructionDeclaration(
Action action, RobotCursor cursor, bool humanComments,
out string declaration)
{
string dec = null;
switch (action.type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
// Accelerations and velocoties have different meaning for moveJ and moveL instructions.
// Joint motion is essentially the same as Axes motion, just the input is a pose instead of a joints vector.
if (cursor.motionType == MotionType.Joint)
{
dec = string.Format(" movej({0}, a={1}, v={2}, r={3})",
GetPoseTargetValue(cursor),
cursor.jointAcceleration > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
cursor.jointSpeed > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
}
else
{
dec = string.Format(" movel({0}, a={1}, v={2}, r={3})",
GetPoseTargetValue(cursor),
cursor.acceleration > Geometry.EPSILON2 ? Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_ACCELERATION,
cursor.speed > Geometry.EPSILON2 ? Math.Round(0.001 * cursor.speed, Geometry.STRING_ROUND_DECIMALS_M) : DEFAULT_TOOL_SPEED,
Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
}
break;
case ActionType.RotationSpeed:
dec = string.Format(" {0} WARNING: RotationSpeed() has no effect in UR robots, try JointSpeed() or JointAcceleration() instead", COMMENT_CHAR);
break;
case ActionType.Axes:
// HAL generates a "set_tcp(p[0,0,0,0,0,0])" call here which I find confusing...
dec = string.Format(" movej({0}, a={1}, v={2}, r={3})",
GetJointTargetValue(cursor),
cursor.jointAcceleration > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointAcceleration, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_ACCELERATION,
cursor.jointSpeed > Geometry.EPSILON2 ? Math.Round(Geometry.TO_RADS * cursor.jointSpeed, Geometry.STRING_ROUND_DECIMALS_RADS) : DEFAULT_JOINT_SPEED,
Math.Round(0.001 * cursor.precision, Geometry.STRING_ROUND_DECIMALS_M));
break;
case ActionType.Message:
ActionMessage am = (ActionMessage)action;
dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
am.message);
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(" sleep({0})",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
COMMENT_CHAR,
ac.comment);
break;
case ActionType.Attach:
ActionAttach aa = (ActionAttach)action;
dec = string.Format(" set_tcp({0})", // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
GetToolValue(cursor));
break;
case ActionType.Detach:
ActionDetach ad = (ActionDetach)action;
dec = string.Format(" set_tcp(p[0,0,0,0,0,0])"); // @TODO: should need to add a "set_payload(m, CoG)" dec afterwards...
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (!aiod.isDigit)
{
dec = $" {COMMENT_CHAR} ERROR on \"{aiod}\": only integer pin names are possible";
}
else if (aiod.pinNum < 0 || aiod.pinNum > 7)
{
dec = $" {COMMENT_CHAR} ERROR on \"{aiod}\": IO number not available";
}
else
{
dec = $" set_{(aiod.isToolPin ? "tool" : "standard")}_digital_out({aiod.pinNum}, {(aiod.on ? "True" : "False")})";
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (!aioa.isDigit)
{
dec = $" {COMMENT_CHAR} ERROR on \"{aioa}\": only integer pin names are possible";
}
else if (aioa.pinNum < 0 || aioa.pinNum > 1)
{
dec = $" {COMMENT_CHAR} ERROR on \"{aioa}\": IO number not available";
}
else if (aioa.value < 0 || aioa.value > 1)
{
dec = $" {COMMENT_CHAR} ERROR on \"{aioa}\": value out of range [0.0, 1.0]";
}
else
{
dec = $" set_{(aioa.isToolPin ? "tool" : "standard")}_analog_out({aioa.pinNum}, {Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE)})";
}
break;
case ActionType.CustomCode:
ActionCustomCode acc = action as ActionCustomCode;
if (!acc.isDeclaration)
{
dec = $" {acc.statement}";
}
break;
//default:
// dec = string.Format(" # ACTION \"{0}\" NOT IMPLEMENTED", action);
// break;
}
if (humanComments && action.type != ActionType.Comment)
{
dec = string.Format("{0} {1} [{2}]",
dec,
COMMENT_CHAR,
action.ToString());
}
//else if (ADD_ACTION_ID)
//{
// dec = string.Format("{0} {1} [{2}]",
// dec,
// COMMENT_CHAR,
// action.id);
//}
declaration = dec;
return(dec != null);
}
internal bool GenerateInstructionDeclaration(
Action action, RobotCursor cursor,
out string declaration)
{
string dec = null;
switch (action.Type)
{
// KUKA does explicit setting of velocities and approximate positioning, so these actions make sense as instructions
case ActionType.Speed:
dec = string.Format(CultureInfo.InvariantCulture,
" $VEL = {{CP {0}, ORI1 100, ORI2 100}}",
Math.Round(0.001 * cursor.speed, 3 + Geometry.STRING_ROUND_DECIMALS_MM));
break;
case ActionType.Precision:
dec = string.Format(CultureInfo.InvariantCulture,
" $APO.CDIS = {0}",
cursor.precision);
break;
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
dec = string.Format(" {0} {1} {2}",
cursor.motionType == MotionType.Joint ? "PTP" : "LIN",
GetPositionTargetValue(cursor),
cursor.precision >= 1 ? "C_DIS" : "");
break;
case ActionType.Axes:
dec = string.Format(" {0} {1} {2}",
"PTP",
GetAxisTargetValue(cursor),
cursor.precision >= 1 ? "C_DIS" : ""); // @TODO: figure out how to turn this into C_PTP
break;
// @TODO: apparently, messages in KRL are kind fo tricky, with several manuals just dedicated to it.
// Will figure this out later.
case ActionType.Message:
ActionMessage am = (ActionMessage)action;
dec = string.Format(" {0} MESSAGE: \"{1}\" (messages in KRL currently not supported in Machina)",
commChar,
am.message);
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(CultureInfo.InvariantCulture,
" WAIT SEC {0}",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
commChar,
ac.comment);
break;
case ActionType.DefineTool:
ActionDefineTool adt = action as ActionDefineTool;
dec = string.Format(" {0} Tool \"{1}\" defined", // this action has no actual instruction, just add a comment
COMMENT_CHAR,
adt.tool.name);
break;
case ActionType.AttachTool:
ActionAttachTool at = (ActionAttachTool)action;
dec = string.Format(" $TOOL = {0}",
GetToolValue(cursor));
break;
case ActionType.DetachTool:
ActionDetachTool ad = (ActionDetachTool)action;
dec = string.Format(" $TOOL = $NULLFRAME");
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (!aiod.isDigit)
{
dec = $" {commChar} ERROR on \"{aiod}\": only integer pin names are possible";
}
else if (aiod.pinNum < 1 || aiod.pinNum > 32) // KUKA starts counting pins by 1
{
dec = $" {commChar} ERROR on \"{aiod}\": IO number not available";
}
else
{
dec = $" $OUT[{aiod.pinNum}] = {(aiod.on ? "TRUE" : "FALSE")}";
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (!aioa.isDigit)
{
dec = $" {commChar} ERROR on \"{aioa}\": only integer pin names are possible";
}
else if (aioa.pinNum < 1 || aioa.pinNum > 16) // KUKA: analog pins [1 to 16]
{
dec = $" {commChar} ERROR on \"{aioa}\": IO number not available";
}
else if (aioa.value < -1 || aioa.value > 1)
{
dec = $" {commChar} ERROR on \"{aioa}\": value out of range [-1.0, 1.0]";
}
else
{
//dec = $" $ANOUT[{aioa.pinNum}] = {Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE)}";
dec = string.Format(CultureInfo.InvariantCulture,
" $ANOUT[{0}] = {1}",
aioa.pinNum,
Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE));
}
break;
case ActionType.CustomCode:
ActionCustomCode acc = action as ActionCustomCode;
if (!acc.isDeclaration)
{
dec = $" {acc.statement}";
}
break;
//default:
// dec = string.Format(" ; ACTION \"{0}\" NOT IMPLEMENTED", action);
// break;
}
if (ADD_ACTION_STRING && action.Type != ActionType.Comment)
{
dec = string.Format("{0} {1} [{2}]",
dec,
commChar,
action.ToString());
}
else if (ADD_ACTION_ID)
{
dec = string.Format("{0} {1} [{2}]",
dec,
commChar,
action.Id);
}
declaration = dec;
return(dec != null);
}
