dynamixelserver.cpp

#include "dynamixelexceptions.h"
#include "dynamixelserver.h"
#include "eventexceptions.h"
#include "ftdiserver.h"
#include <sstream>

CDynamixelServer *CDynamixelServer::pinstance=NULL;

CDynamixelServer::CDynamixelServer()
{
  this->event_server=CEventServer::instance();
  this->stop_scan_event_id="dynamixel_server_stop_scan_event";
  this->event_server->create_event(this->stop_scan_event_id);
  this->scan_done_event_id="dynamixel_server_scan_done_event";
  this->event_server->create_event(this->scan_done_event_id);
  this->thread_server=CThreadServer::instance();
  this->scan_thread_id="dynamixel_server_scan_thread";
  this->thread_server->create_thread(this->scan_thread_id);
  this->thread_server->attach_thread(this->scan_thread_id,this->scan_thread,this);
  this->state=dyn_created;
  this->devices.clear();
  this->comm_dev=NULL; 
  this->bus_info.baud_rate=-1;
  this->bus_info.bus_id=-1;
}

CDynamixelServer::CDynamixelServer(const CDynamixelServer& object)
{

}

CDynamixelServer& CDynamixelServer::operator = (const CDynamixelServer& object)
{
  return *this->pinstance;
}

CDynamixelServer *CDynamixelServer::instance(void)
{
  if (CDynamixelServer::pinstance == NULL)
  {
    CDynamixelServer::pinstance = new CDynamixelServer(); // Creamos la instancia
  }
  return CDynamixelServer::pinstance; // Retornamos la dirección de la instancia
}

unsigned char CDynamixelServer::compute_checksum(unsigned char *packet,int len)
{
  int i=0;
  short int checksum=0;

  if(packet==NULL)
  {
    /* handle exceptions */
    throw CDynamixelServerException(_HERE_,"Invalid packet structure");
  }
  else
  {
    if(len<=0)
    {
      /* handle exceptions */
      throw CDynamixelServerException(_HERE_,"Invalid packet length");
    }
    else
    {
      for(i=2;i<len;i++)
        checksum+=packet[i];
      checksum=~checksum;
      checksum=checksum%256;
    }
  }

  return checksum;
}

void *CDynamixelServer::scan_thread(void *param)
{
  CDynamixelServer *dyn_server=(CDynamixelServer *)param;
  CFTDIServer *ftdi_server=CFTDIServer::instance();
  std::string serial;
  TDynDevice device;
  int freq=0,id=0;
  bool end=false,found=false;

  dyn_server->close();
  dyn_server->state=dyn_scanning;
  while(!end)
  {
    try{
      serial=ftdi_server->get_serial_number(dyn_server->bus_info.bus_id);
      dyn_server->comm_dev=ftdi_server->get_device(serial);
      for(freq=0;freq<9;freq++)
      {
        dyn_server->set_baudrate(frequencies[freq]);
        for(id=0;id<0xFD;id++)
        {
          if(dyn_server->event_server->event_is_set(dyn_server->stop_scan_event_id))
          {
            dyn_server->event_server->reset_event(dyn_server->stop_scan_event_id);
            dyn_server->close();
            pthread_exit(NULL);
          }
          else
          {
            try{
              dyn_server->ping(id,200);
            }catch(CEventTimeoutException &e){
              continue;
            }
            dyn_server->dynamixel_access.enter();
            dyn_server->bus_info.baud_rate=frequencies[freq];
            device.id=id;
            device.used=false;
            dyn_server->dynamixel_access.exit();
            dyn_server->devices.push_back(device);
            found=true;
          }
        }
        if(found)
        {
          end=true;
          break;
        }
      }
    }catch(CException &e){
      dyn_server->close();
      pthread_exit(NULL);
    }
  }
  dyn_server->dynamixel_access.enter();
  dyn_server->state=dyn_scan_done;
  dyn_server->event_server->set_event(dyn_server->scan_done_event_id);
  dyn_server->dynamixel_access.exit();
  pthread_exit(NULL);
}

int CDynamixelServer::get_num_buses(void)
{
  CFTDIServer *ftdi_server=CFTDIServer::instance();
  int num_dev=0,i=0,num_buses=0;
  std::string description;

  this->dynamixel_access.enter();
  try{
    num_dev=ftdi_server->get_num_devices();
    for(i=0;i<num_dev;i++)
    {
      description=ftdi_server->get_description(i);
      if(description=="FT232R USB UART")
        num_buses++; 
    }
  }catch(CException &e){
    /* handle exceptions */
    this->dynamixel_access.exit();
    throw;
  }
  this->dynamixel_access.exit();

  return num_buses;
}

