Two way communication requires good structure. I have chosen a package structure of 8 bytes. The first byte is a letter, “A” to ‘Z’, that indicates the type of package. The next six bytes form two groups of three characters, each group is used to carry data. A group consists of numbers ‘0’ to ‘9’ e.g. an integer of value = 1 is transmitted as ‘001’. This provides a range of 0 to 999. Within the hardware domain of sensors and motors many items produce data as bytes or are controlled by bytes: however this is not true in every case. One might chose to transfer in bytes (1 to 255) but this may not be sufficiently visible for human inspection. The choice is always yours. The really important point is that for your projects you should establish a simple list of rules that are stricitly applied.
In my project a package always has the structure: Znnnnnn#. Note that every package ends with a terminator equal to #. There are only two other possibilities: Znnn#### or Z#######. All three structures are provided for through out the C# code. Two programs exist: the Robot and the Controller.
I will next describe the commication flow of one package:
The TrackBar in the ‘Controller’ user interface is moved to establish an integer that represents the required speed. This generates an event:
private void tBarRightSpeed_MouseUp(object sender, MouseEventArgs e)
{
iRightSpeed = tBarRightSpeed.Value;
textRightSpeed.Text = IntToString(iRightSpeed, 3);
}
The use of the MouseUp event ensures that only the final position of the TrackBar generates a command. This value is displayed in a TextBox.
The variable iRightSpeed is accessible throughout the Form Class:
public int iLeftSpeed, iRightSpeed, iDirectionF, iDirectionR;
Outside the Form Class a delegate is declared:
public delegate int GetRightSpeedCallback();
Inside the Form Class a function exists to transfer the data item, iRightSpeed, from the Form thread to the Command thread:
private int GetRightSpeed()
{
if (this.tBarRightSpeed.InvokeRequired)
{
GetRightSpeedCallback d = new etRightSpeedCallback(GetRightSpeed);
object returned = this.Invoke(d, new object[] { });
return (int)returned;
}
else
{
return iRightSpeed;
}
}
In the Command thread the above function is called:
oRightSpeed = this.GetRightSpeed();
This transfers the data from the Form thread to the Command thread of the Controller program. Next the data item, oRightSpeed, is combined into a package and placed in the outwards queue.
if (oForward != xForward && oForward == true)
{
currentCommand = "F";
str = currentCommand + IntToString(oLeftSpeed, 3) + IntToString(oRightSpeed, 3) + "#";
bBuffer = new byte[8];
encoding.GetBytes(str, 0, 8, bBuffer, 0);
success = false;
if (str.Length > 0)
success = TryEnqueue(outQueue, bBuffer);
if (success == false)
Logger.Log("CommandWorker: Failed to add to outwards queue: " + str, logWriter);
}
The Send thread collects the package from the queue and sends it via wifi to the Robot:
while (bContinue)
{
if (outQueue.Count > 0)
bBuffer = TryDequeue(outQueue);
else
{
Thread.Sleep(200);
continue;
}
if (bBuffer.Length > 0 && (bBuffer[0] >= (byte)'A' && bBuffer[0] <= (byte)'Z'))
{
networkStream.Write(bBuffer, 0, bBuffer.Length);
bBuffer[0] = (byte)'\0';
}
}
Now we switch to the Robot program. The Receive thread is waiting to receive our package and place it on the inwards queue:
arraySize = serverSocket.Available;
if (arraySize > 0)
{
received = new byte[arraySize];
bytesRec = serverSocket.Receive(received, 0, arraySize, SocketFlags.None);
}
if (bytesRec > 0)
{
sbReceived = new StringBuilder(new string(Encoding.UTF8.GetChars(received)));
Debug.Print("Text received: " + sbReceived.ToString());
}
else
continue;
if (sbRemainder.Length > 0)
{
sbReceived = sbReceived.Insert(0, sbRemainder.ToString(), sbRemainder.Length); // attach the previous remainder to the new data
sbRemainder.Clear();
}
count = 0;
for (int i = 0; i < sbReceived.Length; i++)
{
if (sbReceived[i] < '!' || sbReceived[i] > '~')
continue;
if (sbReceived[i] == delimiter)
{
if (i > 0)
{
if (sbReceived[i - 1] != delimiter && sbReceived[i - 1] != 0x00) //End of package
{
if (bBuffer.Length > 0)
{
bBuffer[count] = (byte)'\0';
success = TryEnqueue(inQueue, bBuffer);
The Command thread in the Robot program collects the package from the queue then splits the package into its three parts:
foreach (byte b in bBuffer)
{
if (b >= (byte)'A' && b <= (byte)'Z' && done == false)
{
cmd = (char)b;
done = true;
continue;
}
if (b == (byte)'#' || b == (byte)'\0')
{
break;
}
if (done == true)
{
if (b >= (byte)'0' && b <= (byte)'9')
comBuffer[i++] = (char)b;
else if (i < 6)
comBuffer[i++] = '0';
}
}
str = new string(comBuffer, 0, 3);
num1 = T35_Console_Library.StrVal(str);
str = new string(comBuffer, 3, 3);
num2 = T35_Console_Library.StrVal(str);
Debug.Print("Command: " + cmd + " " + num1.ToString() + " " + num2.ToString());
We now have cmd, num1 and num2 from the package sent by the Controller to the Robot e.g. cmd = ‘F’ num1 = 150 and num2 = 150. Next the data is applied to three drivers, each driver controls two motors, The code ensures that all the motors are spinning in the right direction (forward) at the same speed:
case 'F': //Move Forward
{
num1 = num1 + 128;
num2 = num2 + 128;
if (!(num1 >= 129 && num1 <= 255)) //Positive Speed 129 to 255
num1 = 128; //Set Speed zero
if (!(num2 >= 129 && num2 <= 255)) //Positive Speed 129 to 255
num2 = 128; //Set Speed zero
bool bflag = false;
int driver, motor;
int speed = 0;
for (driver = 0; driver < 3; driver++)
{
for (motor = 0; motor < 2; motor++)
{
if (driver == 0) //B2 (right)
{
if (motor == 0)
speed = 255 - num2;
else
speed = num2;
}
if (driver == 2) //B6 (front & back)
{
if (motor == 0)
speed = 255 - ((num1 + num2)/2);
else
speed = ((num1 + num2) / 2);
}
if (driver == 1) //B4 (left)
{
if (motor == 1)
speed = 255 - num1;
else
speed = num1;
}
I2CBusClass.Transmit(true, false, (byte)(0xB2 + (driver * 2))); // Driver address with a start bit
I2CBusClass.Transmit(false, false, (byte)motor); // Register to be set: Motor speed
bflag = I2CBusClass.Transmit(false, true, (byte)speed); // Value of speed
}
}
break;
Remember we are presently in the Command thread of the Robot progam. However there is also a Data thread busy collecting data from the same motors using the same I2CBusClass library. As with many hardware resources more than one thread may wish to access the same resource at the same time. We need to modify the library to provide for this:
public class I2CBusClass
{
private static object threadlock = new object();
Being a static function it needs to create a thread lock object. Within each library function it is also necessary to lock the code to ensure that only one thread can enter the function at the same time.
public static bool Transmit(bool sendStartCondition, bool sendStopCondition, byte byteToSend)
{
lock (threadlock)
{
This completes the journey where one integer (containing a speed value) has travelled from the user interface to the motor resource.
Keep them comming