Skip to content
Snippets Groups Projects
Select Git revision
  • devel
  • master default protected
  • cpp_simulator
3 results
Blame Permalink
motion_manager.c 16.81 KiB 
#include "motion_manager.h"
#include "motion_manager_registers.h"
#include "motion_module.h"
#include "ram.h"

/* private functions */
void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
{
  unsigned char reg_value[4]={0};

  if(reg->address!=0xFFFF)
  {
    dyn_master_read_table(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,reg->address,reg->size,reg_value);
    if(reg->size==1)
      (*value)=reg_value[0];
    else if(reg->size==2)
      (*value)=reg_value[0]+(reg_value[1]<<8);
    else if(reg->size==4)
      (*value)=reg_value[0]+(reg_value[1]<<8)+(reg_value[2]<<16)+(reg_value[3]<<24);
  }
}

void mmanager_write_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int value)
{
}

void mmanager_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
  TMotionManager *manager=(TMotionManager *)module;

  ram_read_table(manager->memory,address,length,data);
}

void mmanager_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
{
  TMotionManager *manager=(TMotionManager *)module;
  unsigned short int eeprom_offset,ram_offset,i,j;
  unsigned char byte_value,mod;

  eeprom_offset=address-manager->eeprom_base_address;
  if(ram_in_window(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES,address,length))
  {
    for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
    {
      if(ram_in_range(manager->eeprom_base_address+MM_SERVO_OFFSET_OFFSET+i,address,length))
        mmanager_set_offset(manager,i,(signed char)(data[MM_SERVO_OFFSET_OFFSET-eeprom_offset+i]));
    }
  }
  ram_offset=address-manager->ram_base_address;
  if(ram_in_window(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET,DYN_MANAGER_MAX_NUM_DEVICES/2,address,length))
  {
    for(i=0,j=0;i<DYN_MANAGER_MAX_NUM_DEVICES/2;i++,j+=2)
    {
      if(ram_in_range(manager->ram_base_address+MM_ENABLE_MODULE_OFFSET+i,address,length))
      {
        byte_value=data[MM_ENABLE_MODULE_OFFSET-ram_offset+i];
        if(byte_value&MM_EVEN_SER_EN) mmanager_enable_servo(manager,j);
        else mmanager_disable_servo(manager,j);
        mod=(byte_value&MM_EVEN_SER_MOD)>>4;
        mmanager_set_module(manager,j,(TModules)mod);
        if(byte_value&MM_ODD_SER_EN) mmanager_enable_servo(manager,j+1);
        else mmanager_disable_servo(manager,j+1);
        mod=byte_value&MM_ODD_SER_MOD;
        mmanager_set_module(manager,j+1,(TModules)mod);
      }
    }
  }
  ram_write_table(manager->memory,address,length,data);
}

unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle);
inline unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle)
{
  return ((angle+mmanager->servo_configs[servo_id]->center_angle)*mmanager->servo_configs[servo_id]->encoder_resolution)/mmanager->servo_configs[servo_id]->max_angle;
}

short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value);
inline short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value)
{
  return (((short int)((value*mmanager->servo_configs[servo_id]->max_angle)/mmanager->servo_configs[servo_id]->encoder_resolution))-mmanager->servo_configs[servo_id]->center_angle);
}

void mmanager_compute_targets(TMotionManager *mmanager)
{
  short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]={0};
  unsigned char i;
  short int angle;

  // compute the balance offsets
  if(mmanager->balance_enabled==0x01)
    mmanager->balance(offsets);
  // convert the angles to digital values
  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
  {
    if(mmanager->servo_values[i].enabled)// servo is enabled and present
    {
      angle=(mmanager->servo_values[i].target_angle>>9)+offsets[i]+(mmanager->servo_values[i].offset<<3);
      //>>16 from the action codification, <<7 from the manager codification
      mmanager->servo_values[i].target_value=mmanager_angle_to_value(mmanager,i,angle);
      if(mmanager->servo_configs[i]->protocol_ver==1 || (mmanager->servo_configs[i]->model!=29 && mmanager->servo_configs[i]->model!=310 && mmanager->servo_configs[i]->model!=320 && mmanager->servo_configs[i]->model!=360))// get the target value as the feedback
      {
        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2]=angle&0x00FF;
        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2+1]=(angle>>8)&0x00FF;
      }
    }
  }
}