int CDynamixelServer::get_bus_id(void)
{
  return this->bus_info.bus_id;
}

void CDynamixelServer::set_bus_id(int bus_id)
{
  CFTDIServer *ftdi_server=CFTDIServer::instance();
  std::string serial;

  if(bus_id>(this->get_num_buses()-1))
  {
    /* handle exception */
    throw CDynamixelServerException(_HERE_,"Invalid bus identifier");
  }
  else
  {
    if(this->bus_info.bus_id!=bus_id)
    {
      if(this->comm_dev!=NULL)
      {
        this->comm_dev->close();
        delete this->comm_dev;
        this->comm_dev=NULL;
      }
      serial=ftdi_server->get_serial_number(bus_id);
      this->comm_dev=ftdi_server->get_device(serial);
      // configure the communciation device by default
      this->set_baudrate(1000000); 
      this->bus_info.bus_id=bus_id;
    }
  } 
} 

void CDynamixelServer::start_scan(void)
{
  if(this->bus_info.bus_id==-1)
  {
    /* handle exceptions */
    throw CDynamixelServerException(_HERE_,"No bus has been selected.");
  }
  else
  {
    this->stop_scan();
    this->close();
    this->thread_server->start_thread(this->scan_thread_id);
  }
}

void CDynamixelServer::stop_scan(void)
{
  if(this->state==dyn_scanning)
  {
    this->event_server->set_event(this->stop_scan_event_id);
    this->thread_server->end_thread(this->scan_thread_id);
    this->bus_info.bus_id=-1;
  } 
}

bool CDynamixelServer::is_scan_done(void)
{
  if(this->bus_info.bus_id!=-1)
  {
    if(this->state==dyn_scan_done)
      return true;
    else 
      return false;
  }
  else 
  {
    /* handle exceptions */
    throw CDynamixelServerException(_HERE_,"No bus has been selected.");
  }
}

std::string CDynamixelServer::get_scan_done_event_id(void)
{
  return this->scan_done_event_id;
}

int CDynamixelServer::get_baudrate(void)
{
  int baud_rate=0;

  this->dynamixel_access.enter();
  baud_rate=this->bus_info.baud_rate;
  this->dynamixel_access.exit();

  return baud_rate;
}

int CDynamixelServer::get_num_devices(void)
{
  int num=0;

  this->dynamixel_access.enter();
  num=this->devices.size();
  this->dynamixel_access.exit();

  return num;
}

std::vector<int> CDynamixelServer::get_device_ids(void)
{
  std::vector<int> ids;
  int i=0;

  this->dynamixel_access.enter();
  for(i=0;i<this->devices.size();i++)
    ids.push_back(this->devices[i].id);
  this->dynamixel_access.exit();

  return ids;
}

CDynamixel *CDynamixelServer::get_device(int dev_id)
{
  std::stringstream device_name;
  CDynamixel *dynamixel=NULL;
  std::string name,serial;
  bool updated=false;
  TDynDevice device;
  int i=0;

  this->dynamixel_access.enter();
  if(this->bus_info.bus_id==-1)
  {
    this->dynamixel_access.exit();
    /* handle exceptions */
    throw CDynamixelServerException(_HERE_,"No bus has been selected.");
  }
  else
  {
    for(i=0;i<this->devices.size();i++)
      if(this->devices[i].id==dev_id)
      {
        if(this->devices[i].used)
        {
          /* handle exceptions */
          this->dynamixel_access.exit();
          throw CDynamixelServerException(_HERE_,"Device has already been assigned.");
        }
      }
  }
  this->dynamixel_access.exit();
  if(dynamixel==NULL)
  {
    // try to ping the desired device
    try{
      if(this->bus_info.baud_rate==-1)
      {
        /* handle exceptions */
        throw CDynamixelServerException(_HERE_,"No baudrate has been selected.");
      }
      else
      {
        this->ping(dev_id,500);
        this->dynamixel_access.enter();
        device_name.str("");
        device_name << "dynamixel_bus_" << this->bus_info.bus_id << "_dev_" << dev_id;
        name=device_name.str();
        dynamixel=new CDynamixel(name);
        dynamixel->usb_dev=this->comm_dev;
        dynamixel->usb_access=&this->dynamixel_access;
        dynamixel->node_address=dev_id;
        dynamixel->usb_rx_event_id=this->comm_dev->get_rx_event_id();
        for(i=0;i<devices.size();i++)
          if(this->devices[i].id==dev_id)
          {
            this->devices[i].used=true;
            updated=true;
          }
        if(!updated)
        {
          device.id=dev_id;
          device.used=true;
          this->devices.push_back(device);
        }
        this->dynamixel_access.exit();
      }
    }catch(CEventTimeoutException &e){
      this->dynamixel_access.exit();
      /* handle exceptions */
      throw CDynamixelServerException(_HERE_,"No Dynamixel device found with the specified identifier");
    }
  }
  
  return dynamixel;
}

