Main Site Documentation

Project - Robot Communication Part Two - Threading


#1

Robot Communication Part Two - Threading

In Part One I placed the Socket Server in the controller and the Socket Client in the robot. In terms of C# framework, this provided full .NET for the Server and NETMF for the client. This seemed the natural way of establishing two-way communication especially as the server is the more complex side of the process. In Part Two, I have reversed this structure and placed the Socket Server in the robot and the Socket Client in the controller. In Part Three the plan is to introduce WinForms (a single threaded module) to the controller. Apparently the combination of WinForms and a Socket Server (a distributed threading module) is difficult to code, hence Part Three will attempt to add WinForms to the Client Socket code. Part Four, I hope, will add all the robot hardware related NETMF code.

The primary purpose of this stage of the project is to introduce multi-threading. I have used the same structure of four threads: receiving, sending, commands and data: in both the controller and the robot. Between each pair of threads a queue is used to help the transition between threads (see the attached diagram). This structure is a first time beginners guess. But be warned, with any multi-threaded program, it is best to thoroughly test the code before adding the threading.

Other than writing code for a Arduino Tank I have never programmed anything before. Please post any comments or suggestions about the design or code.


#2

Nice :slight_smile: Keep them comming


#3

Thanks Gus. WinForms might be a while…


#4

sorry, what do you mean with

what is the task of the client


#5

The client socket code is in the controller (full .NET). And I will need code like:


        public void SetText(string str)
        {
            if (this.textBoxData.InvokeRequired)
            {
                SetTextCallback d = new SetTextCallback(SetText);
                this.Invoke(d, new object[] { str });
            }
            else
            {
                this.textBoxData.Text = str;
            }
        }



#6

has the client his on intelligence or does the client only show the robot outputs and
the user controls the robot ?
I’m thinking about an HTML-control-site to control the robot. no client code was needed.


#7

@ VB-Daniel: Many thanks for your interest. The robot will be at least 90% controlled via the laptop but will self prevent crashes. A significant amount of data will flow back from the robot and I hope that the user interface will have a good response time to data received and commands given. Going by my test today it looks ok on winForms. I would be very new to using HTML etc but will look at your suggestion if I get stuck. I have attached a screen shot in which you can see commands received by the robot from the laptop at the bottom of the screen and data flowing from the robot to the laptop into the textbox.


#8

@ KG1, I am also working on a robot control PC and call it Robot Console.

The connection is via bluetooth which a virtule comport used. The Lego NXT has a slightly different protocol hence the radio button.
The messages include the time so you can also replay the log.
The application tabs give an actual animation of the process.

The shown Manual1 tab is the tab changes to the motor control.


#9

Hi Hinnie, I am most interested in your work on your application Robot Console. You are at a more advanced stage than I have yet achieved. If you are willing to share more information with me, please write to me by email: kgordon(at)paradise.net.nz. I believe you could write some interesting codeshare articles on GHI’s site but if you need a product neutral site consider http://www.rescuerobot.org. I am keen to learn more about your robot progress. Regards, KG1


#10

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.