void mmanager_compute_angles(TMotionManager *mmanager)
{
  unsigned char i;
  short int angle;

  // convert the digital values to angles
  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
  {
    if(mmanager->servo_values[i].enabled)// servo is enabled and present
    {
      angle=mmanager_value_to_angle(mmanager,i,mmanager->servo_values[i].current_value);
      mmanager->servo_values[i].current_angle=angle<<9;
      if(mmanager->servo_configs[i]->protocol_ver==2 || mmanager->servo_configs[i]->model==29 || mmanager->servo_configs[i]->model==310 || mmanager->servo_configs[i]->model==320 || mmanager->servo_configs[i]->model==360)// get the actual feedback
      {
        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2]=angle&0x00FF;
        mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2+1]=(angle>>8)&0x00FF;
      }
    }
  }
}

void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned short int model)
{
  unsigned int cw_value,ccw_value;
  unsigned char i;

  if(id<DYN_MANAGER_MAX_NUM_DEVICES)
  {
    for(i=0;i<NUM_SERVO_MODELS;i++)
      if(model==servo_data[i].model)
        mmanager->servo_configs[id]=&servo_data[i];

    mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
    mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
    mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&ccw_value);
    mmanager->servo_values[id].target_value=mmanager->servo_values[id].current_value;
    mmanager->servo_values[id].current_angle=(mmanager_value_to_angle(mmanager,id,mmanager->servo_values[id].current_value)<<9);
    mmanager->servo_values[id].target_angle=mmanager->servo_values[id].current_angle;
    // read the servo limits
    mmanager->servo_configs[id]->cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_value)<<9);
    mmanager->servo_configs[id]->ccw_angle_limit=(mmanager_value_to_angle(mmanager,id,ccw_value)<<9);
    mmanager->present_servos|=(0x00000001<<id);
    mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET]=mmanager->present_servos&0x000000FF;
    mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+1]=(mmanager->present_servos>>8)&0x000000FF;
    mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+2]=(mmanager->present_servos>>16)&0x000000FF;
    mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+3]=(mmanager->present_servos>>24)&0x000000FF;
  }
}

