stm32_dyn_master.c

#include "dynamixel_master.h"
#include "action.h"
#include "dyn_servos.h"

int num_servos;
double real_angles[PAGE_MAX_NUM_SERVOS];

/* public functions */
void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
{
}

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

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

TReturnLevel 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)
{
  if(master->version==DYN_VER1)
  {
    if(id!=0 && id<=num_servos)
      return DYN_SUCCESS;
    else
      return DYN_COMM_ERROR;
  }
  else
    return DYN_COMM_ERROR;
}

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

unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
{
  if(address==P_MODEL_NUMBER_L)
    *data=SERVO_MX28;
  else if(address==P_PRESENT_POSITION_L)
    *data=((((real_angles[id-1]*180.0)/3.143159)+180.0)*4096.0)/360.0;
  else if(address==P_CW_ANGLE_LIMIT_L)
    *data=0;
  else if(address==P_CCW_ANGLE_LIMIT_L)
    *data=4095;
  else
    *data=0x0000;

  return DYN_SUCCESS;
}

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

  for(i=0;i<length;i++)
    data[i]=0x00;

  return DYN_SUCCESS;
}

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

unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data)
{
  return DYN_SUCCESS;
}

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

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

unsigned char dyn_master_action(TDynamixelMaster *master)
{
  return DYN_SUCCESS;
}

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

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

  for(i=0;i<num;i++)
  {
    for(j=0;j<length;j++)
      data[i].data_addr[j]=0x00;
  }

  return DYN_SUCCESS;
}

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 int i,j;

  for(i=0;i<num;i++)
  {
    for(j=0;j<length[i];j++)
      data[i].data_addr[j]=0x00;
  }

  return DYN_SUCCESS;
}

unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
{
  return DYN_SUCCESS;
}

unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt)
{
  return DYN_SUCCESS;
}