internal static string ExternalAxesToParameters(ExternalAxes extax)
{
string param = "";
for (int i = 0; i < extax.Length; i++)
{
if (extax[i] == null)
{
// RAPID's StrToVal() will parse 9E9 into a 9E+9 num value, and ignore that axis on motions
param += "9E9";
}
else
{
param += Math.Round((double)extax[i], Geometry.STRING_ROUND_DECIMALS_MM)
.ToString(CultureInfo.InvariantCulture);
}
if (i < extax.Length - 1)
{
param += " ";
}
}
return(param);
}
internal void UpdateRobotStatus()
{
if (bot == null)
{
ResetRobotStatus();
return;
}
uiContext.Post(x =>
{
MVector pos = bot.GetCurrentPosition();
string posStr = pos?.ToString(true) ?? "-";
lbl_Status_TCP_Position_Value.Content = posStr;
MOrientation ori = bot.GetCurrentRotation();
string oriStr = ori?.ToString(true) ?? "-";
lbl_Status_TCP_Orientation_Value.Content = oriStr;
Joints axes = bot.GetCurrentAxes();
string axesStr = axes?.ToString(true) ?? "-";
lbl_Status_Axes_Value.Content = axesStr;
ExternalAxes extax = bot.GetCurrentExternalAxes();
bool nullext = true;
if (extax != null)
{
for (int i = 0; i < 6; i++)
{
if (extax[i] != null)
{
nullext = false;
break;
}
}
}
lbl_Status_Ext_Axes_Value.Content = nullext ? "-" : extax.ToString(true);
double speed = bot.GetCurrentSpeed();
double acc = bot.GetCurrentAcceleration();
string speedacc = Math.Round(speed, MMath.STRING_ROUND_DECIMALS_MM) + " mm/s / " + Math.Round(acc, MMath.STRING_ROUND_DECIMALS_MM) + " mm/s^2";
lbl_Status_SpeedAcceleration_Value.Content = speedacc;
double precision = bot.GetCurrentPrecision();
lbl_Status_Precision_Value.Content =
Math.Round(precision, MMath.STRING_ROUND_DECIMALS_MM) + " mm";
MotionType mtype = bot.GetCurrentMotionMode();
lbl_Status_MotionMode_Value.Content = mtype.ToString();
lbl_Status_Tool_Value.Content = bot.GetCurrentTool()?.name ?? "(no tool)";
}, null);
}
private void DataReceived(string res)
{
lock (_dataReceivedLock)
{
string[] _responseChunks = res.Split(' ');
int resType = Convert.ToInt32(_responseChunks[0].Substring(1));
double[] data = new double[_responseChunks.Length - 1];
for (int i = 0; i < data.Length; i++)
{
// @TODO: add sanity like Double.TryParse(...)
data[i] = Double.Parse(_responseChunks[i + 1]);
}
switch (resType)
{
// ">20 1 2 1;" Sends version numbers
case ABBCommunicationProtocol.RES_VERSION:
this._deviceDriverVersion = Convert.ToInt32(data[0]) + "." + Convert.ToInt32(data[1]) + "." + Convert.ToInt32(data[2]);
int comp = Utilities.Strings.CompareVersions(ABBCommunicationProtocol.MACHINA_SERVER_VERSION, _deviceDriverVersion);
if (comp > -1)
{
logger.Verbose($"Using ABB Driver version {ABBCommunicationProtocol.MACHINA_SERVER_VERSION}, found {_deviceDriverVersion}.");
}
else
{
logger.Warning($"Found driver version {_deviceDriverVersion}, expected at least {ABBCommunicationProtocol.MACHINA_SERVER_VERSION}. Please update driver module or unexpected behavior may arise.");
}
break;
// ">21 400 300 500 0 0 1 0;"
case ABBCommunicationProtocol.RES_POSE:
this.initPos = new Vector(data[0], data[1], data[2]);
this.initRot = new Rotation(new Quaternion(data[3], data[4], data[5], data[6]));
break;
// ">22 0 0 0 0 90 0;"
case ABBCommunicationProtocol.RES_JOINTS:
this.initAx = new Joints(data[0], data[1], data[2], data[3], data[4], data[5]);
break;
// ">23 1000 9E9 9E9 9E9 9E9 9E9;"
case ABBCommunicationProtocol.RES_EXTAX:
this.initExtAx = new ExternalAxes(data[0], data[1], data[2], data[3], data[4], data[5]);
break;
// ">24 X Y Z QW QX QY QZ J1 J2 J3 J4 J5 J6 A1 A2 A3 A4 A5 A6;"
case ABBCommunicationProtocol.RES_FULL_POSE:
Vector pos = new Vector(data[0], data[1], data[2]);
Rotation rot = new Rotation(new Quaternion(data[3], data[4], data[5], data[6]));
Joints ax = new Joints(data[7], data[8], data[9], data[10], data[11], data[12]);
ExternalAxes extax = new ExternalAxes(data[13], data[14], data[15], data[16], data[17], data[18]);
this._motionCursor.UpdateFullPose(pos, rot, ax, extax);
this._parentDriver.parentControl.RaiseMotionUpdateEvent();
break;
}
}
}
