void Com_DataReceived(byte[] data)
{
if (waitingForResponse)
{
Array.Copy(data, ResponseBuffer, data.Length);
autoEvent.Set();
}
else
{
uint address = BitConverter.ToUInt32(data, 0);
JointCommands response = (JointCommands)data[4];
if (response == JointCommands.Associate)
{
if (!Controllers.ContainsKey(address))
{
Controllers.Add(address, new Controller()
{
Robot = this, Id = address
});
if (Controllers.Count() >= AssocNumJoints)
{
IsAssociating = false;
}
}
}
}
}
public void SendCommand(JointCommands command, Controller joint = null, byte[] payload = null)
{
//lock(commLock)
//{
if (Com == null)
{
return;
}
int numBytesToSend = 6; // number of bytes (1) + joint address (4) + command (1)
if (payload != null)
{
numBytesToSend += payload.Length;
}
byte[] buffer = new byte[numBytesToSend];
uint address;
if (joint == null)
{
address = 0; // broadcast mode
}
else
{
address = joint.Id;
}
// Number of bytes in payload
buffer[0] = (byte)numBytesToSend;
// Address
byte[] addressBytes = BitConverter.GetBytes(address);
// We don't really care if our int addresses are "correct" or not, since they're just bits.
//if(BitConverter.IsLittleEndian)
// addressBytes.Reverse();
Array.Copy(addressBytes, 0, buffer, 1, 4);
// Command
buffer[5] = (byte)command;
if (payload != null)
{
Array.Copy(payload, 0, buffer, 6, payload.Length);
}
//Debug.WriteLine("Sending: " + new SoapHexBinary(buffer).ToString());
Com.Send(buffer);
//}
}
public async Task <byte[]> RequestData(JointCommands command, Controller joint, byte[] payload = null)
{
if (Com == null)
{
return(null);
}
Task <byte[]> SendCommandTask = Task <byte[]> .Run <byte[]>(
() => {
SendCommand(command, joint, payload);
waitingForResponse = true;
autoEvent.WaitOne(10000);
waitingForResponse = false;
return(ResponseBuffer);
}
);
byte[] data = await SendCommandTask;
return(data);
}
public void SendCommand(JointCommands command, Controller joint = null, byte[] payload = null)
{
if (Com == null)
{
return;
}
int numBytesToSend = 5; // joint address (4) + command (1)
if (payload != null)
{
numBytesToSend += payload.Length;
}
byte[] buffer = new byte[numBytesToSend];
uint address;
if (joint == null)
{
address = 0; // broadcast mode
}
else
{
address = joint.Id;
}
// Address
byte[] addressBytes = BitConverter.GetBytes(address);
// We don't really care if our int addresses are "correct" or not, since they're just bits.
//if(BitConverter.IsLittleEndian)
// addressBytes.Reverse();
Array.Copy(addressBytes, buffer, 4);
// Command
buffer[4] = (byte)command;
if (payload != null)
{
Array.Copy(payload, 0, buffer, 5, payload.Length);
}
Com.Send(buffer);
}
void Com_DataReceived(byte[] data)
{
// This is a hack to deal with the discover messages
uint address = BitConverter.ToUInt32(data, 1);
JointCommands response = (JointCommands)data[5];
switch (response)
{
case JointCommands.Associate:
if (!Controllers.ContainsKey(address))
{
Controllers.Add(address, new Controller()
{
Robot = this, Id = address
});
if (Controllers.Count() >= AssocNumJoints)
{
IsAssociating = false;
}
Com_UpdateSetpoints();
//Com.AddAddress(address);
}
else
{
return;
}
break;
case JointCommands.GetStatus:
//Debug.WriteLine("Received response: " + new SoapHexBinary(ResponseBuffer).ToString());
foreach (var joint in Controllers)
{
if (joint.Value.Id == address)
{
joint.Value.SetStatus(data);
return;
}
}
break;
case JointCommands.GetHallPos:
foreach (var joint in Controllers)
{
if (joint.Value.Id == address)
{
joint.Value.SetHallPos(data);
return;
}
}
break;
case JointCommands.GetPots:
foreach (var joint in Controllers)
{
if (joint.Value.Id == address)
{
joint.Value.SetPots(data);
return;
}
}
break;
case JointCommands.GetCurrent:
foreach (var joint in Controllers)
{
if (joint.Value.Id == address)
{
joint.Value.SetCurrent(data);
return;
}
}
break;
}
}
