dynamixel_master.c

#include "dynamixel_master.h"

/* private functions */
unsigned char dyn_master_irq_receive_cb(void *dyn_master,unsigned char byte)
{
  TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master;

  if(dyn->comm_dev->time!=0x00000000)
    time_set_timeout(dyn->comm_dev->time,dyn->rx_timeout_ms*1000);
  switch(dyn->received_bytes)
  {
    case 0: if(byte==0xFF)
            {
              dyn->rx_buffer[dyn->received_bytes]=byte;
              dyn->received_bytes++;
            }
            break;
    case 1: if(byte==0xFF)
            {
              dyn->rx_buffer[dyn->received_bytes]=byte;
              dyn->received_bytes++;
            }
            else
              dyn->received_bytes--;
            break;
    case 2: if(byte!=0xFF)
            {
              dyn->rx_buffer[dyn->received_bytes]=byte;
              dyn->received_bytes++;
            }
            break;
    case 3: dyn->rx_buffer[dyn->received_bytes]=byte;
            dyn->received_bytes=0;
            /* finish reception by IRQ */
            comm_cancel_irq_receive(dyn->comm_dev);
            /* enable dma RX */
            comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[4],byte);
            break;
    default: break;
  }

  return 0x00;
}

unsigned char dyn_master_dma_send_cb(void *dyn_master)
{
  TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master;

  dyn->set_rx_mode();
  if(dyn->rx_no_answer)
      dyn->rx_no_answer=0x00;
  else
  {
    /* enable RX interrupts */
    comm_receive_irq(dyn->comm_dev,0);
  }

  return 0x00;
}

unsigned char dyn_master_dma_receive_cb(void *dyn_master)
{
  TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master;

  dyn->rx_num_packets--;
  if(dyn->rx_num_packets==0x00)
  {
    if(dyn->comm_dev->time!=0x00000000)
      time_cancel_timeout(dyn->comm_dev->time);
    dyn->packet_ready=0x01;
  }
  else
  {
    /* enable RX interrupts */
    comm_receive_irq(dyn->comm_dev,0);
  }

  return 0x00;
}

void dummy_dyn_master_set_tx_mode(void)
{

}

void dummy_dyn_master_set_rx_mode(void)
{

}

unsigned char dyn_master_wait_transmission(TDynamixelMaster *master)
{
  unsigned char error;

  while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY);
  
  if(error==COMM_SUCCESS)
    return DYN_SUCCESS;
  else
    return DYN_COMM_ERROR;
}

unsigned char dyn_master_send(TDynamixelMaster *master)
{
  unsigned char error;

  // wait until any previous transmission ends
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
    return error;
  // set the DMA transfer
  comm_send_dma(master->comm_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4);
  return DYN_SUCCESS;
} 

unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
{
  if(master->comm_dev->time!=0x00000000)
  {
    // start the new timeout
    time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
  }
  // wait for the status packet
  while(!master->packet_ready)
  {
    if(master->comm_dev->time!=0x00000000)
    {
      if(time_is_timeout(master->comm_dev->time))
      {
        comm_cancel_irq_receive(master->comm_dev);
        comm_cancel_dma_receive(master->comm_dev);
        master->received_bytes=0x00;
        return DYN_TIMEOUT;
      }
    }
  }
  master->packet_ready=0x00;
  // check the input packet checksum
  if(dyn_check_checksum(master->rx_buffer)==0xFF)
    return dyn_get_status_error(master->rx_buffer);
  else
    return DYN_CHECKSUM_ERROR;
}

/* public functions */
void dyn_master_init(TDynamixelMaster *master,TComm *dev)
{
  /* assign communication functions */
  dev->irq_receive_cb=dyn_master_irq_receive_cb;
  dev->dma_send_cb=dyn_master_dma_send_cb;
  dev->dma_receive_cb=dyn_master_dma_receive_cb;
  master->comm_dev=dev;
  dev->data=master;
  /* initialize the internal callbacks */
  master->set_tx_mode=dummy_dyn_master_set_tx_mode;
  master->set_rx_mode=dummy_dyn_master_set_rx_mode;
  /* initialize internal variables */
  master->packet_ready=0x00;
  master->received_bytes=0x00;
  master->rx_timeout_ms=50;
  master->rx_no_answer=0x00;
  master->rx_num_packets=0x00;
  master->return_level=return_all;

  master->set_rx_mode();
}

void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
{
  master->rx_timeout_ms=timeout_ms;
}

inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level)
{
  master->return_level=level;
}

inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master)
{
  return master->return_level;
}

void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
{
  unsigned char i;

  *num=0;
  for(i=0;i<254;i++)
  {
    if(dyn_master_ping(master,i)==DYN_SUCCESS)// the device exists
    {
      ids[*num]=i;
      (*num)++;
    }
  }
}

unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
{
  unsigned char error;

  // generate the ping packet for the desired device
  dyn_init_ping_packet(master->tx_buffer,id);
  master->rx_num_packets=0x01;
  master->rx_no_answer=0x00;
  // enable transmission
  master->set_tx_mode();
  // send the data
  if((error=dyn_master_send(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the transmission to end
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the replay within the given timeout
  error=dyn_master_wait_reception(master);

  return error;
}

unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data)
{
  return dyn_master_read_table(master,id,address,1,data);
}

unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
{
  unsigned char error;
  unsigned char data_int[2];

  // generate the ping packet for the desired device
  error=dyn_master_read_table(master,id,address,2,data_int);
  (*data)=data_int[0]+data_int[1]*256;

  return error;
}

unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
{
  unsigned char error;

  // generate the read packet for the desired device
  dyn_init_read_packet(master->tx_buffer,id,address,length);
  master->rx_num_packets=0x01;
  if(master->return_level==no_return || id==DYN_BROADCAST_ID)
    master->rx_no_answer=0x01;
  else
    master->rx_no_answer=0x00;
  // enable transmission
  master->set_tx_mode();
  // send the data
  if((error=dyn_master_send(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the transmission to end
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the replay within the given timeout
  if(master->return_level!=no_return && id!=DYN_BROADCAST_ID)
  {
    if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
    {
      if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
        error=DYN_INST_ERROR;
    }
  } 

  return error;
}

unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data)
{
  return dyn_master_write_table(master,id,address,1,&data);
}

unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data)
{
  unsigned char data_int[2];

  data_int[0]=data%256;
  data_int[1]=data/256;
  return dyn_master_write_table(master,id,address,2,data_int);
}

unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
{
  unsigned char error;

  // generate the write packet for the desired device
  dyn_init_write_packet(master->tx_buffer,id,address,length,data);
  master->rx_num_packets=0x01;
  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
    master->rx_no_answer=0x00;
  else
    master->rx_no_answer=0x01;
  // enable transmission
  master->set_tx_mode();
  // send the data
  if((error=dyn_master_send(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the transmission to end
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the replay within the given timeout
  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
    error=dyn_master_wait_reception(master);

  return error;
}

unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
{
  unsigned char error;

  // generate the registered write packet for the desired device
  dyn_init_reg_write_packet(master->tx_buffer,id,address,length,data);
  master->rx_num_packets=0x01;
  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
    master->rx_no_answer=0x00;
  else
    master->rx_no_answer=0x01;
  // enable transmission
  master->set_tx_mode();
  // send the data
  if((error=dyn_master_send(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the transmission to end
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the replay within the given timeout
  if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
    error=dyn_master_wait_reception(master);

  return error;
}

unsigned char dyn_master_action(TDynamixelMaster *master)
{
  unsigned char error;

  // generate the action packet for the desired device
  dyn_init_action_packet(master->tx_buffer);
  master->rx_num_packets=0x01;
  master->rx_no_answer=0x01;
  // enable transmission
  master->set_tx_mode();
  // send the data
  if((error=dyn_master_send(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the transmission to end
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }

  return error;
}

unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
{
  unsigned char error;

  // generate the write packet for the desired device
  dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
  master->rx_num_packets=0x01;
  master->rx_no_answer=0x01;
  // enable transmission
  master->set_tx_mode();
  // send the data
  if((error=dyn_master_send(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the transmission to end
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }

  return error;
}

unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
{
  unsigned char error,i;

  // generate the read packet for the desired device
  dyn_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
  master->rx_num_packets=0x01;
  if(master->return_level==no_return)
    master->rx_no_answer=0x01;
  else
    master->rx_no_answer=0x00;
  // enable transmission
  master->set_tx_mode();
  // send the data
  if((error=dyn_master_send(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the transmission to end
  if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
  {
    master->set_rx_mode();
    return error;
  }
  // wait for the replay within the given timeout
  if(master->return_level!=no_return)
  {
    for(i=0;i<num;i++)
    {
      if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
      {
        if(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
          error=DYN_INST_ERROR;
      }
    }
  }

  return error;
}