dynamixel_slave.c

#include "dynamixel_slave.h"

/* private functions */
unsigned char dyn_slave_irq_receive_cb(void *dyn_slave,unsigned char byte)
{
  TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave;

  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==0xFD)// version 2 header
            {
              if(dyn->version==DYN_VER2)// the module is configured for version 2
              {
                dyn->rx_buffer[dyn->received_bytes]=byte;
                dyn->received_bytes++;
              }
              else
                dyn->received_bytes=0;// ignore packet and restart synchronization
            }
            else if(byte!=0xFF)
            {
              dyn->rx_buffer[dyn->received_bytes]=byte;
	      dyn->received_bytes++;
            }
	    break;
    case 3: dyn->rx_buffer[dyn->received_bytes]=byte;
            if(dyn->version==DYN_VER1)
            {
              dyn->op_length=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],dyn->op_length);
            }
            else
              dyn->received_bytes++;
	    break;
    case 4: dyn->rx_buffer[dyn->received_bytes]=byte;
            dyn->received_bytes++;
            break;
    case 5: dyn->rx_buffer[dyn->received_bytes]=byte;
            dyn->received_bytes++;
            dyn->op_length=byte;
            break;
    case 6: dyn->rx_buffer[dyn->received_bytes]=byte;
            dyn->received_bytes++;
            dyn->op_length+=(byte<<8);
            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[7],dyn->op_length);
            break;
    default: break;
  }

  return 0x00;
}

unsigned char dyn_slave_dma_send_cb(void *dyn_slave)
{
  TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave;

  // enable tx interrupts
  dyn->set_rx_mode();  

  return 0x00;
}

unsigned char dyn_slave_dma_receive_cb(void *dyn_slave)
{
  TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave;

  comm_receive_irq(dyn->comm_dev,0);// reenable reception by IRQ
  if(dyn->comm_dev->time!=0x00000000)
    time_cancel_timeout(dyn->comm_dev->time);
  dyn->packet_ready=0x01;

  return 0x00;
}

void dummy_dyn_slave_set_tx_mode(void)
{

}

void dummy_dyn_slave_set_rx_mode(void)
{

}

void dummy_dyn_slave_on_ping(void)
{

}

unsigned char dummy_dyn_slave_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
{
  return DYN_SUCCESS;
}

unsigned char dummy_dyn_slave_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
{
  return DYN_SUCCESS;
}

unsigned char dummy_dyn_slave_on_reset(void)
{
  return DYN_SUCCESS;
}

unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
{
  return DYN_NO_DEVICE;
}


void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,unsigned short int length, unsigned char *data)
{
  // wait until the previous transmission has ended (if any)
  while(comm_is_send_done(slave->comm_dev)==COMM_BUSY);
  if(slave->return_delay>0)
    if(slave->comm_dev->time!=0x00000000)
      time_delay_us(slave->comm_dev->time,slave->return_delay<<1);
  if(slave->version==DYN_VER1)
  {
    // create the status packet
    dyn_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
    // set the tx mode, if necessary
    slave->set_tx_mode();
    // start transmission by DMA
    comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
  }
  else
  {
    // create the status packet
    dyn2_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
    // set the tx mode, if necessary
    slave->set_tx_mode();
    // start transmission by DMA
    comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
  }
}

