/// <summary>
/// Add pose to the current pose, mainly used for moving it just slightly
/// </summary>
/// <param name="pose">Pose to be added to the current UR10 pose</param>
public void SendAddedPose(URPose pose)
{
_writer.AddURPose(pose);
}
/// <summary>
/// Force it to pose an entirely new way
/// </summary>
/// <param name="pose"></param>
public void SendNewPose(URPose pose)
{
_writer.SendURPose(pose);
}
public void ReceivePose(URPose pose)
{
LastKnownPose = pose;
OnCurrentURPoseReceived(pose);
}
/// <summary>Analyses the cartesian info when robot message type is CartesianInfo.
/// Subpackage containing different informations about the CartesianInfo, such as TCP position and offset.
/// <para>Value for CartesianInfo is 4.</para>
/// <para>Package length is 101 bytes.</para>
/// </summary>
/// <param name="data">Byte array received on the socket stream.</param>
private void CartesianInfo(byte[] data)
{
//Debug.WriteLine("Package type: {0} - CARTESIAN INFO", data[count + 1]);
count++;
//Debug.WriteLine("X: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double xPoseDouble = GetDoubleFromByteArray(data, count);
//Debug.WriteLine("X: " + xPoseDouble);
count = count + 8;
//Debug.WriteLine("Y: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double yPoseDouble = GetDoubleFromByteArray(data, count);
//Debug.WriteLine("Y: " + yPoseDouble);
count = count + 8;
//Debug.WriteLine("Z: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double zPoseDouble = GetDoubleFromByteArray(data, count);
//Debug.WriteLine("Z: " + zPoseDouble);
count = count + 8;
//Debug.WriteLine("Rx: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double RxPoseDouble = GetDoubleFromByteArray(data, count);
//Debug.WriteLine("Rx: " + RxPoseDouble);
count = count + 8;
//Debug.WriteLine("Ry: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double RyPoseDouble = GetDoubleFromByteArray(data, count);
//Debug.WriteLine("Ry: " + RyPoseDouble);
count = count + 8;
//Debug.WriteLine("Rz: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double RzPoseDouble = GetDoubleFromByteArray(data, count);
//Debug.WriteLine("Rz: " + RzPoseDouble);
count = count + 8;
//Debug.WriteLine("TCPOffsetX: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Debug.WriteLine("TCPOffsetY: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Debug.WriteLine("TCPOffsetZ: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Debug.WriteLine("TCPOffsetRx: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Debug.WriteLine("TCPOffsetRy: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Debug.WriteLine("TCPOffsetRz: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
URPose pose = new URPose(xPoseDouble, yPoseDouble, zPoseDouble, RxPoseDouble, RyPoseDouble, RzPoseDouble);
Data.Instance.ReceivePose(pose);
//events.UpdateActualURPose(pose);
if (OnActualURPose != null)
{
OnActualURPose(pose);
}
}
public void SendEntirePose(URPose pose)
{
Data.Instance.SendNewPose(pose);
}
private void Change_TCP_Rotation(double rx, double ry, double rz)
{
URPose p = new URPose(0, 0, 0, rx, ry, rz);
Data.Instance.SendAddedPose(p);
}
private void Change_TCP_Position(double x, double y, double z)
{
URPose p = new URPose(x, y, z, 0, 0, 0);
Data.Instance.SendAddedPose(p);
}
