public void PingServos()
{
CommBuffer buffer = comm.outputBuffer;
buffer.Write(0xFF);
buffer.Write(0x09);
buffer.Write(0x03);
buffer.Write(0x08);
buffer.Write(0x00);
comm.SendMessage();
}
public void Ping()
{
CommBuffer buffer = comm.outputBuffer;
/*buffer.Write(0xFF);
* buffer.Write(0x0A);
* buffer.Write(0x03);
* buffer.Write(0x08);
* buffer.Write(0x00);*/
buffer.Write(0xFF);
buffer.Write(0x14);
buffer.Write(0x01);
comm.SendMessage();
}
void port_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
byte[] data = new byte[port.BytesToRead];
port.Read(data, 0, data.Length);
for (int i = 0; i < data.Length; i++)
{
inputBuffer.Write(data[i]);
}
if (inputBuffer.CommandReady())
{
byte[] command = inputBuffer.ReadCommand();
if (command != null)
{
switch (command[1])
{
case 0x4A:
if (command.Length > 10)
{
PositionReceived(command);
}
if (command.Length == 4)
{
CheckAcknowledge(command);
}
break;
case 0x54:
LocalisationResponse(command);
break;
case 0x49:
if (command.Length > 10)
{
ServoStateReceived(command);
}
if (command.Length == 4)
{
CheckAcknowledge(command);
}
break;
case 0x47:
if (command.Length == 4)
{
CheckAcknowledge(command);
}
break;
default:
Console.WriteLine("INVALID RESPONSE " + command[1]); // Throw exception? no.
break;
}
}
}
}