public void HandleMotorsMessage(byte[] buffer)
{
if (InvokeRequired)
{
Invoke(new DGuiCallBuffer(HandleMotorsMessage), buffer);
return;
}
string msg = "Motors Message Received: ";
for (int i = 0; i < buffer.Length; i++)
{
msg += buffer[i] + " ";
}
PostMessage(msg);
if (motors.IsConnected)
{
byte[] newbuffer = new byte[buffer.Length - 1];
for (int i = 1; i < buffer.Length; i++)
{
newbuffer[i - 1] = buffer[i];
}
motors.Send(newbuffer);
}
}
public void HandleMotorsMessage(byte[] rbuffer)
{
if (InvokeRequired)
{
Invoke(new DGuiCallBuffer(HandleMotorsMessage), rbuffer);
return;
}
byte[] buffer = new byte[rbuffer.Length];
string msg = "Motors Message Received: ";
for (int i = 0; i < rbuffer.Length; i++)
{
buffer[i] = rbuffer[i];
msg += buffer[i] + " ";
}
PostMessage(msg);
if (motors.IsConnected)
{
if (buffer.Length >= SERVER_CMD_SIZE)
{
byte[] newbuffer = new byte[MOTOR_PACKET_SIZE];
for (int i = 1; i < MOTOR_PACKET_SIZE; i++)
{
newbuffer[i - 1] = buffer[i];
}
newbuffer[MOTOR_PACKET_SIZE - 1] = EOT; // end byte
// disable last driving if new driving cmd recieved
if (newbuffer[1] == DRIVING_CMD)
{
drivingtimer.Enabled = false; // disable last driving
}
// send same motor command 3 times due to the hardware issue
motors.Send(newbuffer);
System.Threading.Thread.Sleep(200);
motors.Send(newbuffer);
if (newbuffer[1] == DRIVING_CMD)
{
// obtain driving time from command buffer, which are the last 2 bytes before the end byte
byte[] B_duration = new byte[2];
B_duration[0] = buffer[SERVER_CMD_SIZE - 2];
B_duration[1] = buffer[SERVER_CMD_SIZE - 3];
ushort duration = BitConverter.ToUInt16(B_duration, 0); // driving time (ms)
msg = "Duration = ";
msg += duration.ToString();
PostMessage(msg);
if (duration > 0)
{
// reset driving time
drivingtimer.Interval = duration * 50;
drivingtimer.Enabled = true;
}
}
msg = "Message sent: ";
for (int i = 0; i < newbuffer.Length; i++)
{
msg += newbuffer[i] + " ";
}
PostMessage(msg);
}
}
}