void dyn_v1_slave_loop(TDynamixelSlave *slave)
{
  static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,length,address,prev_id,id;

  id=dyn_get_id(slave->rx_buffer);
  if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
  {
    // check the packet checksum
    if(dyn_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay
    {
      // process the packet
      switch(dyn_get_instruction(slave->rx_buffer))
      {
        case DYN_PING: slave->on_ping();
                       if(id!=DYN_BROADCAST_ID)
                         dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
                       break;
        case DYN_READ: error=slave->on_read(dyn_get_read_address(slave->rx_buffer),dyn_get_read_length(slave->rx_buffer),data);
                       if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
                       {
                         if(error==DYN_NO_ERROR)
                           dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn_get_read_length(slave->rx_buffer),data);
                         else
                           dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                       }
                       break;
        case DYN_WRITE: length=dyn_get_write_data(slave->rx_buffer,data);
                        error=slave->on_write(dyn_get_write_address(slave->rx_buffer),length,data);
                        if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
                        {
                          if(error==DYN_NO_ERROR)
                            dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
                          else
                            dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                        }
                        break;
        case DYN_REG_WRITE: slave->reg_length=dyn_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
                            slave->reg_address=dyn_get_reg_write_address(slave->rx_buffer);
                            if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
                              dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
                            break;
        case DYN_ACTION: if(slave->reg_address!=0xFFFF)
                         {
                           error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
                           slave->reg_address=0xFFFF;
                         }
                         else
                           if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
                             dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                         break;
        case DYN_RESET:
                        break;
        case DYN_SYNC_READ: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                            break;
        case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
                               error=slave->on_write(address,length,data);
                             break;
        case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
                            if(prev_id!=0xFF)
                            {
                              if(prev_id==0x00)// first device to answer
                              {
                                error=slave->on_read(address,length,data);
                                if(error==DYN_NO_ERROR)
                                  dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
                                else
                                  dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                              }
                              else// wait for the previous device in the sequence to send its data
                              {
                                slave->sync_bulk_address=address;
                                slave->sync_bulk_length=length;
                                slave->sync_bulk_prev_id=prev_id;
                                slave->bulk_read_pending=0x01;       
                              }
                            }
                            break;
        case DYN_BULK_WRITE: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                             break;
        default:
                 break;
      }
    }
    else
    {
      // send a checksum error answer
      if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
        dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
    }
  }
  else
  {
    if(slave->bulk_read_pending)
    {
      if(id==slave->sync_bulk_prev_id)
      {
        slave->bulk_read_pending=0x00;
        error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
        if(error==DYN_NO_ERROR)
          dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
        else
          dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
      }
    }
    else// the packet is addressed to another device, so relay it
    {
      if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
      {
        // set the tx mode, if necessary
        slave->set_tx_mode();
        // start transmission by DMA
        comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
      }
    }
  }
}

void dyn_v2_slave_loop(TDynamixelSlave *slave)
{
  static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,prev_id,id;
  static unsigned short int length,address;

  id=dyn2_get_id(slave->rx_buffer);
  if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
  {
    // check the packet checksum
    if(dyn2_check_checksum(slave->rx_buffer)==0x01)// the incomming packet is okay
    {
      // process the packet
      switch(dyn2_get_instruction(slave->rx_buffer))
      {
        case DYN_PING: slave->on_ping();
                       if(id!=DYN_BROADCAST_ID)
                         dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
                       break;
        case DYN_READ: error=slave->on_read(dyn2_get_read_address(slave->rx_buffer),dyn2_get_read_length(slave->rx_buffer),data);
                       if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
                       {
                         if(error==DYN_NO_ERROR)
                           dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn2_get_read_length(slave->rx_buffer),data);
                         else
                           dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                       }
                       break;
        case DYN_WRITE: length=dyn2_get_write_data(slave->rx_buffer,data);
                        error=slave->on_write(dyn2_get_write_address(slave->rx_buffer),length,data);
                        if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
                        {
                          if(error==DYN_NO_ERROR)
                            dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
                          else
                            dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                        }
                        break;
        case DYN_REG_WRITE: slave->reg_length=dyn2_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
                            slave->reg_address=dyn2_get_reg_write_address(slave->rx_buffer);
                            if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
                              dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
                            break;
        case DYN_ACTION: if(slave->reg_address!=0xFFFF)
                         {
                           error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
                           slave->reg_address=0xFFFF;
                         }
                         else
                           if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
                             dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                         break;
        case DYN_RESET:
                        break;
        case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(slave->rx_buffer,slave->address,&address,&length);
                            if(prev_id!=0xFF)
                            {
                              if(prev_id==0x00)// first device to answer
                              {
                                error=slave->on_read(address,length,data);
                                if(error==DYN_NO_ERROR)
                                  dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
                                else
                                  dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                              }
                              else// wait for the previous device in the sequence to send its data
                              {
                                slave->sync_bulk_address=address;
                                slave->sync_bulk_length=length;
                                slave->sync_bulk_prev_id=prev_id;
                                slave->sync_read_pending=0x01;
                              }
                            }
                            break;
        case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
                               error=slave->on_write(address,length,data);
                             break;
        case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
                            if(prev_id!=0xFF)
                            {
                              if(prev_id==0x00)// first device to answer
                              {
                                error=slave->on_read(address,length,data);
                                if(error==DYN_NO_ERROR)
                                  dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
                                else
                                  dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
                              }
                              else// wait for the previous device in the sequence to send its data
                              {
                                slave->sync_bulk_address=address;
                                slave->sync_bulk_length=length;
                                slave->sync_bulk_prev_id=prev_id;
                                slave->bulk_read_pending=0x01;       
                              }
                            }
                            break;
        case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))
                               error=slave->on_write(address,length,data);
                             break;
      
        default:
                 break;
      }
    }
    else
    {
      // send a checksum error answer
      if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
        dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
    }
  }
  else
  {
    if(slave->bulk_read_pending)
    {
      if(id==slave->sync_bulk_prev_id)
      {
        slave->bulk_read_pending=0x00;
        error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
        if(error==DYN_NO_ERROR)
          dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
        else
          dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
      }
    }
    else if(slave->sync_read_pending)
    {
      if(id==slave->sync_bulk_prev_id)
      {
        slave->sync_read_pending=0x00;
        error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
        if(error==DYN_NO_ERROR)
          dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
        else
          dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
      }
    }
    else// the packet is addressed to another device, so relay it
    {
      if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS)
      {
        // set the tx mode, if necessary
        slave->set_tx_mode();
        // start transmission by DMA
        comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+8);
      }
    }
  }
}

