Skip to content
Snippets Groups Projects
dynamixel_master.c 6.34 KiB
#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 short int timeout_left=master->rx_timeout_ms*1000;
  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->set_rx_mode();
}

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

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

}

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)
{

}

unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
{

}

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

}

unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data)
{

}

unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data)
{

}

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

}

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

}

unsigned char dyn_master_action(TDynamixelMaster *master)
{

}

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 dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
{

}