Random timeouts using AX-12s, CM-5 and Forest Moon library

[pirobot]

Hello,

I have read a few other posts on this forum regarding how to deal with communication errors when using AX-12s and the CM-5 over a serial connection. I'm having a similar problem with timeouts that I have tried to debug for several weeks with no luck.

I have a robot with 10 AX-12+ servos connected to a CM-5 which in turn is connected to my PC by way of a USB-Serial converter. The CM-5 and servos are powered by the 12V brick that came with the Bioloid kit. I am controlling the servos using a C# program that references the Forest Moon Dynamixel library found at:

http://www.forestmoon.com/Software/AX12ArmSample/

I am running the servos in Synchronized mode and I make a call to Synchronize() in a dedicated thread that looks like this:

Code: Select all

while (true)
{
   DynaNet.Synchronize();
   Thread.Sleep(50);
   sensorValues[HeadPan] = HeadPan.CurrentPosition;
   Thread.Sleep(50);
   sensorValues[HeadTilt] = HeadTilt.CurrentPosition;
   Thread.Sleep(50);
   etc.
}

As you can see, after synchronizing any outstanding writes, I query two servos for their current position. In fact, I query several other servos also for their positions but I left them out of the code above for brevity.

In the meantime, in a different thread, I am setting the goal positions and speeds of the servos to track a moving target. This thread updates the position and speed settings about once every 50ms. But I am assuming they are only written to the servos when the Synchronize() method is called in the thread shown above.

With both threads running, the robot generally behaves nice and smoothly and the servos work as expected. However, at random intervals after starting--any where from 10 seconds to 10 minutes--I get a timeout exception and my control program grinds to a halt and the robot freezes. I have tried eliminating hardware problems by swapping out cables, servos, the USB-Serial cable and even the CM-5 but with no effect. I have the timeout on the serial port set to 1000 msec which I assume should be more than enough for 10 servos.

Fortunately, the Forest Moon library comes with source code and I was able to isolate where the timeout is occurring. It happens always on the same line within the ReadPacket method. I include the code block below to illustrate:

Code: Select all

protected byte[] ReadPacket(out int id)
      {
         // ReadPacket is only ever called immediately following a WriteInstruction (sent packet)
         // So we can use this opportunity to time the response from the Dynamixel.

         System.Diagnostics.Stopwatch watch = System.Diagnostics.Stopwatch.StartNew();

         // set an invalid id for error return cases
         id = 0xFF;
         //
         // status packet returned from Dynamixel servo:
         // 0      1      2    3        4       5            5 + data-length
         // [0xFF] [0xFF] [id] [length] [error] [...data...] [checksum]
         //
         // 1st header byte
            int b = 0;

            try
            {
                b = Stream.ReadByte();
            }
            catch (System.TimeoutException)
            {
                Console.WriteLine("TIMEOUT 1");
            }

         // Stop the Stopwatch and record the statistics for later
         watch.Stop();
         ResponseTotalElapsed += watch.ElapsedMilliseconds;
         ResponseCount++;
         if (watch.ElapsedMilliseconds > ResponseMaxElapsed)
            ResponseMaxElapsed = watch.ElapsedMilliseconds;

         if (b != 0xFF)
         {
            ++ErrorCnt1stHeaderByte;
            return null;
         }
         // 2nd header byte
            try
            {
                b = Stream.ReadByte();
            }
            catch (System.TimeoutException)
            {
                Console.WriteLine("TIMEOUT 2");
            }

         if (b != 0xFF)
         {
            ++ErrorCnt2ndHeaderByte;
            return null;
         }
         // id (or 3rd header byte??)
            try
            {
                b = Stream.ReadByte();
            }
            catch (System.TimeoutException)
            {
                Console.WriteLine("TIMEOUT 3");
            }

         if (b == 0xFF)      // have seen a third header byte! not sure why/how
         {
            ++ErrorCnt3rdHeaderByte;
                try
                {
                    b = Stream.ReadByte();
                }
                catch (System.TimeoutException)
                {
                    Console.WriteLine("TIMEOUT 4");
                }
         }
         id = b;
         // packet length (includes data-length plus 2)
         int len = Stream.ReadByte() - 2;
         if (len <0> 0)
         {
            // read the data, if any
            data = new byte[len];
            int offset = 0;
            while (len > 0)
            {
               int cnt = Stream.Read(data, offset, len);
               len -= cnt;
               offset += cnt;
            }
         }
         // get the CheckSum byte
         // CONSIDER: Could validate the checksum and reject the packet.
         int CheckSum = Stream.ReadByte();
         // let anyone listening know about any errors reported in the packet
         // use the 'InErrorHandler' flag to avoid recursion from the user's handler
         if (error != 0 && DynamixelError != null && !InErrorHandler)
         {
            InErrorHandler = true;
            DynamixelError(this, new DynamixelErrorArgs(id, error));
            InErrorHandler = false;
         }
         return data;
      }

The try-catch blocks were added by me to find out where the timeout is occurring and it always occurs at the point where I print out the message "TIMEOUT 1".

Ideally I would like to eliminate the timeouts altogether but I'd also settle for being able to recover gracefully from a timeout when it is caught. Unfortunately, I don't know what I would add to the try-catch block to recover control.

If anyone has any suggestions, I would be most grateful!

Many thanks,
patrick

http://www.pirobot.org

[i-Bot]

If you timeout after around 100ms, simply resend the command and try again. You may need to purge the receive buffer.

[pirobot]

Thanks i-Bot! This is exactly the tip I needed. I modified the ReadData method as shown below and it works like a charm. I can even unplug a cable, see the"Time Out" message, then plug the cable back in and the robot continues where it left off.

Thanks again!
patrick

http://www.pirobot.org

Code: Select all

protected byte[] ReadData(int id, Register startAddress, int count)
{
   List<byte> cmd = new List<byte>();
   byte[] returnPacket = null;
   bool retry = true;

   // the start address and count form the parameters for the command
   // packet
   cmd.Add((byte)startAddress);
   cmd.Add((byte)count);
   while (retry)
   {
       WriteInstruction(id, Instruction.ReadData, cmd);
       try
      {
         returnPacket = ReadPacket(id, count);
         retry = false;
      }
      catch (System.TimeoutException)
      {
          Console.WriteLine("TIME OUT accessing ID: " + id);
          Stream.Flush();
      }
   }
   return returnPacket;
}

[pirobot]

Just an update on how I finally solved the timeout issue I was having. Scott Ferguson, author of the Dynamixel library I am using (http://www.forestmoon.com/Software/AX12ArmSample/), kindly pointed out to me that there was no need to use the CM-5's RS-232 connection since I have a USB2Dynamixel device. Controlling the Dynamixels over the TTL connection instead of RS-232 instantly resolved all my timeout problems. Even better, I can now run at 1 Mb/s instead of 56k and I no longer have to insert delays between queries to the Dynamixels. So the bottom line is: don't use a Serial-USB converted and then attach it to the CM-5's serial port. Use the TTL connection instead. (Although I still use the CM-5 as a power hub and bus distribution point.)

--patrick

http://www.pirobot.org