/* public functions */
void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version)
{
  /* assign communication functions */
  dev->irq_receive_cb=dyn_slave_irq_receive_cb;
  dev->dma_send_cb=dyn_slave_dma_send_cb;
  dev->dma_receive_cb=dyn_slave_dma_receive_cb;
  slave->comm_dev=dev;
  slave->version=version;
  dev->data=slave;
  /* initialize the internal callbacks */
  slave->set_tx_mode=dummy_dyn_slave_set_tx_mode;
  slave->set_rx_mode=dummy_dyn_slave_set_rx_mode;
  slave->on_ping=dummy_dyn_slave_on_ping;
  slave->on_read=dummy_dyn_slave_on_read;
  slave->on_write=dummy_dyn_slave_on_write;
  slave->on_reset=dummy_dyn_slave_on_reset;
  slave->on_relay=dummy_dyn_slave_on_relay;
  /* initialize internal variables */
  slave->address=address;
  slave->return_delay=0x00;
  slave->return_level=return_all;
  slave->packet_ready=0x00;
  slave->rx_timeout_ms=50;
  slave->received_bytes=0x00;
  slave->reg_address=0xFFFF;
  slave->reg_length=0x0000;
  slave->sync_bulk_address=0x0000;
  slave->sync_bulk_length=0x0000;
  slave->sync_bulk_prev_id=0x00;
  slave->sync_read_pending=0x00;
  slave->bulk_read_pending=0x00;

  slave->set_rx_mode();
  /* enable reception by IRQ */
  comm_receive_irq(slave->comm_dev,0);
}

void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
{
  slave->rx_timeout_ms=timeout_ms;
}

inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address)
{
  slave->address=address;
}

inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave)
{
  return slave->address;
}

inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay)
{
  slave->return_delay=delay;
}

inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave)
{
  return slave->return_delay;
}

inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level)
{
  slave->return_level=level;
}

inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave)
{
  return slave->return_level;
}

inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
{
  return slave->version;
}

void dyn_slave_loop(TDynamixelSlave *slave)
{
  if(slave->packet_ready)// check if a new instruction packet has been received
  {
    slave->packet_ready=0x00;
    // check address
    if(slave->version==DYN_VER1)
      dyn_v1_slave_loop(slave);
    else
      dyn_v2_slave_loop(slave);
  }
  else
  {
    if(slave->comm_dev->time!=0x00000000)
    {
      if(time_is_timeout(slave->comm_dev->time))
      { 
        /* cancel any IRQ or DMA reception */
        comm_cancel_irq_receive(slave->comm_dev);
        comm_cancel_dma_receive(slave->comm_dev);
        slave->received_bytes=0;
        /* enable reception by IRQ */
        comm_receive_irq(slave->comm_dev,0);
      }
    }
  }
}