Skip to content
Snippets Groups Projects
Commit 0a05031c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added functions to set the deisred control mode in both the single motor and groups.

parent 41713e18
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/dynamixel_motor.cpp
+ 79
− 25
View file @ 0a05031c
...@@ -53,11 +53,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b ...@@ -53,11 +53,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
this->dynamixel_dev=this->dyn_server->get_device(dev_id,version); this->dynamixel_dev=this->dyn_server->get_device(dev_id,version);
this->info.id=dev_id; this->info.id=dev_id;
this->get_model(); this->get_model();
this->get_position_range(&this->config.min_angle,&this->config.max_angle); this->get_control_mode();
if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
this->mode=torque_ctrl;
else
this->mode=angle_ctrl;
this->config.max_temperature=this->get_temperature_limit(); this->config.max_temperature=this->get_temperature_limit();
this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage); this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
this->config.max_torque=this->get_max_torque(); this->config.max_torque=this->get_max_torque();
...@@ -110,11 +106,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int ba ...@@ -110,11 +106,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int ba
this->dynamixel_dev=this->dyn_server->get_device(dev_id,version); this->dynamixel_dev=this->dyn_server->get_device(dev_id,version);
this->info.id=dev_id; this->info.id=dev_id;
this->get_model(); this->get_model();
this->get_position_range(&this->config.min_angle,&this->config.max_angle); this->get_control_mode();
if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
this->mode=torque_ctrl;
else
this->mode=angle_ctrl;
this->config.max_temperature=this->get_temperature_limit(); this->config.max_temperature=this->get_temperature_limit();
this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage); this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
this->config.max_torque=this->get_max_torque(); this->config.max_torque=this->get_max_torque();
...@@ -327,6 +319,47 @@ void CDynamixelMotor::get_model(void) ...@@ -327,6 +319,47 @@ void CDynamixelMotor::get_model(void)
} }
} }
void CDynamixelMotor::set_control_mode(control_mode mode)
{
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
try{
if(this->info.model=="XL_320")
{
this->dynamixel_dev->write_byte_register(this->registers[dyn_control_mode],(unsigned char)mode);
usleep(10000);
this->mode=mode;
}
else
{
if(this->mode!=mode)
{
if(mode==angle_ctrl)
{
this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
this->mode=angle_ctrl;
}
else
{
this->set_position_range(-this->info.center_angle,-this->info.center_angle);
this->mode=torque_ctrl;
}
}
}
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
this->reset_motor();
throw;
}
}
}
void CDynamixelMotor::load_config(TDynamixel_config &config) void CDynamixelMotor::load_config(TDynamixel_config &config)
{ {
this->set_position_range(config.min_angle,config.max_angle); this->set_position_range(config.min_angle,config.max_angle);
...@@ -1124,11 +1157,7 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed) ...@@ -1124,11 +1157,7 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
else else
{ {
try{ try{
if(this->mode==torque_ctrl) this->set_control_mode(angle_ctrl);
{
this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
this->mode=angle_ctrl;
}
cmd[0]=this->from_angles(angle); cmd[0]=this->from_angles(angle);
cmd[1]=this->from_speeds(speed); cmd[1]=this->from_speeds(speed);
this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4); this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4);
...@@ -1153,11 +1182,7 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed) ...@@ -1153,11 +1182,7 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed)
else else
{ {
try{ try{
if(this->mode==torque_ctrl) this->set_control_mode(angle_ctrl);
{
this->set_position_range(-this->info.center_angle,this->info.max_angle-this->info.center_angle);
this->mode=angle_ctrl;
}
this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos); this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos);
cmd[0]=this->from_angles(angle+this->to_angles(pos)); cmd[0]=this->from_angles(angle+this->to_angles(pos));
cmd[1]=this->from_speeds(speed); cmd[1]=this->from_speeds(speed);
...@@ -1183,11 +1208,7 @@ void CDynamixelMotor::move_torque(double torque_ratio) ...@@ -1183,11 +1208,7 @@ void CDynamixelMotor::move_torque(double torque_ratio)
else else
{ {
try{ try{
if(this->mode==angle_ctrl) this->set_control_mode(torque_ctrl);
{
this->set_position_range(-this->info.center_angle,-this->info.center_angle);
this->mode=torque_ctrl;
}
if(torque_ratio<0.0) if(torque_ratio<0.0)
torque+=0x0400; torque+=0x0400;
torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF; torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
...@@ -1410,6 +1431,39 @@ void CDynamixelMotor::set_punch(unsigned short int punch_value) ...@@ -1410,6 +1431,39 @@ void CDynamixelMotor::set_punch(unsigned short int punch_value)
} }
} }
control_mode CDynamixelMotor::get_control_mode(void)
{
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
try{
if(this->info.model=="XL-320")
{
this->dynamixel_dev->read_byte_register(this->registers[dyn_control_mode],(unsigned char *)&this->mode);
}
else
{
this->get_position_range(&this->config.min_angle,&this->config.max_angle);
if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
this->mode=torque_ctrl;
else
this->mode=angle_ctrl;
}
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
this->reset_motor();
throw;
}
}
return this->mode;
}
CDynamixelMotor::~CDynamixelMotor() CDynamixelMotor::~CDynamixelMotor()
{ {
/* stop the motor */ /* stop the motor */
......
src/dynamixel_motor.h
+ 11
− 2
View file @ 0a05031c
...@@ -64,7 +64,7 @@ typedef struct ...@@ -64,7 +64,7 @@ typedef struct
unsigned short int punch; unsigned short int punch;
}TDynamixel_config; }TDynamixel_config;
typedef enum{angle_ctrl=0,torque_ctrl=1} control_mode; typedef enum{angle_ctrl=2,torque_ctrl=1} control_mode;
/** /**
* \brief * \brief
...@@ -145,7 +145,11 @@ class CDynamixelMotor ...@@ -145,7 +145,11 @@ class CDynamixelMotor
* *
*/ */
void get_model(void); void get_model(void);
/**
* \brief
*
*/
void set_control_mode(control_mode mode);
public: public:
/** /**
* \brief * \brief
...@@ -361,6 +365,11 @@ class CDynamixelMotor ...@@ -361,6 +365,11 @@ class CDynamixelMotor
* *
*/ */
void set_punch(unsigned short int punch_value); void set_punch(unsigned short int punch_value);
/**
* \brief
*
*/
control_mode get_control_mode(void);
/** /**
* \brief * \brief
* *
......
src/dynamixel_motor_group.cpp
+ 49
− 43
View file @ 0a05031c
...@@ -54,7 +54,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id ...@@ -54,7 +54,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
this->group_id=group_id; this->group_id=group_id;
this->dyn_server=CDynamixelServer::instance(); this->dyn_server=CDynamixelServer::instance();
this->clear(); this->clear();
this->mode=angle_ctrl; this->mode=torque_ctrl;
/* initialize motion sequence attributes */ /* initialize motion sequence attributes */
this->sequence_state=mtn_empty; this->sequence_state=mtn_empty;
this->sequence_current_step=-1; this->sequence_current_step=-1;
...@@ -68,18 +68,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id ...@@ -68,18 +68,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
this->dyn_server->config_bus(bus_id,baudrate); this->dyn_server->config_bus(bus_id,baudrate);
for(i=0;i<ids.size();i++) for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version); this->init_motor(ids[i],version);
if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle) this->set_control_mode(angle_ctrl);
{
this->mode=torque_ctrl;
for(i=1;i<ids.size();i++)
this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
}
else
{
this->mode=angle_ctrl;
for(i=1;i<ids.size();i++)
this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
}
this->init_events(); this->init_events();
this->init_threads(); this->init_threads();
}catch(CException &e){ }catch(CException &e){
...@@ -104,6 +93,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu ...@@ -104,6 +93,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu
this->group_id=group_id; this->group_id=group_id;
this->dyn_server=CDynamixelServer::instance(); this->dyn_server=CDynamixelServer::instance();
this->clear(); this->clear();
this->mode=torque_ctrl;
if(ids.size()==0) if(ids.size()==0)
{ {
/* handle exceptions */ /* handle exceptions */
...@@ -113,18 +103,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu ...@@ -113,18 +103,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu
this->dyn_server->config_bus(bus_id,baudrate); this->dyn_server->config_bus(bus_id,baudrate);
for(i=0;i<ids.size();i++) for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version); this->init_motor(ids[i],version);
if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle) this->set_control_mode(angle_ctrl);
{
this->mode=torque_ctrl;
for(i=1;i<ids.size();i++)
this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
}
else
{
this->mode=angle_ctrl;
for(i=1;i<ids.size();i++)
this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
}
this->init_events(); this->init_events();
this->init_threads(); this->init_threads();
}catch(CException &e){ }catch(CException &e){
...@@ -855,6 +834,43 @@ void CDynamixelMotorGroup::clear(void) ...@@ -855,6 +834,43 @@ void CDynamixelMotorGroup::clear(void)
this->seq.clear(); this->seq.clear();
} }
void CDynamixelMotorGroup::set_control_mode(control_mode mode)
{
unsigned int i=0;
if(this->mode!=mode)
{
for(i=0;i<this->servo_id.size();i++)
{
try{
if(this->dynamixel_dev[i]==NULL)
{
/* handle exceptions */
throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
if(this->info[i].model=="XL_320")
this->dynamixel_dev[i]->write_byte_register(this->registers[i][dyn_control_mode],(unsigned char)mode);
else
{
if(mode==torque_ctrl)
this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
else
this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
}
}
this->mode=mode;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms[i])
this->reset_motor(i);
throw;
}
}
}
}
int CDynamixelMotorGroup::load_motion(unsigned int step_id) int CDynamixelMotorGroup::load_motion(unsigned int step_id)
{ {
static std::vector<int> time(this->get_num_motors()); static std::vector<int> time(this->get_num_motors());
...@@ -1340,12 +1356,7 @@ void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std:: ...@@ -1340,12 +1356,7 @@ void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std::
unsigned int i=0; unsigned int i=0;
try{ try{
if(this->mode==torque_ctrl) this->set_control_mode(angle_ctrl);
{
for(i=0;i<this->servo_id.size();i++)
this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
this->mode=angle_ctrl;
}
data.resize(this->servo_id.size()); data.resize(this->servo_id.size());
for(i=0;i<this->servo_id.size();i++) for(i=0;i<this->servo_id.size();i++)
{ {
...@@ -1373,12 +1384,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std:: ...@@ -1373,12 +1384,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::
unsigned int i=0; unsigned int i=0;
try{ try{
if(this->mode==torque_ctrl) this->set_control_mode(angle_ctrl);
{
for(i=0;i<this->servo_id.size();i++)
this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
this->mode=angle_ctrl;
}
data.resize(this->servo_id.size()); data.resize(this->servo_id.size());
for(i=0;i<this->servo_id.size();i++) for(i=0;i<this->servo_id.size();i++)
{ {
...@@ -1415,12 +1421,7 @@ void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios) ...@@ -1415,12 +1421,7 @@ void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios)
unsigned int i=0; unsigned int i=0;
try{ try{
if(this->mode==angle_ctrl) this->set_control_mode(torque_ctrl);
{
for(i=0;i<this->servo_id.size();i++)
this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
this->mode=torque_ctrl;
}
data.resize(this->servo_id.size()); data.resize(this->servo_id.size());
for(i=0;i<this->servo_id.size();i++) for(i=0;i<this->servo_id.size();i++)
{ {
...@@ -1689,6 +1690,11 @@ TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void) ...@@ -1689,6 +1690,11 @@ TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void)
return this->sequence_state; return this->sequence_state;
} }
control_mode CDynamixelMotorGroup::get_control_mode(void)
{
return this->mode;
}
void CDynamixelMotorGroup::add_sequence_step(std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion) void CDynamixelMotorGroup::add_sequence_step(std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion)
{ {
TDynamixelMotionStep step; TDynamixelMotionStep step;
......
src/dynamixel_motor_group.h
+ 10
− 0
View file @ 0a05031c
...@@ -273,6 +273,11 @@ class CDynamixelMotorGroup ...@@ -273,6 +273,11 @@ class CDynamixelMotorGroup
* *
*/ */
void clear(void); void clear(void);
/**
* \brief
*
*/
void set_control_mode(control_mode mode);
/* motion sequence protected functions */ /* motion sequence protected functions */
/** /**
* \brief * \brief
...@@ -403,6 +408,11 @@ class CDynamixelMotorGroup ...@@ -403,6 +408,11 @@ class CDynamixelMotorGroup
* *
*/ */
void set_punch(std::vector<unsigned short int> &punches); void set_punch(std::vector<unsigned short int> &punches);
/**
* \brief
*
*/
control_mode get_control_mode(void);
/* motion sequence public functions */ /* motion sequence public functions */
/** /**
* \brief * \brief
......
src/dynamixel_registers.cpp
+ 4
− 7
View file @ 0a05031c
#include "dynamixel_registers.h" #include "dynamixel_registers.h"
unsigned short int std_compl_reg[40]={std_model_number, unsigned short int std_compl_reg[39]={std_model_number,
std_firmware_version, std_firmware_version,
std_id, std_id,
std_baudrate, std_baudrate,
...@@ -29,7 +29,6 @@ unsigned short int std_compl_reg[40]={std_model_number, ...@@ -29,7 +29,6 @@ unsigned short int std_compl_reg[40]={std_model_number,
std_goal_pos, std_goal_pos,
std_goal_speed, std_goal_speed,
std_torque_limit, std_torque_limit,
(unsigned short int)-1,
std_current_pos, std_current_pos,
std_current_speed, std_current_speed,
std_current_load, std_current_load,
...@@ -41,7 +40,7 @@ unsigned short int std_compl_reg[40]={std_model_number, ...@@ -41,7 +40,7 @@ unsigned short int std_compl_reg[40]={std_model_number,
(unsigned short int)-1, (unsigned short int)-1,
std_punch}; std_punch};
unsigned short int std_pid_reg[40]={std_model_number, unsigned short int std_pid_reg[39]={std_model_number,
std_firmware_version, std_firmware_version,
std_id, std_id,
std_baudrate, std_baudrate,
...@@ -70,7 +69,6 @@ unsigned short int std_pid_reg[40]={std_model_number, ...@@ -70,7 +69,6 @@ unsigned short int std_pid_reg[40]={std_model_number,
std_goal_pos, std_goal_pos,
std_goal_speed, std_goal_speed,
std_torque_limit, std_torque_limit,
(unsigned short int)-1,
std_current_pos, std_current_pos,
std_current_speed, std_current_speed,
std_current_load, std_current_load,
...@@ -82,7 +80,7 @@ unsigned short int std_pid_reg[40]={std_model_number, ...@@ -82,7 +80,7 @@ unsigned short int std_pid_reg[40]={std_model_number,
(unsigned short int)-1, (unsigned short int)-1,
std_punch}; std_punch};
unsigned short int xl_reg[40]={xl_model_number, unsigned short int xl_reg[39]={xl_model_number,
xl_firmware_version, xl_firmware_version,
xl_id, xl_id,
xl_baudrate, xl_baudrate,
...@@ -95,7 +93,7 @@ unsigned short int xl_reg[40]={xl_model_number, ...@@ -95,7 +93,7 @@ unsigned short int xl_reg[40]={xl_model_number,
xl_high_voltage_limit, xl_high_voltage_limit,
xl_max_torque, xl_max_torque,
xl_return_level, xl_return_level,
(unsigned short int)-1, xl_alarm_shtdwn,
xl_alarm_shtdwn, xl_alarm_shtdwn,
xl_down_cal, xl_down_cal,
xl_up_cal, xl_up_cal,
...@@ -110,7 +108,6 @@ unsigned short int xl_reg[40]={xl_model_number, ...@@ -110,7 +108,6 @@ unsigned short int xl_reg[40]={xl_model_number,
(unsigned short int)-1, (unsigned short int)-1,
xl_goal_pos, xl_goal_pos,
xl_goal_speed, xl_goal_speed,
(unsigned short int)-1,
xl_goal_torque, xl_goal_torque,
xl_current_pos, xl_current_pos,
xl_current_speed, xl_current_speed,
......
src/dynamixel_registers.h
+ 3
− 4
View file @ 0a05031c
#ifndef _DYNAMIXEL_REGISTERS_H #ifndef _DYNAMIXEL_REGISTERS_H
#define _DYNAMIXEL_REGISTERS_H #define _DYNAMIXEL_REGISTERS_H
extern unsigned short int std_compl_reg[40]; extern unsigned short int std_compl_reg[39];
extern unsigned short int std_pid_reg[40]; extern unsigned short int std_pid_reg[39];
extern unsigned short int xl_reg[40]; extern unsigned short int xl_reg[39];
typedef enum { typedef enum {
// [Access] [Description] // [Access] [Description]
...@@ -38,7 +38,6 @@ typedef enum { ...@@ -38,7 +38,6 @@ typedef enum {
goal_pos, // (RW) Goal Position goal_pos, // (RW) Goal Position
goal_speed, // (RW) Moving Speed goal_speed, // (RW) Moving Speed
torque_limit, // (RW) Torque Limit torque_limit, // (RW) Torque Limit
goal_torque, // (RW) Torque Limit
current_pos, // (R) Current Position current_pos, // (R) Current Position
current_speed, // (R) Current Speed current_speed, // (R) Current Speed
current_load, // (R) Current Load current_load, // (R) Current Load
......
src/examples/motor_testbench.cpp
+ 4
− 2
View file @ 0a05031c
...@@ -14,7 +14,7 @@ int main(int argc, char *argv[]) ...@@ -14,7 +14,7 @@ int main(int argc, char *argv[])
CDynamixelMotor *cont; CDynamixelMotor *cont;
std::string serial="A400gavq"; std::string serial="A400gavq";
TDynamixel_info info; TDynamixel_info info;
double angle; double angle,old_angle;
double amplitude,freq; double amplitude,freq;
CTime time; CTime time;
TDynamixel_pid pid; TDynamixel_pid pid;
...@@ -93,13 +93,15 @@ int main(int argc, char *argv[]) ...@@ -93,13 +93,15 @@ int main(int argc, char *argv[])
cont->turn_led_on(); cont->turn_led_on();
sleep(1); sleep(1);
cont->turn_led_off();*/ cont->turn_led_off();*/
angle=cont->get_current_angle();
for(i=0;i<freq;i++) for(i=0;i<freq;i++)
{ {
old_angle=angle;
angle=amplitude*sin(i*2*3.14159/freq); angle=amplitude*sin(i*2*3.14159/freq);
cont->move_absolute_angle(angle,0); cont->move_absolute_angle(angle,0);
time.set(); time.set();
std::cout << time.getTimeInMicroseconds(); std::cout << time.getTimeInMicroseconds();
std::cout << "," << angle; std::cout << "," << old_angle;
std::cout << "," << cont->get_current_angle() << std::endl; std::cout << "," << cont->get_current_angle() << std::endl;
usleep(7800); usleep(7800);
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment