// convert RobosapienV2 sensor parameters to bytes
public static byte[] convertRSV2Sensorparams2Bytes(RSV2Sensorparams parms)
{
byte[] rawparms = new byte[30];
rawparms[0] = parms.P_Left_Foot_Front_Bumper;
rawparms[1] = parms.P_Left_Foot_Rear_Bumper;
rawparms[2] = parms.P_Right_Foot_Front_Bumper;
rawparms[3] = parms.P_Right_Foot_Rear_Bumper;
rawparms[4] = parms.P_Left_Hand_Bumper;
rawparms[5] = parms.P_Right_Hand_Bumper;
rawparms[6] = parms.P_Left_Hand_Pickup;
rawparms[7] = parms.P_Right_Hand_Pickup;
// Analog sensors
rawparms[8] = (byte)(parms.P_Camera_HSD & 0x0FF);
rawparms[9] = (byte)((parms.P_Camera_HSD >> 8) & 0x0FF);
rawparms[10] = (byte)(parms.P_Camera_RST & 0x0FF);
rawparms[11] = (byte)((parms.P_Camera_RST >> 8) & 0x0FF);
rawparms[12] = (byte)(parms.P_Camera_RDY & 0x0FF);
rawparms[13] = (byte)((parms.P_Camera_RDY >> 8) & 0x0FF);
rawparms[14] = (byte)(parms.P_Camera_SK & 0x0FF);
rawparms[15] = (byte)((parms.P_Camera_SK >> 8) & 0x0FF);
rawparms[16] = (byte)(parms.P_Left_Shoulder_Pot & 0x0FF);
rawparms[17] = (byte)((parms.P_Left_Shoulder_Pot >> 8) & 0x0FF);
rawparms[18] = (byte)(parms.P_Right_Shoulder_Pot & 0x0FF);
rawparms[19] = (byte)((parms.P_Right_Shoulder_Pot >> 8) & 0x0FF);
rawparms[20] = (byte)(parms.P_Left_Mic & 0x0FF);
rawparms[21] = (byte)((parms.P_Left_Mic >> 8) & 0x0FF);
rawparms[22] = (byte)(parms.P_Right_Mic & 0x0FF);
rawparms[23] = (byte)((parms.P_Right_Mic >> 8) & 0x0FF);
rawparms[24] = (byte)(parms.P_Left_Head_IR & 0x0FF);
rawparms[25] = (byte)((parms.P_Left_Head_IR >> 8) & 0x0FF);
rawparms[26] = (byte)(parms.P_Center_Head_IR & 0x0FF);
rawparms[27] = (byte)((parms.P_Center_Head_IR >> 8) & 0x0FF);
rawparms[28] = (byte)(parms.P_Right_Head_IR & 0x0FF);
rawparms[29] = (byte)((parms.P_Right_Head_IR >> 8) & 0x0FF);
return(rawparms);
}
// convert bytes to an Robosapien V2 Swensor readings structure
public static RSV2Sensorparams convertBytes2RSV2Sensorparams(byte[] rawparms)
{
RSV2Sensorparams parms = new RSV2Sensorparams();
parms.P_Left_Foot_Front_Bumper = rawparms[0];
parms.P_Left_Foot_Rear_Bumper = rawparms[1];
parms.P_Right_Foot_Front_Bumper = rawparms[2];
parms.P_Right_Foot_Rear_Bumper = rawparms[3];
parms.P_Left_Hand_Bumper = rawparms[4];
parms.P_Right_Hand_Bumper = rawparms[5];
parms.P_Left_Hand_Pickup = rawparms[6];
parms.P_Right_Hand_Pickup = rawparms[7];
// Analog sensors
parms.P_Camera_HSD = (ushort)(rawparms[8] + rawparms[9] * 256);
parms.P_Camera_RST = (ushort)(rawparms[10] + rawparms[11] * 256);
parms.P_Camera_RDY = (ushort)(rawparms[12] + rawparms[13] * 256);
parms.P_Camera_SK = (ushort)(rawparms[14] + rawparms[15] * 256);
parms.P_Left_Shoulder_Pot = (ushort)(rawparms[16] + rawparms[17] * 256);
parms.P_Right_Shoulder_Pot = (ushort)(rawparms[18] + rawparms[19] * 256);
parms.P_Left_Mic = (ushort)(rawparms[20] + rawparms[21] * 256);
parms.P_Right_Mic = (ushort)(rawparms[22] + rawparms[23] * 256);
parms.P_Left_Head_IR = (ushort)(rawparms[24] + rawparms[25] * 256);
parms.P_Center_Head_IR = (ushort)(rawparms[26] + rawparms[27] * 256);
parms.P_Right_Head_IR = (ushort)(rawparms[28] + rawparms[29] * 256);
return(parms);
}
// convert RobosapienV2 sensor parameters to bytes
public static byte[] convertRSV2Sensorparams2Bytes(RSV2Sensorparams parms)
{
byte[] rawparms = new byte[30];
rawparms[0] = parms.P_Left_Foot_Front_Bumper;
rawparms[1] = parms.P_Left_Foot_Rear_Bumper;
rawparms[2] = parms.P_Right_Foot_Front_Bumper;
rawparms[3] = parms.P_Right_Foot_Rear_Bumper;
rawparms[4] = parms.P_Left_Hand_Bumper;
rawparms[5] = parms.P_Right_Hand_Bumper;
rawparms[6] = parms.P_Left_Hand_Pickup;
rawparms[7] = parms.P_Right_Hand_Pickup;
// Analog sensors
rawparms[8] = (byte)(parms.P_Camera_HSD & 0x0FF);
rawparms[9] = (byte)((parms.P_Camera_HSD >> 8) & 0x0FF);
rawparms[10] = (byte)(parms.P_Camera_RST & 0x0FF);
rawparms[11] = (byte)((parms.P_Camera_RST >> 8) & 0x0FF);
rawparms[12] = (byte)(parms.P_Camera_RDY & 0x0FF);
rawparms[13] = (byte)((parms.P_Camera_RDY >> 8) & 0x0FF);
rawparms[14] = (byte)(parms.P_Camera_SK & 0x0FF);
rawparms[15] = (byte)((parms.P_Camera_SK >> 8) & 0x0FF);
rawparms[16] = (byte)(parms.P_Left_Shoulder_Pot & 0x0FF);
rawparms[17] = (byte)((parms.P_Left_Shoulder_Pot >> 8) & 0x0FF);
rawparms[18] = (byte)(parms.P_Right_Shoulder_Pot & 0x0FF);
rawparms[19] = (byte)((parms.P_Right_Shoulder_Pot >> 8) & 0x0FF);
rawparms[20] = (byte)(parms.P_Left_Mic & 0x0FF);
rawparms[21] = (byte)((parms.P_Left_Mic >> 8) & 0x0FF);
rawparms[22] = (byte)(parms.P_Right_Mic & 0x0FF);
rawparms[23] = (byte)((parms.P_Right_Mic >> 8) & 0x0FF);
rawparms[24] = (byte)(parms.P_Left_Head_IR & 0x0FF);
rawparms[25] = (byte)((parms.P_Left_Head_IR >> 8) & 0x0FF);
rawparms[26] = (byte)(parms.P_Center_Head_IR & 0x0FF);
rawparms[27] = (byte)((parms.P_Center_Head_IR >> 8) & 0x0FF);
rawparms[28] = (byte)(parms.P_Right_Head_IR & 0x0FF);
rawparms[29] = (byte)((parms.P_Right_Head_IR >> 8) & 0x0FF);
return rawparms;
}
// convert bytes to an Robosapien V2 Swensor readings structure
public static RSV2Sensorparams convertBytes2RSV2Sensorparams(byte[] rawparms)
{
RSV2Sensorparams parms = new RSV2Sensorparams();
parms.P_Left_Foot_Front_Bumper = rawparms[0];
parms.P_Left_Foot_Rear_Bumper = rawparms[1];
parms.P_Right_Foot_Front_Bumper = rawparms[2];
parms.P_Right_Foot_Rear_Bumper = rawparms[3];
parms.P_Left_Hand_Bumper = rawparms[4];
parms.P_Right_Hand_Bumper = rawparms[5];
parms.P_Left_Hand_Pickup = rawparms[6];
parms.P_Right_Hand_Pickup = rawparms[7];
// Analog sensors
parms.P_Camera_HSD = (ushort)(rawparms[8] + rawparms[9] * 256);
parms.P_Camera_RST = (ushort)(rawparms[10] + rawparms[11] * 256);
parms.P_Camera_RDY = (ushort)(rawparms[12] + rawparms[13] * 256);
parms.P_Camera_SK = (ushort)(rawparms[14] + rawparms[15] * 256);
parms.P_Left_Shoulder_Pot = (ushort)(rawparms[16] + rawparms[17] * 256);
parms.P_Right_Shoulder_Pot = (ushort)(rawparms[18] + rawparms[19] * 256);
parms.P_Left_Mic = (ushort)(rawparms[20] + rawparms[21] * 256);
parms.P_Right_Mic = (ushort)(rawparms[22] + rawparms[23] * 256);
parms.P_Left_Head_IR = (ushort)(rawparms[24] + rawparms[25] * 256);
parms.P_Center_Head_IR = (ushort)(rawparms[26] + rawparms[27] * 256);
parms.P_Right_Head_IR = (ushort)(rawparms[28] + rawparms[29] * 256);
return parms;
}