void mmanager_setup(TMotionManager *mmanager)
{
  unsigned char * data[DYN_MANAGER_MAX_NUM_DEVICES];
  unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES];
  unsigned short int address[DYN_MANAGER_MAX_NUM_DEVICES];
  unsigned short int length[DYN_MANAGER_MAX_NUM_DEVICES];
  unsigned char i,num=0,first=1;
  unsigned short int start_address=0;

  // initialize the motion operation
  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
  {
    if(mmanager->dyn_module.assigned_ids[i]==0x01)
    {
      if(mmanager->servo_configs[i]->protocol_ver==1)
      {
        data[num]=&mmanager->servo_values[i].cw_compliance;
        ids[num]=i;
        num++;
        if(first==1)
        {
          first=0;
          if(mmanager->servo_configs[i]->pid==1)
            start_address=mmanager->servo_configs[i]->registers[pos_pid_p].address;
          else
            start_address=mmanager->servo_configs[i]->registers[cw_comp_slope].address;
        }
      }
    }
  }
  if(num>0)
  {
    if(mmanager->motion_op[0]!=0x00000000)
      dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[0]);
    mmanager->motion_op[0]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,4,(unsigned char * const*)&data);
  }
  num=0;
  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
  {
    if(mmanager->dyn_module.assigned_ids[i]==0x01)
    {
      if(mmanager->servo_configs[i]->protocol_ver==2)
      {
        data[num]=(unsigned char *)&mmanager->servo_values[i].target_value;
        ids[num]=i;
        num++;
        if(first==1)
        {
          first=0;
          start_address=mmanager->servo_configs[i]->registers[goal_pos].address;
        }
      }
    }
  }
  if(num>0)
  {
    if(mmanager->motion_op[1]!=0x00000000)
      dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->motion_op[1]);
    mmanager->motion_op[1]=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,start_address,2,(unsigned char * const*)&data);
  }
  /* create a feedback operation with the servos that support bulk read */
  num=0;
  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
  {
    if(mmanager->dyn_module.assigned_ids[i]==0x01)
    {
      if(mmanager->servo_configs[i]->protocol_ver==2 || mmanager->servo_configs[i]->model==29 || mmanager->servo_configs[i]->model==310 || mmanager->servo_configs[i]->model==320 || mmanager->servo_configs[i]->model==360)// MX series with version 1 or version 2
      {
        address[num]=mmanager->servo_configs[i]->registers[current_pos].address;
        data[num]=(unsigned char *)&mmanager->servo_values[i].current_value;
        length[num]=mmanager->servo_configs[i]->registers[current_pos].size;
        ids[num]=i;
        num++;
      }
    }
  }
  if(num>0)
  {
    if(mmanager->feedback_op[0]!=0x00000000)
      dyn_manager_delete_op(mmanager->dyn_module.manager,mmanager->feedback_op[0]);
    mmanager->feedback_op[0]=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data);
  }
}

void mmanager_set_period(TMotionManager *mmanager,unsigned short period_ms)
{
  unsigned char i;

  for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
    if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->set_period!=0x00000000)
      mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_ms);
}

void mmanager_pre_process(TMotionManager *mmanager)
{
  unsigned char i;

  for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
  {
    if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->pre_process!=0x00000000)
      mmanager->modules[i]->pre_process(mmanager->modules[i]->data);
  }
  /* call the balance function */
  mmanager_compute_targets(mmanager);
}

void mmanager_post_process(TMotionManager *mmanager)
{
  unsigned char i;

  /* transform the current values to angles */
  mmanager_compute_angles(mmanager);
  for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
  {
    if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->post_process!=0x00000000)
      mmanager->modules[i]->post_process(mmanager->modules[i]->data);
  }
}

