internal bool GenerateInstructionDeclaration(
Action action,
RobotCursor cursor,
Dictionary <double, string> velNames,
Dictionary <double, string> zoneNames,
Dictionary <Tool, string> toolNames,
out string declaration)
{
string dec = null;
switch (action.type)
{
case ActionType.Acceleration:
bool zero = cursor.acceleration < Geometry.EPSILON2;
dec = string.Format(" WorldAccLim {0};",
zero ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M));
break;
case ActionType.JointSpeed:
case ActionType.JointAcceleration:
dec = string.Format(" {0} WARNING: {1}() has no effect in ABB robots.",
commChar,
action.type);
break;
// @TODO: push/pop management should be done PROGRAMMATICALLY, not this CHAPUZa...
case ActionType.PushPop:
// Find if there was a change in acceleration, and set the corresponsing instruction...
ActionPushPop app = action as ActionPushPop;
if (app.push)
{
break; // only necessary for pops
}
if (Math.Abs(cursor.acceleration - cursor.settingsBuffer.SettingsBeforeLastPop.Acceleration) < Geometry.EPSILON2)
{
break; // no change
}
// If here, there was a change, so...
bool zeroAcc = cursor.acceleration < Geometry.EPSILON2;
dec = string.Format(" WorldAccLim {0};",
zeroAcc ? "\\Off" : "\\On := " + Math.Round(0.001 * cursor.acceleration, Geometry.STRING_ROUND_DECIMALS_M));
break;
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
dec = string.Format(" {0} {1}, {2}, {3}, {4}\\{5};",
cursor.motionType == MotionType.Joint ? "MoveJ" : "MoveL",
GetUNSAFERobTargetValue(cursor),
velNames[cursor.speed],
zoneNames[cursor.precision],
cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
"WObj:=WObj0");
break;
case ActionType.Axes:
dec = string.Format(" MoveAbsJ {0}, {1}, {2}, {3}\\{4};",
GetJointTargetValue(cursor),
velNames[cursor.speed],
zoneNames[cursor.precision],
cursor.tool == null ? "Tool0" : toolNames[cursor.tool],
"WObj:=WObj0");
break;
case ActionType.Message:
ActionMessage am = (ActionMessage)action;
dec = string.Format(" TPWrite \"{0}\";",
am.message.Length <= 80 ?
am.message :
am.message.Substring(0, 80)); // ABB TPWrite messages can only be 80 chars long
break;
case ActionType.Wait:
ActionWait aw = (ActionWait)action;
dec = string.Format(" WaitTime {0};",
0.001 * aw.millis);
break;
case ActionType.Comment:
ActionComment ac = (ActionComment)action;
dec = string.Format(" {0} {1}",
commChar,
ac.comment);
break;
case ActionType.Attach:
ActionAttach aa = (ActionAttach)action;
dec = string.Format(" {0} Tool \"{1}\" attached", // this action has no actual RAPID instruction, just add a comment
commChar,
aa.tool.name);
break;
case ActionType.Detach:
ActionDetach ad = (ActionDetach)action;
dec = string.Format(" {0} Tools detached", // this action has no actual RAPID instruction, just add a comment
commChar);
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (aiod.pin < 0 || aiod.pin >= cursor.digitalOutputs.Length)
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
commChar,
aiod.ToString());
}
else
{
dec = string.Format(" SetDO {0}, {1};",
cursor.digitalOutputNames[aiod.pin],
aiod.on ? "1" : "0");
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (aioa.pin < 0 || aioa.pin >= cursor.analogOutputs.Length)
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
commChar,
aioa.ToString());
}
else
{
dec = string.Format(" SetAO {0}, {1};",
cursor.analogOutputNames[aioa.pin],
aioa.value);
}
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} [{3}]",
dec,
dec == null ? " " : "", // add indentation to align with code
commChar,
action.ToString());
}
else if (ADD_ACTION_ID)
{
dec = string.Format("{0}{1} {2} [{3}]",
dec,
dec == null ? " " : "", // add indentation to align with code
commChar,
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(" $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(" $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(" 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.Attach:
ActionAttach at = (ActionAttach)action;
dec = string.Format(" $TOOL = {0}",
GetToolValue(cursor));
break;
case ActionType.Detach:
ActionDetach ad = (ActionDetach)action;
dec = string.Format(" $TOOL = $NULLFRAME");
break;
case ActionType.IODigital:
ActionIODigital aiod = (ActionIODigital)action;
if (aiod.pin < 1 || aiod.pin >= cursor.digitalOutputs.Length) // KUKA starts counting pins by 1
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
commChar,
aiod.ToString());
}
else
{
dec = string.Format(" $OUT[{0}] = {1}",
aiod.pin,
aiod.on ? "TRUE" : "FALSE");
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (aioa.pin < 1 || aioa.pin >= cursor.analogOutputNames.Length || aioa.pin > 16) // KUKA: analog pins [1 to 16]
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
commChar,
aioa.ToString());
}
else if (aioa.value < -1 || aioa.value > 1)
{
dec = string.Format(" {0} ERROR on \"{1}\": value out of range [-1.0, 1.0]",
commChar,
aioa.ToString());
}
else
{
dec = string.Format(" $ANOUT[{0}] = {1}",
aioa.pin,
Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE));
}
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);
}
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.pin < 0 || aiod.pin >= cursor.digitalOutputs.Length)
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
COMMENT_CHAR,
aiod.ToString());
}
else if (aiod.pin > 7)
{
dec = string.Format(" {0} ERROR on \"{1}\": digital IO pin not available in UR robot",
COMMENT_CHAR,
aiod.ToString());
}
else
{
dec = string.Format(" set_standard_digital_out({0}, {1})",
aiod.pin,
aiod.on ? "True" : "False");
}
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = (ActionIOAnalog)action;
if (aioa.pin < 0 || aioa.pin >= cursor.analogOutputs.Length)
{
dec = string.Format(" {0} ERROR on \"{1}\": IO number not available",
COMMENT_CHAR,
aioa.ToString());
}
else if (aioa.pin > 1)
{
dec = string.Format(" {0} ERROR on \"{1}\": analog IO pin not available in UR robot",
COMMENT_CHAR,
aioa.ToString());
}
else if (aioa.value < 0 || aioa.value > 1)
{
dec = string.Format(" {0} ERROR on \"{1}\": value out of range [0.0, 1.0]",
COMMENT_CHAR,
aioa.ToString());
}
else
{
dec = string.Format(" set_standard_analog_out({0}, {1})",
aioa.pin,
Math.Round(aioa.value, Geometry.STRING_ROUND_DECIMALS_VOLTAGE));
}
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);
}