void CDynamixelServer::free_device(int dev_id)
{
  int i=0;

  for(i=0;i<this->devices.size();i++)
  {
    if(this->devices[i].id==dev_id)
    {
      if(this->devices[i].used)
        this->devices[i].used=false;
    }
  }
}

void CDynamixelServer::set_baudrate(int baudrate)
{
  TFTDIconfig ftdi_config;
  unsigned char *packet;
  int i=0,length=0;

  if(this->comm_dev!=NULL)
  {
    if(baudrate <= 0 || baudrate > 1000000)
    {
      /* handle exceptions */
      throw CDynamixelServerException(_HERE_,"Invalid baudrate");
    }
    else
    {
      try{
        if(this->devices.size()>0)
        {
          length=8+2*devices.size();
          packet=new unsigned char[length];
          packet[0]=0xFF;
          packet[1]=0xFF;
          packet[2]=0XFE;
          packet[3]=length-4;
          packet[4]=0x83;
          packet[5]=0x04;
          packet[6]=0x01;
          for(i=0;i<this->devices.size();i++)
          {
            packet[7+i*2]=devices[i].id;
            packet[7+i*2+1]=((2000000/baudrate)-1);
          }
          packet[length-1]=0x00;
          packet[length-1]=this->compute_checksum(packet,length);
          // send the data
          this->dynamixel_access.enter();
          if(this->comm_dev->write(packet,length)!=length)
          {
            this->dynamixel_access.exit();
            throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
          }
          delete[] packet;
          this->dynamixel_access.exit();
        }
        /* reconfigure the communciations device */
        ftdi_config.word_length=8;
        ftdi_config.stop_bits=1;
        ftdi_config.parity=0;
        ftdi_config.read_timeout = 1000;
        ftdi_config.write_timeout = 1000;
        ftdi_config.latency_timer = 1;
        ftdi_config.baud_rate=baudrate;
        this->comm_dev->config(&ftdi_config);
        this->bus_info.baud_rate=baudrate;
      }catch(CException &e){
        /* handle exceptions */
        throw;
      }
    }
  }
  else
  {
    /* handle exceptions */
    throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
  }
}

void CDynamixelServer::action(void)
{
  unsigned char packet[6]={0xFF,0xFF,0xFE,0x02,0x05,0x00};

  if(this->comm_dev!=NULL)
  {
    packet[5]=0xFA;
 
    this->dynamixel_access.enter();
    if(this->comm_dev->write(packet,6)!=6)
    {
      // handle exceptions
      this->dynamixel_access.exit();
      throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
    }
    this->dynamixel_access.exit();
  }
  else
  {
    /* handle exceptions */
    throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
  }
}

void CDynamixelServer::ping(int dev_id,int time)
{
  unsigned char packet[6]={0xFF,0xFF,0x00,0x02,0x01,0x00};
  std::list<std::string> events;
  unsigned char data[6];
  int num=0,read=0;

  if(this->comm_dev!=NULL)
  {
    this->dynamixel_access.enter();
    packet[2]=dev_id;
    packet[5]=(~(dev_id+3))%256;
    events.push_back(this->comm_dev->get_rx_event_id());

    if(this->comm_dev->write(packet,6)!=6)
    {
      /* handle exceptions */
      this->dynamixel_access.exit();
      throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
    }
    else
    {
      try{
        do{
          this->event_server->wait_all(events,time);
          num=this->comm_dev->read(&data[read],6-read);
          read+=num;
        }while(read<6);
      }catch(CEventTimeoutException &e){
        this->dynamixel_access.exit();
        throw;
      }
    }
    this->dynamixel_access.exit();
  }
  else
  {
    /* handle exceptions */
    throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
  }
} 

void CDynamixelServer::close(void)
{
  int i=0;

  this->dynamixel_access.enter();
  if(this->devices.size()>0)
  {
    this->devices.clear();
  }
  if(this->comm_dev!=NULL)
  {
    this->comm_dev->close();
    delete this->comm_dev;
    this->comm_dev=NULL;
  }
  this->state=dyn_created;
  this->bus_info.baud_rate=-1;
  this->dynamixel_access.exit();
}