Statistics: Posted by pirobot — Wed Mar 03, 2010 6:47 pm

Code:

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;
}

Statistics: Posted by pirobot — Thu Feb 25, 2010 5:27 pm

Code:

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

Code:

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;
      }

Statistics: Posted by pirobot — Thu Feb 25, 2010 1:20 am