/// <summary>
/// Searches the network for a robot controller and establishes a connection with the specified one by position.
/// Performs no LogOn actions or similar.
/// </summary>
/// <returns></returns>
private bool LoadController(int controllerID)
{
// Scan the network and hookup to the specified controller
bool success = false;
// This is specific to ABB, should become abstracted at some point...
logger.Verbose("Scanning the network for controllers...");
NetworkScanner scanner = new NetworkScanner();
ControllerInfo[] controllers = scanner.GetControllers();
if (controllers.Length > 0)
{
int cId = controllerID > controllers.Length ? controllers.Length - 1 :
controllerID < 0 ? 0 : controllerID;
controller = ControllerFactory.CreateFrom(controllers[cId]);
if (controller != null)
{
//isConnected = true;
logger.Verbose($"Found controller {controller.SystemName} on {controller.Name}");
success = true;
logger.Debug(controller);
//Console.WriteLine(controller.RobotWare);
//Console.WriteLine(controller.RobotWareVersion);
//try
//{
// this.robotWare = this.controller.RobotWare;
// this.robotWareOptions = this.robotWare.Options;
// this._hasMultiTasking = HasMultiTaskOption(this.robotWareOptions);
// this._hasEGM = HasEGMOption(this.robotWareOptions);
//}
//catch
//{
// Console.WriteLine("Could not access ROBOTWARE options");
//}
}
else
{
logger.Debug("Could not connect to controller...");
}
}
else
{
logger.Debug("No controllers found on the network");
}
//if (!success)
//{
// Disconnect();
// //throw new Exception("ERROR: could not LoadController()");
//}
return(success);
}
/// <summary>
/// This method sends buffered instructions to the client socket on the robot whenever necessary
/// </summary>
/// <param name="obj"></param>
private void ServerSendingMethod(object obj)
{
if (Thread.CurrentThread.Name == null)
{
Thread.CurrentThread.Name = "MachinaTCPDriverServerSendingMethod";
}
logger.Verbose("Started TCP server sender for UR robot communication");
// Expire thread if no socket
while (_robotSocket != null)
{
while (this.ShouldSend() && this._issueCursor.AreActionsPending())
{
_sendMsgBytes = this._translator.GetBytesForNextAction(this._issueCursor);
// If the action had instruction represenation
if (_sendMsgBytes != null)
{
_robotNetworkStream.Write(_sendMsgBytes, 0, _sendMsgBytes.Length);
_sentIDs.Add(this._issueCursor.GetLastAction().Id);
_sentMessages++;
logger.Debug("Sending:");
logger.Debug(" " + this._issueCursor.GetLastAction());
logger.Debug(" [" + string.Join(",", (Util.ByteArrayToInt32Array(_sendMsgBytes))) + "]");
}
// Action was released to the device, raise event
this._parentDriver.parentControl.RaiseActionReleasedEvent();
}
//RaiseBufferEmptyEventCheck();
Thread.Sleep(30);
}
logger.Verbose("Stopped TCP server sender for UR robot communication");
}
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;
}
}
}