public void AddURPose(DataStructures.URPose pose)
{
DataStructures.URPose currentPose = DummyUR10.Instance.pose;
currentPose.Xpose += pose.Xpose;
currentPose.Ypose += pose.Ypose;
currentPose.Zpose += pose.Zpose;
currentPose.RXpose += pose.RXpose;
currentPose.RYpose += pose.RYpose;
currentPose.RZpose += pose.RZpose;
DummyUR10.Instance.pose = currentPose;
}
public void SendURPose(DataStructures.URPose pose)
{
byte[] data = new byte[12];
ushort ID = 2;
byte unit = 0;
ushort StartAddress = 135;
Array.Copy(BitConverter.GetBytes(IPAddress.HostToNetworkOrder(Convert.ToInt16(pose.Xpose * 1000))), 0, data, 0, 2);
Array.Copy(BitConverter.GetBytes(IPAddress.HostToNetworkOrder(Convert.ToInt16(pose.Ypose * 1000))), 0, data, 2, 2);
Array.Copy(BitConverter.GetBytes(IPAddress.HostToNetworkOrder(Convert.ToInt16(pose.Zpose * 1000))), 0, data, 4, 2);
Array.Copy(BitConverter.GetBytes(IPAddress.HostToNetworkOrder(Convert.ToInt16(pose.RXpose * 1000))), 0, data, 6, 2);
Array.Copy(BitConverter.GetBytes(IPAddress.HostToNetworkOrder(Convert.ToInt16(pose.RYpose * 1000))), 0, data, 8, 2);
Array.Copy(BitConverter.GetBytes(IPAddress.HostToNetworkOrder(Convert.ToInt16(pose.RZpose * 1000))), 0, data, 10, 2);
byte[] result = new byte[100];
if (_modbus != null)
{
_modbus.WriteMultipleRegister(ID, unit, StartAddress, data, ref result);
}
}
public void SendURPose(DataStructures.URPose pose)
{
//INSTANT!
dataInstance.SetLastKnownPose(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;
DataStructures.URPose pose = new DataStructures.URPose(xPoseDouble, yPoseDouble, zPoseDouble, RxPoseDouble, RyPoseDouble, RzPoseDouble);
dataInstance.SetLastKnownPose(pose);
//events.UpdateActualURPose(pose);
if (OnActualURPose != null)
{
OnActualURPose(pose);
}
}
public void SetLastKnownPose(DataStructures.URPose pose)
{
LastKnownPose = pose;
OnCurrentURPoseReceived?.Invoke(pose);
}
/*
* /// <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(DataStructures.URPose pose)
* {
* writer.AddURPose(pose);
* }*/
/// <summary>
/// Force it to pose an entirely new way
/// </summary>
/// <param name="pose"></param>
public void SendNewPose(DataStructures.URPose pose)
{
writer.SendURPose(pose);
}