/* public functions */
unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
{
  unsigned char i;

  /* initialize the base module */
  dyn_module_init(&mmanager->dyn_module,memory,ram_base_address,eeprom_base_address);
  /* add all known servo models */
  for(i=0;i<NUM_SERVO_MODELS;i++)
    dyn_module_add_model(&mmanager->dyn_module,servo_data[i].model);
  mmanager->dyn_module.data=mmanager;
  mmanager->dyn_module.add_device=(void (*)(void *,unsigned char,unsigned short int))mmanager_add_device;
  mmanager->dyn_module.setup=(void (*)(void *))mmanager_setup;
  mmanager->dyn_module.set_period=(void (*)(void *,unsigned short int))mmanager_set_period;
  mmanager->dyn_module.pre_process=(void (*)(void *))mmanager_pre_process;
  mmanager->dyn_module.post_process=(void (*)(void *))mmanager_post_process;
  /* initialize internal attributes */
  mmanager->balance_enabled=0x00;
  mmanager->num_modules=0x00;
  /* initialize motion module base attributes */
  for(i=0;i<MM_MAX_NUM_MOTION_MODULES;i++)
    mmanager->modules[i]=0x00000000;
  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
  {
    /* servo configuration */
    mmanager->servo_configs[i]=0x00000000;
    /* servo values */
    mmanager->servo_values[i].cw_compliance=0x00;
    mmanager->servo_values[i].ccw_compliance=0x00;
    mmanager->servo_values[i].target_value=0x0000;
    mmanager->servo_values[i].target_angle=0;
    mmanager->servo_values[i].current_value=0x0000;
    mmanager->servo_values[i].current_angle=0;
    mmanager->servo_values[i].module=MM_NONE;
    mmanager->servo_values[i].enabled=0x00;
    mmanager->servo_values[i].offset=0x00;
  }
  mmanager->enable_op=0x00000000;
  mmanager->motion_op[0]=0x00000000;
  mmanager->motion_op[1]=0x00000000;
  mmanager->feedback_op[0]=0x00000000;
  mmanager->feedback_op[1]=0x00000000;
  mmanager->balance=0x00000000;
  mmanager->present_servos=0x00000000;

  /* initialize memory module */
  mem_module_init(&mmanager->mem_module);
  mmanager->mem_module.data=mmanager;
  mmanager->mem_module.write_cmd=mmanager_write_cmd;
  mmanager->mem_module.read_cmd=mmanager_read_cmd;
  if(!mem_module_add_ram_segment(&mmanager->mem_module,ram_base_address+RAM_DYN_MODULE_LENGTH,RAM_MM_LENGTH))
    return 0x00;
  mmanager->ram_base_address=ram_base_address+RAM_DYN_MODULE_LENGTH;
  if(!mem_module_add_eeprom_segment(&mmanager->mem_module,eeprom_base_address+EEPROM_DYN_MODULE_LENGTH,EEPROM_MM_LENGTH))
    return 0x00;
  mmanager->eeprom_base_address=eeprom_base_address+EEPROM_DYN_MODULE_LENGTH;
  if(!mem_add_module(memory,&mmanager->mem_module))
    return 0x00;
  mmanager->memory=memory;
  for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES/2;i++)
    mmanager->memory->data[ram_base_address+MM_ENABLE_MODULE_OFFSET+i]=0x77;

  return 0x01;
}

void mmanager_set_module(TMotionManager *mmanager,unsigned char servo_id,TModules module)
{
  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
  {
    if(module!=mmanager->servo_values[servo_id].module)
    {
      if(module!=MM_NONE && mmanager->modules[module]->set_module!=0x00000000)
        mmanager->modules[module]->set_module(mmanager->modules[module]->data,servo_id);
      mmanager->servo_values[servo_id].module=module;
    }
  }
}

TModules mmanager_get_module(TMotionManager *mmanager,unsigned char servo_id)
{
  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
    return mmanager->servo_values[servo_id].module;
  else
    return MM_NONE;
}

void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id)
{
  unsigned char * const data=&(mmanager->servo_values[servo_id].enabled);

  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
  {
    if(mmanager->servo_values[servo_id].enabled==0x00)
    {
      mmanager->servo_values[servo_id].enabled=0x01;
      /* add an operation to enable the servo */
      if(mmanager->servo_configs[servo_id]!=0x00000000)
      {
        if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
        {
          mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
          dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
        }
        else
          dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
      }
    }
  }
}

void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id)
{
  unsigned char * const data=&(mmanager->servo_values[servo_id].enabled);

  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
  {
    if(mmanager->servo_values[servo_id].enabled==0x01)
    {
      mmanager->servo_values[servo_id].enabled=0x00;
      /* add an operation to enable the servo */
      if(mmanager->servo_configs[servo_id]!=0x00000000)
      {
        if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op)
        {
          mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,mmanager->servo_configs[servo_id]->registers[torque_en].address,1,&data);
          dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1);
        }
        else
          dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data);
      } 
    }
  }
}

unsigned char mmanager_is_servo_enabled(TMotionManager *mmanager,unsigned char servo_id)
{
  if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES)
    return mmanager->servo_values[servo_id].enabled;
  else
    return 0x00;
}

void mmanager_add_module(TMotionManager *mmanager,TMotionModule *module)
{
  if(module->id!=MM_NONE)
  {
    if(mmanager->modules[module->id]==0x00000000)
      mmanager->num_modules++;
    mmanager->modules[module->id]=module;
    module->manager=mmanager;
  }
}