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;
}
}
}
/// <summary>
/// Performs all necessary steps to successfuly connect to the device using the RobotStudio API.
/// </summary>
/// <param name="deviceId"></param>
/// <returns></returns>
public bool Connect(int deviceId)
{
_deviceId = deviceId;
// In general, the Disconnect() + return false pattern instead of throwing errors
// avoids the controller getting hung with an undisposed mastership or log from the client,
// making failures a little more robust (and less annoying...)
// Connect to the ABB real/virtual controller
if (!LoadController(deviceId))
{
logger.Warning("Could not connect to controller, found no device on the network");
Disconnect();
return(false);
}
// Load the controller's IP
if (!LoadIP())
{
logger.Warning("Could not connect to controller, failed to find the controller's IP");
Disconnect();
return(false);
}
// Log on to the controller
if (!LogOn())
{
logger.Warning("Could not connect to controller, failed to log on to the controller");
Disconnect();
return(false);
}
if (!LoadRobotWareOptions())
{
logger.Warning("Could not connect to controller, failed to retrieve RobotWare options from the controller");
Disconnect();
return(false);
}
// Is controller in Automatic mode with motors on?
if (!IsControllerInAutoMode())
{
logger.Warning("Could not connect to controller, please set up controller to AUTOMATIC MODE and try again.");
Disconnect();
return(false);
}
if (!IsControllerMotorsOn())
{
logger.Warning("Could not connect to controller, please set up Motors On mode in controller");
Disconnect();
return(false);
}
// Test if Rapid Mastership is available
if (!TestMastershipRapid())
{
logger.Warning("Could not connect to controller, mastership not available");
Disconnect();
return(false);
}
// Load main task from the controller
if (!LoadMainTask())
{
logger.Warning("Could not connect to controller, failed to load main task");
Disconnect();
return(false);
}
if (this._hasMultiTasking)
{
if (!LoadMonitorTask())
{
logger.Info("Your device has the capacity to be monitored in real time, but could not be set up automatically; please refer to the documentation on how to set Machina monitoring on ABB robots.");
}
}
if (!SetRunMode(CycleType.Once))
{
logger.Warning("Could not connect to controller, failed to set runmode to once");
Disconnect();
return(false);
}
// Subscribe to relevant events to keep track of robot execution
if (!SubscribeToEvents())
{
logger.Warning("Could not connect to controller, failed to subscribe to robot controller events");
Disconnect();
return(false);
}
_isConnected = true;
return(_isConnected);
}