public override byte[] GetBytesForNextAction(RobotCursor cursor)
{
if (!cursor.ApplyNextAction(out _action))
{
return(null); // cursor buffer is empty
}
_params = null;
switch (_action.Type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
RotationVector rv = cursor.rotation.AA.ToRotationVector();
_params = new int[]
{
_action.Id,
cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEL,
(int)Math.Round(cursor.position.X * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Y * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Z * 0.001 * FACTOR_M),
(int)Math.Round(rv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Z * Geometry.TO_RADS * FACTOR_RAD)
};
// Workaround to SW3.0 crash problem: https://github.com/RobotExMachina/Machina.NET/issues/7
if (ZERO_PRECISION_ON_SAME_POSITION_MOTION && cursor.precision != 0 && cursor.prevPosition != null && cursor.position.IsSimilar(cursor.prevPosition))
{
Logger.Debug("Applying ZERO_PRECISION_ON_SAME_POSITION_MOTION");
_params = WrapParamsWithZeroPrecision(_params, cursor);
}
break;
case ActionType.Axes:
_params = new int[]
{
_action.Id,
INST_MOVEJ_Q,
(int)Math.Round(cursor.axes.J1 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J2 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J3 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J4 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J5 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.axes.J6 * Geometry.TO_RADS * FACTOR_RAD),
};
// Another ZERO_PRECISION_ON_SAME_POSITION_MOTION check should happen here for 6th axis rotation only...
break;
case ActionType.Wait:
ActionWait aa = _action as ActionWait;
_params = new int[]
{
_action.Id,
INST_SLEEP,
(int)Math.Round(aa.millis * 0.001 * FACTOR_SEC)
};
break;
// Not implemented yet
//case ActionType.Message:
// ActionMessage am = (ActionMessage)action;
// dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
// am.message);
// break;
case ActionType.AttachTool:
//ActionAttachTool aatt = _action as ActionAttachTool;
//Tool t = aatt.tool;
Tool t = cursor.tool; // can cursor.tool be null? The action would have not gone through if there wasn't a tool available for attachment, right?
RotationVector trv = t.TCPOrientation.ToRotationVector();
_params = new int[]
{
_action.Id,
INST_SET_TOOL,
(int)Math.Round(t.TCPPosition.X * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Y * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Z * 0.001 * FACTOR_M),
(int)Math.Round(trv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Z * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(t.Weight * FACTOR_KG)
};
break;
case ActionType.DetachTool:
_params = new int[]
{
_action.Id,
INST_SET_TOOL,
0,
0,
0,
0,
0,
0,
0
};
break;
case ActionType.IODigital:
ActionIODigital aiod = _action as ActionIODigital;
_params = new int[]
{
_action.Id,
INST_SET_DIGITAL_OUT,
aiod.pinNum,
aiod.on ? 1 : 0,
aiod.isToolPin ? 1 : 0
};
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = _action as ActionIOAnalog;
_params = new int[]
{
_action.Id,
INST_SET_ANALOG_OUT,
aioa.pinNum,
(int)Math.Round(aioa.value * FACTOR_VOLT)
//aioa.isToolPin ? 1 : 0 // there is no analog out on the tool
};
break;
// ╔═╗╔═╗╔╦╗╔╦╗╦╔╗╔╔═╗╔═╗
// ╚═╗║╣ ║ ║ ║║║║║ ╦╚═╗
// ╚═╝╚═╝ ╩ ╩ ╩╝╚╝╚═╝╚═╝
// Speed is now set globally, and the driver takes care of translating that internally
case ActionType.Speed:
_params = new int[]
{
_action.Id,
INST_ALL_SPEED,
(int)Math.Round(cursor.speed * 0.001 * FACTOR_M)
};
break;
// Idem for acceleration
case ActionType.Acceleration:
_params = new int[]
{
_action.Id,
INST_ALL_ACC,
(int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)
};
break;
case ActionType.Precision:
_params = new int[]
{
_action.Id,
INST_BLEND,
(int)Math.Round(cursor.precision * 0.001 * FACTOR_M)
};
break;
case ActionType.PushPop:
ActionPushPop app = _action as ActionPushPop;
if (app.push)
{
break;
}
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
Dictionary <int, int> poppedSettings = new Dictionary <int, int>();
// These are the states kept in the controller as of v1.0 of the driver
//if (beforePop.Speed != cursor.speed)
// poppedSettings.Add(INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M));
//if (beforePop.Acceleration != cursor.acceleration)
// poppedSettings.Add(INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M));
//if (beforePop.JointSpeed != cursor.jointSpeed)
// poppedSettings.Add(INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD));
//if (beforePop.JointAcceleration != cursor.jointAcceleration)
// poppedSettings.Add(INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD));
// These are the states kept in the controller as of v1.0 of the driver
if (beforePop.Speed != cursor.speed)
{
poppedSettings.Add(INST_ALL_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M));
}
if (beforePop.Acceleration != cursor.acceleration)
{
poppedSettings.Add(INST_ALL_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M));
}
if (beforePop.Precision != cursor.precision)
{
poppedSettings.Add(INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M));
}
// Generate a buffer with all instructions, ids of -1 except for the last one.
_params = new int[3 * poppedSettings.Count];
int it = 0;
foreach (var setting in poppedSettings)
{
_params[3 * it] = it == poppedSettings.Count - 1 ? app.Id : -1; // only attach the real id to the last instruction
_params[3 * it + 1] = setting.Key;
_params[3 * it + 2] = setting.Value;
it++;
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
//// Send comma-separated integers
//case ActionType.CustomCode:
// ActionCustomCode acc = _action as ActionCustomCode;
// int[] values;
// if (Numeric.CommaSeparatedStringToInts(out values))
// {
// }
// else
// {
// Logger.Warning("Invalid CustomCode: please use a string of comma-separated integers, like \"1,);
// }
// break;
// If the Action wasn't on the list above, it doesn't have a message representation...
default:
Logger.Verbose("Cannot stream action `" + _action + "`");
return(null);
}
if (_params == null)
{
return(null);
}
_buffer = Utilities.Conversion.Int32ArrayToByteArray(_params, false);
return(_buffer);
}
public override byte[] GetBytesForNextAction(RobotCursor cursor)
{
if (!cursor.ApplyNextAction(out _action))
{
return(null); // cursor buffer is empty
}
_params = null;
switch (_action.type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
RotationVector rv = cursor.rotation.AA.ToRotationVector();
_params = new int[]
{
_action.id,
cursor.motionType == MotionType.Joint ? INST_MOVEJ_P : INST_MOVEL,
(int)Math.Round(cursor.position.X * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Y * 0.001 * FACTOR_M),
(int)Math.Round(cursor.position.Z * 0.001 * FACTOR_M),
(int)Math.Round(rv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(rv.Z * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.Axes:
_params = new int[]
{
_action.id,
INST_MOVEJ_Q,
(int)Math.Round(cursor.joints.J1 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J2 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J3 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J4 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J5 * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(cursor.joints.J6 * Geometry.TO_RADS * FACTOR_RAD),
};
break;
case ActionType.Wait:
ActionWait aa = _action as ActionWait;
_params = new int[]
{
_action.id,
INST_SLEEP,
(int)Math.Round(aa.millis * 0.001 * FACTOR_SEC)
};
break;
// Not implemented yet
//case ActionType.Message:
// ActionMessage am = (ActionMessage)action;
// dec = string.Format(" popup(\"{0}\", title=\"Machina Message\", warning=False, error=False)",
// am.message);
// break;
case ActionType.Attach:
ActionAttach aatt = _action as ActionAttach;
Tool t = aatt.tool;
RotationVector trv = t.TCPOrientation.ToRotationVector();
_params = new int[]
{
_action.id,
INST_SET_TOOL,
(int)Math.Round(t.TCPPosition.X * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Y * 0.001 * FACTOR_M),
(int)Math.Round(t.TCPPosition.Z * 0.001 * FACTOR_M),
(int)Math.Round(trv.X * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Y * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(trv.Z * Geometry.TO_RADS * FACTOR_RAD),
(int)Math.Round(t.Weight * FACTOR_M)
};
break;
case ActionType.Detach:
_params = new int[]
{
_action.id,
INST_SET_TOOL,
0,
0,
0,
0,
0,
0,
0
};
break;
case ActionType.IODigital:
ActionIODigital aiod = _action as ActionIODigital;
_params = new int[]
{
_action.id,
INST_SET_DIGITAL_OUT,
aiod.pinNum,
aiod.on ? 1 : 0
};
break;
case ActionType.IOAnalog:
ActionIOAnalog aioa = _action as ActionIOAnalog;
_params = new int[]
{
_action.id,
INST_SET_DIGITAL_OUT,
aioa.pinNum,
(int)Math.Round(aioa.value * FACTOR_VOLT)
};
break;
// ╔═╗╔═╗╔╦╗╔╦╗╦╔╗╔╔═╗╔═╗
// ╚═╗║╣ ║ ║ ║║║║║ ╦╚═╗
// ╚═╝╚═╝ ╩ ╩ ╩╝╚╝╚═╝╚═╝
case ActionType.Speed:
_params = new int[]
{
_action.id,
INST_TCP_SPEED,
(int)Math.Round(cursor.speed * 0.001 * FACTOR_M)
};
break;
case ActionType.Acceleration:
_params = new int[]
{
_action.id,
INST_TCP_ACC,
(int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M)
};
break;
case ActionType.JointSpeed:
_params = new int[]
{
_action.id,
INST_Q_SPEED,
(int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.JointAcceleration:
_params = new int[]
{
_action.id,
INST_Q_ACC,
(int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD)
};
break;
case ActionType.Precision:
_params = new int[]
{
_action.id,
INST_BLEND,
(int)Math.Round(cursor.precision * 0.001 * FACTOR_M)
};
break;
case ActionType.PushPop:
ActionPushPop app = _action as ActionPushPop;
if (app.push)
{
break;
}
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
Dictionary <int, int> poppedSettings = new Dictionary <int, int>();
// These are the states kept in the controller as of v1.0 of the driver
if (beforePop.Speed != cursor.speed)
{
poppedSettings.Add(INST_TCP_SPEED, (int)Math.Round(cursor.speed * 0.001 * FACTOR_M));
}
if (beforePop.Acceleration != cursor.acceleration)
{
poppedSettings.Add(INST_TCP_ACC, (int)Math.Round(cursor.acceleration * 0.001 * FACTOR_M));
}
if (beforePop.JointSpeed != cursor.jointSpeed)
{
poppedSettings.Add(INST_Q_SPEED, (int)Math.Round(cursor.jointSpeed * Geometry.TO_RADS * FACTOR_RAD));
}
if (beforePop.JointAcceleration != cursor.jointAcceleration)
{
poppedSettings.Add(INST_Q_ACC, (int)Math.Round(cursor.jointAcceleration * Geometry.TO_RADS * FACTOR_RAD));
}
if (beforePop.Precision != cursor.precision)
{
poppedSettings.Add(INST_BLEND, (int)Math.Round(cursor.precision * 0.001 * FACTOR_M));
}
// Generate a buffer with all instructions, ids of -1 except for the last one.
_params = new int[3 * poppedSettings.Count];
int it = 0;
foreach (var setting in poppedSettings)
{
_params[3 * it] = it == poppedSettings.Count - 1 ? app.id : -1; // only attach the real id to the last instruction
_params[3 * it + 1] = setting.Key;
_params[3 * it + 2] = setting.Value;
it++;
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
}
if (_params == null)
{
return(null);
}
_buffer = Util.Int32ArrayToByteArray(_params, false);
return(_buffer);
}
internal const char STR_MESSAGE_RESPONSE_CHAR = '$'; // Flags a message as a response to an information request(acknowledgments do not include it)
/// <summary>
/// Given an Action and a RobotCursor representing the state of the robot after application,
/// return a List of strings with the messages necessary to perform this Action adhering to
/// the Machina-ABB-Server protocol.
/// </summary>
/// <param name="action"></param>
/// <param name="cursor"></param>
/// <returns></returns>
internal override List <string> GetActionMessages(Action action, RobotCursor cursor)
{
List <string> msgs = new List <string>();
switch (action.Type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
//// MoveL/J X Y Z QW QX QY QZ
msgs.Add(string.Format(CultureInfo.InvariantCulture,
"{0}{1} {2} {3} {4} {5} {6} {7} {8} {9}{10}",
STR_MESSAGE_ID_CHAR,
action.Id,
cursor.motionType == MotionType.Linear ? INST_MOVEL : INST_MOVEJ,
Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.rotation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Axes:
// MoveAbsJ J1 J2 J3 J4 J5 J6
msgs.Add(string.Format(CultureInfo.InvariantCulture,
"{0}{1} {2} {3} {4} {5} {6} {7} {8}{9}",
STR_MESSAGE_ID_CHAR,
action.Id,
INST_MOVEABSJ,
Math.Round(cursor.axes.J1, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.axes.J2, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.axes.J3, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.axes.J4, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.axes.J5, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.axes.J6, Geometry.STRING_ROUND_DECIMALS_DEGS),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Speed:
// (setspeed V_TCP[V_ORI V_LEAX V_REAX])
msgs.Add(Invariant($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}")); // this accepts more velocity params, but those are still not implemented in Machina...
break;
case ActionType.Acceleration:
Logger.Debug("Acceleration not implemented in ABBCommunicationProtocol");
break;
case ActionType.Precision:
// (setzone FINE TCP[ORI EAX ORI LEAX REAX])
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}"); // this accepts more zone params, but those are still not implemented in Machina...
break;
case ActionType.Wait:
// !WaitTime T
ActionWait aw = (ActionWait)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_WAITTIME} {0.001 * aw.millis}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.Message:
// !TPWrite "MSG"
ActionMessage am = (ActionMessage)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_TPWRITE} \"{am.message}\"{STR_MESSAGE_END_CHAR}");
break;
case ActionType.AttachTool:
// !(settool X Y Z QW QX QY QZ KG CX CY CZ)
//ActionAttachTool aa = (ActionAttachTool)action;
//Tool t = aa.tool;
Tool t = cursor.tool; // @TODO: should I just pull from the library? need to rethink the general approach: take info from cursor state (like motion actions) or action data...
msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13}{14}",
STR_MESSAGE_ID_CHAR,
action.Id,
INST_TOOL,
Math.Round(t.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPOrientation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.Weight, Geometry.STRING_ROUND_DECIMALS_KG),
Math.Round(t.CenterOfGravity.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.CenterOfGravity.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.CenterOfGravity.Z, Geometry.STRING_ROUND_DECIMALS_MM),
STR_MESSAGE_END_CHAR));
break;
case ActionType.DetachTool:
// !(settool0)
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_NOTOOL}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.IODigital:
// !SetDO "NAME" ON
ActionIODigital aiod = (ActionIODigital)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SETDO} \"{aiod.pinName}\" {(aiod.on ? 1 : 0)}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.IOAnalog:
// !SetAO "NAME" V
ActionIOAnalog aioa = (ActionIOAnalog)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SETAO} \"{aioa.pinName}\" {aioa.value}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.PushPop:
ActionPushPop app = action as ActionPushPop;
if (app.push)
{
return(null);
}
else
{
// Only precision, speed and acceleration are states kept on the controller
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
if (beforePop.Speed != cursor.speed)
{
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}");
}
if (beforePop.Acceleration != cursor.acceleration)
{
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_ACCELERATION} {cursor.acceleration}{STR_MESSAGE_END_CHAR}");
}
if (beforePop.Precision != cursor.precision)
{
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.Id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}");
}
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
case ActionType.ExternalAxis:
string msg, @params, id;
ActionExternalAxis aea = action as ActionExternalAxis;
// Cartesian msg
if (aea.target == ExternalAxesTarget.All || aea.target == ExternalAxesTarget.Cartesian)
{
if (aea.target == ExternalAxesTarget.All)
{
id = "0"; // If will need to send two messages, only add real id to the last one.
}
else
{
id = aea.Id.ToString();
}
@params = ExternalAxesToParameters(cursor.externalAxesCartesian);
msg = $"{STR_MESSAGE_ID_CHAR}{id} {INST_EXT_JOINTS_ROBTARGET} {@params}{STR_MESSAGE_END_CHAR}";
msgs.Add(msg);
}
// Joints msg
if (aea.target == ExternalAxesTarget.All || aea.target == ExternalAxesTarget.Joint)
{
id = aea.Id.ToString(); // now use the real id in any case
@params = ExternalAxesToParameters(cursor.externalAxesJoints);
msg = $"{STR_MESSAGE_ID_CHAR}{id} {INST_EXT_JOINTS_JOINTTARGET} {@params}{STR_MESSAGE_END_CHAR}";
msgs.Add(msg);
}
break;
case ActionType.ArmAngle:
// Send a request to change only the robtarget portion of the external axes for next motion.
ActionArmAngle aaa = action as ActionArmAngle;
msgs.Add(string.Format("{0}{1} {2} {3} 9E9 9E9 9E9 9E9 9E9{4}",
STR_MESSAGE_ID_CHAR,
action.Id,
INST_EXT_JOINTS_ROBTARGET,
Math.Round(aaa.angle, Geometry.STRING_ROUND_DECIMALS_DEGS),
STR_MESSAGE_END_CHAR));
break;
// When connected live, this is used as a way to stream a message directly to the robot. Unsafe, but oh well...
case ActionType.CustomCode:
ActionCustomCode acc = action as ActionCustomCode;
msgs.Add(acc.statement);
break;
// If the Action wasn't on the list above, it doesn't have a message representation...
default:
Logger.Warning("Cannot stream action `" + action + "`");
return(null);
}
return(msgs);
}
internal const int RES_JOINTS = 22; // ">22 0 0 0 0 90 0;"
// const int RES_EXTAX = 23;
/// <summary>
/// Given an Action and a RobotCursor representing the state of the robot after application,
/// return a List of strings with the messages necessary to perform this Action adhering to
/// the Machina-ABB-Server protocol.
/// </summary>
/// <param name="action"></param>
/// <param name="cursor"></param>
/// <returns></returns>
internal override List <string> GetActionMessages(Action action, RobotCursor cursor)
{
List <string> msgs = new List <string>();
switch (action.type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
//// MoveL/J X Y Z QW QX QY QZ
msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9}{10}",
STR_MESSAGE_ID_CHAR,
action.id,
cursor.motionType == MotionType.Linear ? INST_MOVEL : INST_MOVEJ,
Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.rotation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(cursor.rotation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Axes:
// MoveAbsJ J1 J2 J3 J4 J5 J6
msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8}{9}",
STR_MESSAGE_ID_CHAR,
action.id,
INST_MOVEABSJ,
Math.Round(cursor.joints.J1, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J2, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J3, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J4, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J5, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J6, Geometry.STRING_ROUND_DECIMALS_DEGS),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Speed:
// (setspeed V_TCP[V_ORI V_LEAX V_REAX])
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}"); // this accepts more velocity params, but those are still not implemented in Machina...
break;
case ActionType.Precision:
// (setzone FINE TCP[ORI EAX ORI LEAX REAX])
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}"); // this accepts more zone params, but those are still not implemented in Machina...
break;
case ActionType.Wait:
// !WaitTime T
ActionWait aw = (ActionWait)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_WAITTIME} {0.001 * aw.millis}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.Message:
// !TPWrite "MSG"
ActionMessage am = (ActionMessage)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_TPWRITE} \"{am.message}\"{STR_MESSAGE_END_CHAR}");
break;
case ActionType.Attach:
// !(settool X Y Z QW QX QY QZ KG CX CY CZ)
ActionAttach aa = (ActionAttach)action;
Tool t = aa.tool;
msgs.Add(string.Format("{0}{1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12} {13}{14}",
STR_MESSAGE_ID_CHAR,
action.id,
INST_TOOL,
Math.Round(t.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.TCPOrientation.Q.W, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.X, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.Y, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.TCPOrientation.Q.Z, Geometry.STRING_ROUND_DECIMALS_QUAT),
Math.Round(t.Weight, Geometry.STRING_ROUND_DECIMALS_KG),
Math.Round(t.centerOfGravity.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.centerOfGravity.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(t.centerOfGravity.Z, Geometry.STRING_ROUND_DECIMALS_MM),
STR_MESSAGE_END_CHAR));
break;
case ActionType.Detach:
// !(settool0)
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_NOTOOL}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.IODigital:
// !SetDO "NAME" ON
ActionIODigital aiod = (ActionIODigital)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SETDO} \"{aiod.pinName}\" {(aiod.on ? 1 : 0)}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.IOAnalog:
// !SetAO "NAME" V
ActionIOAnalog aioa = (ActionIOAnalog)action;
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SETAO} \"{aioa.pinName}\" {aioa.value}{STR_MESSAGE_END_CHAR}");
break;
case ActionType.PushPop:
ActionPushPop app = action as ActionPushPop;
if (app.push)
{
return(null);
}
else
{
// Only precision and speed are states kept on the controller
Settings beforePop = cursor.settingsBuffer.SettingsBeforeLastPop;
if (beforePop.Speed != cursor.speed)
{
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_SPEED} {cursor.speed}{STR_MESSAGE_END_CHAR}");
}
if (beforePop.Precision != cursor.precision)
{
msgs.Add($"{STR_MESSAGE_ID_CHAR}{action.id} {INST_ZONE} {cursor.precision}{STR_MESSAGE_END_CHAR}");
}
}
break;
case ActionType.Coordinates:
throw new NotImplementedException(); // @TODO: this should also change the WObj, but not on it yet...
case ActionType.ExternalAxis:
string msg = $"{STR_MESSAGE_ID_CHAR}{action.id} {INST_EXT_JOINTS} ";
for (int i = 0; i < cursor.externalAxes.Length; i++)
{
// RAPID's StrToVal() will parse 9E9 into a 9E+9 num value, and ignore that axis on motions
msg += cursor.externalAxes[i]?.ToString() ?? "9E9";
if (i < cursor.externalAxes.Length - 1)
{
msg += " ";
}
}
msg += STR_MESSAGE_END_CHAR;
msgs.Add(msg);
break;
// CustomCode --> is non-streamable
// If the Action wasn't on the list above, it doesn't have a message representation...
default:
return(null);
}
return(msgs);
}
