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); }