public ControllerVersion(IURRTDE urRTDE)
{
byte[] payload = new byte[0];
var resp = urRTDE.SendReceive(IPackageType.RTDE_GET_URCONTROL_VERSION, payload);
byte[] temp = new byte[4];
Buffer.BlockCopy(resp, 0, temp, 0, 4);
Major = IFromBytes.ToUInt32(temp);
Buffer.BlockCopy(resp, 4, temp, 0, 4);
Minor = IFromBytes.ToUInt32(temp);
Buffer.BlockCopy(resp, 8, temp, 0, 4);
Bugfix = IFromBytes.ToUInt32(temp);
Buffer.BlockCopy(resp, 12, temp, 0, 4);
Build = IFromBytes.ToUInt32(temp);
}
public SetupOutput(IURRTDE urRTDE, double frequency, string variables)
{
var bFreq = IToBytes.FromDouble(frequency);
var bVar = IToBytes.FromString(variables);
byte[] payload = new byte[bFreq.Length + bVar.Length];
Buffer.BlockCopy(bFreq, 0, payload, 0, bFreq.Length);
Buffer.BlockCopy(bVar, 0, payload, bFreq.Length, bVar.Length);
var resp = urRTDE.SendReceive(IPackageType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS, payload);
OutputRecipeId = resp[0];
byte[] temp = new byte[resp.Length - 1];
Buffer.BlockCopy(resp, 1, temp, 0, temp.Length);
VariableTypes = IFromBytes.ToString(temp);
}
public void Receive()
{
var resp = _urRTDE.Receive(IPackageType.RTDE_DATA_PACKAGE);
int offset = 0;
RecipeId = resp[0];
offset += 1;
for (int i = 0; i < _variableTypes.Length; i++)
{
switch (_variableTypes[i])
{
case "INT32":
byte[] tempInt32 = new byte[4];
Buffer.BlockCopy(resp, offset, tempInt32, 0, 4);
offset += 4;
break;
case "UINT32":
byte[] tempUInt32 = new byte[4];
Buffer.BlockCopy(resp, offset, tempUInt32, 0, 4);
offset += 4;
break;
case "VECTOR6D":
byte[] tempV6D = new byte[48];
Buffer.BlockCopy(resp, offset, tempV6D, 0, 48);
var pose = IFromBytes.To6Double(tempV6D);
if (i == 1)
{
ActualTCPPose = new ITCPPosition
{
X = pose[0],
Y = pose[1],
Z = pose[2],
RX = pose[3],
RY = pose[4],
RZ = pose[5]
};
}
else if (i == 2)
{
ActualJointPose = new IJointPosition
{
Base = pose[0],
Shoulder = pose[1],
Elbow = pose[2],
Wrist1 = pose[3],
Wrist2 = pose[4],
Wrist3 = pose[5]
};
}
offset += 48;
break;
case "VECTOR3D":
break;
case "VECTOR6INT32":
break;
case "VECTOR6UINT32":
break;
case "DOUBLE":
byte[] tempDouble = new byte[8];
Buffer.BlockCopy(resp, offset, tempDouble, 0, 8);
offset += 8;
break;
case "UINT64":
break;
case "UINT8":
break;
case "BOOL":
break;
}
}
}
