AX-12 + usb2dynamixel + libftdi (Linux)

[Mandor]

Hi

I'm trying to use my brand new usb2dynamixel. I 've read here that the best way to go was to use libftdi (low latency). I'm trying to ping my dynamixel but I don't see any answer... Here is my current code:

Code: Select all


#include <stdio>
#include <ftdi>
#include "ax12.h"

int main(int argc, char **argv)
{
  int ret;
  struct ftdi_context ftdic;
  ftdi_init(&ftdic);
 
  if((ret = ftdi_usb_open(&ftdic, 0x0403, 0x6001)) < 0)
    {
      fprintf(stderr, "unable to open ftdi device: %d (%s)\n", ret, ftdi_get_error_string(&ftdic));
      return EXIT_FAILURE;
    }
 
  // Read out FTDIChip-ID of R type chips
  if (ftdic.type == TYPE_R)
    {
      unsigned int chipid;
      printf("ftdi_read_chipid: %d\n", ftdi_read_chipid(&ftdic, &chipid));
      printf("FTDI chipid: %X\n", chipid);
    }
 
  if ((ret = ftdi_set_baudrate(&ftdic, 1000000)) != 0)
    {
      fprintf(stderr, "error setting baudrate, ret=%d\n", ret);     
    }
  if (ftdi_set_line_property(&ftdic, BITS_8, STOP_BIT_1, NONE) == -1)
    {
      fprintf(stderr, "error setting properties");
    }
  if (ftdi_setflowctrl(&ftdic, SIO_DISABLE_FLOW_CTRL) == -1)
    {
      fprintf(stderr, "error setting flow control");
    }

  ftdi_write_data_set_chunksize(&ftdic, 1);
  ftdi_read_data_set_chunksize(&ftdic, 1);
  // ping servos
  for (unsigned char i = 0; i < 30; ++i)
    {
      unsigned char buf[] = { 0xff, 0xff, i, 0x02, 0x01, 0xf3 };
      printf("id=%d\n", i);
      unsigned char checksum = 0;
      int len = 6;
      for(size_t i = 2; i < len - 1; i++)
   checksum += buf[i];
     
      buf[5] = ~checksum;
     
     
      ret = ftdi_write_data(&ftdic, buf, len);
      printf("ret=%d\n", ret);
    }
  unsigned char b;
  ret = 0;
  do
    {
      ret = ftdi_read_data(&ftdic, &b, 1);
    }
  while (ret == 0);
  printf ("ret = %d\n", ret);
 
  ftdi_usb_close(&ftdic);
  ftdi_deinit(&ftdic);
 
  return EXIT_SUCCESS;
}


ret is 0 and consequently the code never ends. Any idea ? does anybody here use usb2dynamixel and Linux ?

Thanks.

--
Mandor

[Mandor]

Actually, I get a reply when I change the chunck size to 6...

[limor]

Hi Mandor,
thanks for posting the code.
can you please measure the latecy?
you can use gettimeofday and run the ping few hundred times

Also, please set with robot terminal the delay-response time of servos. The value can go as low as 0. (see the servo dynamixel documentation)

thanks
Limor

[Mandor]

Hi Limor,
Here is a working code :

Code: Select all


#include <stdio>
#include <ftdi>
#include <sys>

int main(int argc, char **argv)
{
  int ret;
  struct ftdi_context ftdic;
  ftdi_init(&ftdic);
  // reset everything in case we messed everything up with a control-c..
  if((ret = ftdi_usb_open(&ftdic, 0x0403, 0x6001)) < 0)
    {
      fprintf(stderr, "unable to open ftdi device: %d (%s)\n", ret, ftdi_get_error_string(&ftdic));
      return EXIT_FAILURE;
    }
  ftdi_usb_purge_buffers(&ftdic);
  usb_reset(ftdic.usb_dev);

  ftdi_usb_close(&ftdic);
 
  // real code starts here
  if((ret = ftdi_usb_open(&ftdic, 0x0403, 0x6001)) < 0)
    {
      fprintf(stderr, "unable to open ftdi device: %d (%s)\n", ret, ftdi_get_error_string(&ftdic));
      return EXIT_FAILURE;
    }


  // Read out FTDIChip-ID of R type chips
  if (ftdic.type == TYPE_R)
    {
      unsigned int chipid;
      printf("ftdi_read_chipid: %d\n", ftdi_read_chipid(&ftdic, &chipid));
      printf("FTDI chipid: %X\n", chipid);
    }

  // 1 Mb
  if ((ret = ftdi_set_baudrate(&ftdic, 1000000)) != 0)
    fprintf(stderr, "error setting baudrate, ret=%d\n", ret);     
  if (ftdi_set_line_property(&ftdic, BITS_8, STOP_BIT_1, NONE) == -1)
    fprintf(stderr, "error setting properties");
  if (ftdi_setflowctrl(&ftdic, SIO_DISABLE_FLOW_CTRL) == -1)
      fprintf(stderr, "error setting flow control");
 
  ftdi_write_data_set_chunksize(&ftdic, 3);
  ftdi_read_data_set_chunksize(&ftdic, 256);

  struct timeval tv;
  gettimeofday(&tv, 0x0);

  for (size_t k = 0; k < 100; ++k)
    {
      ret = 0;
      // ping servos
      unsigned char buf[] = { 0xff, 0xff, 17, 0x02, 0x1, 0xf3 };
      unsigned char checksum = 0;
      int len = 6;
      for(size_t i = 2; i < len - 1; i++)
   checksum += buf[i];     
      buf[5] = ~checksum;
     
      int ret = ftdi_write_data(&ftdic, buf, len);

      unsigned char b;
      int ret2 = 0;
      unsigned char buffer[20];
      do
   {
     ret2 += ftdi_read_data(&ftdic, buffer, 1);
   }
      while (ret2 != ret);
    }
  struct timeval tv2;
  gettimeofday(&tv2, 0x0);
  long sec = tv2.tv_sec - tv.tv_sec;
  long us = tv2.tv_usec - tv.tv_usec; 
  float t = (sec + us * 1e-6) / 100;
  printf("time: %f\n", t);
  ftdi_usb_close(&ftdic);
  ftdi_deinit(&ftdic);
 
  return EXIT_SUCCESS;
}


without changing the delay-response (I have no Windows machine to use the robot-terminal; and no CM-5...), I reliably get:
ftdi_read_chipid: 0
FTDI chipid: E35BFCF9
time: 0.015821

That's sounds good

I will try to change the delay asap.

[limor]

63 packet round-trips is not very good. In the past we have seen 10 or even 20 times better performance.
You can change the response delay register in the servo using your code
just change the outbound packet from PING to set the relevant register(s) on the servo.

The robot terminal firmware on the CM5 sends packets like you do to the servos, on demand, allowing users to interactively send these packets and get the response interpreted as text. (one serial port talks to the servos, the other talks to the PC).