diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 015703c47489ac6809c56dffc305ce7d9d5d4058..7d98175d3f91cb0b8d96ecd334811e332440cb0e 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -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->info.id=dev_id;
this->get_model();
- 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;
+ this->get_control_mode();
this->config.max_temperature=this->get_temperature_limit();
this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
this->config.max_torque=this->get_max_torque();
@@ -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->info.id=dev_id;
this->get_model();
- 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;
+ this->get_control_mode();
this->config.max_temperature=this->get_temperature_limit();
this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
this->config.max_torque=this->get_max_torque();
@@ -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)
{
this->set_position_range(config.min_angle,config.max_angle);
@@ -1124,11 +1157,7 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
else
{
try{
- if(this->mode==torque_ctrl)
- {
- this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
- this->mode=angle_ctrl;
- }
+ this->set_control_mode(angle_ctrl);
cmd[0]=this->from_angles(angle);
cmd[1]=this->from_speeds(speed);
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)
else
{
try{
- if(this->mode==torque_ctrl)
- {
- this->set_position_range(-this->info.center_angle,this->info.max_angle-this->info.center_angle);
- this->mode=angle_ctrl;
- }
+ this->set_control_mode(angle_ctrl);
this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos);
cmd[0]=this->from_angles(angle+this->to_angles(pos));
cmd[1]=this->from_speeds(speed);
@@ -1183,11 +1208,7 @@ void CDynamixelMotor::move_torque(double torque_ratio)
else
{
try{
- if(this->mode==angle_ctrl)
- {
- this->set_position_range(-this->info.center_angle,-this->info.center_angle);
- this->mode=torque_ctrl;
- }
+ this->set_control_mode(torque_ctrl);
if(torque_ratio<0.0)
torque+=0x0400;
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)
}
}
+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()
{
/* stop the motor */
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index ba52bca56d8a3439c974aa4ece4a048ec52d6006..ca81c0a2bf8e2677c8cbb99d15bd3fc670234841 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -64,7 +64,7 @@ typedef struct
unsigned short int punch;
}TDynamixel_config;
-typedef enum{angle_ctrl=0,torque_ctrl=1} control_mode;
+typedef enum{angle_ctrl=2,torque_ctrl=1} control_mode;
/**
* \brief
@@ -145,7 +145,11 @@ class CDynamixelMotor
*
*/
void get_model(void);
-
+ /**
+ * \brief
+ *
+ */
+ void set_control_mode(control_mode mode);
public:
/**
* \brief
@@ -361,6 +365,11 @@ class CDynamixelMotor
*
*/
void set_punch(unsigned short int punch_value);
+ /**
+ * \brief
+ *
+ */
+ control_mode get_control_mode(void);
/**
* \brief
*
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index c0904f58b228c0d80a75c2b2dba5d0c593909d99..38b96d6d9c547bbe8636b576bbeb44fe51ceedfb 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -54,7 +54,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
this->group_id=group_id;
this->dyn_server=CDynamixelServer::instance();
this->clear();
- this->mode=angle_ctrl;
+ this->mode=torque_ctrl;
/* initialize motion sequence attributes */
this->sequence_state=mtn_empty;
this->sequence_current_step=-1;
@@ -68,18 +68,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
this->dyn_server->config_bus(bus_id,baudrate);
for(i=0;i<ids.size();i++)
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->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->set_control_mode(angle_ctrl);
this->init_events();
this->init_threads();
}catch(CException &e){
@@ -104,6 +93,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu
this->group_id=group_id;
this->dyn_server=CDynamixelServer::instance();
this->clear();
+ this->mode=torque_ctrl;
if(ids.size()==0)
{
/* handle exceptions */
@@ -113,18 +103,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu
this->dyn_server->config_bus(bus_id,baudrate);
for(i=0;i<ids.size();i++)
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->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->set_control_mode(angle_ctrl);
this->init_events();
this->init_threads();
}catch(CException &e){
@@ -855,6 +834,43 @@ void CDynamixelMotorGroup::clear(void)
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)
{
static std::vector<int> time(this->get_num_motors());
@@ -1340,12 +1356,7 @@ void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std::
unsigned int i=0;
try{
- if(this->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].max_angle-this->info[i].center_angle);
- this->mode=angle_ctrl;
- }
+ this->set_control_mode(angle_ctrl);
data.resize(this->servo_id.size());
for(i=0;i<this->servo_id.size();i++)
{
@@ -1373,12 +1384,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::
unsigned int i=0;
try{
- if(this->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].max_angle-this->info[i].center_angle);
- this->mode=angle_ctrl;
- }
+ this->set_control_mode(angle_ctrl);
data.resize(this->servo_id.size());
for(i=0;i<this->servo_id.size();i++)
{
@@ -1415,12 +1421,7 @@ void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios)
unsigned int i=0;
try{
- if(this->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].center_angle);
- this->mode=torque_ctrl;
- }
+ this->set_control_mode(torque_ctrl);
data.resize(this->servo_id.size());
for(i=0;i<this->servo_id.size();i++)
{
@@ -1689,6 +1690,11 @@ TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void)
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)
{
TDynamixelMotionStep step;
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index 0d9e5ad97b18dd9eb36472118eaa6f03cf23574b..7c127013e80f5a6f5357d0aac40830a0ba162d97 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -273,6 +273,11 @@ class CDynamixelMotorGroup
*
*/
void clear(void);
+ /**
+ * \brief
+ *
+ */
+ void set_control_mode(control_mode mode);
/* motion sequence protected functions */
/**
* \brief
@@ -403,6 +408,11 @@ class CDynamixelMotorGroup
*
*/
void set_punch(std::vector<unsigned short int> &punches);
+ /**
+ * \brief
+ *
+ */
+ control_mode get_control_mode(void);
/* motion sequence public functions */
/**
* \brief
diff --git a/src/dynamixel_registers.cpp b/src/dynamixel_registers.cpp
index 58904ec06956226413f8d9611a6dd4df606f1570..3aaf496294c24543e567a2bf2b978492cc1bdad1 100644
--- a/src/dynamixel_registers.cpp
+++ b/src/dynamixel_registers.cpp
@@ -1,6 +1,6 @@
#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_id,
std_baudrate,
@@ -29,7 +29,6 @@ unsigned short int std_compl_reg[40]={std_model_number,
std_goal_pos,
std_goal_speed,
std_torque_limit,
- (unsigned short int)-1,
std_current_pos,
std_current_speed,
std_current_load,
@@ -41,7 +40,7 @@ unsigned short int std_compl_reg[40]={std_model_number,
(unsigned short int)-1,
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_id,
std_baudrate,
@@ -70,7 +69,6 @@ unsigned short int std_pid_reg[40]={std_model_number,
std_goal_pos,
std_goal_speed,
std_torque_limit,
- (unsigned short int)-1,
std_current_pos,
std_current_speed,
std_current_load,
@@ -82,7 +80,7 @@ unsigned short int std_pid_reg[40]={std_model_number,
(unsigned short int)-1,
std_punch};
-unsigned short int xl_reg[40]={xl_model_number,
+unsigned short int xl_reg[39]={xl_model_number,
xl_firmware_version,
xl_id,
xl_baudrate,
@@ -95,7 +93,7 @@ unsigned short int xl_reg[40]={xl_model_number,
xl_high_voltage_limit,
xl_max_torque,
xl_return_level,
- (unsigned short int)-1,
+ xl_alarm_shtdwn,
xl_alarm_shtdwn,
xl_down_cal,
xl_up_cal,
@@ -110,7 +108,6 @@ unsigned short int xl_reg[40]={xl_model_number,
(unsigned short int)-1,
xl_goal_pos,
xl_goal_speed,
- (unsigned short int)-1,
xl_goal_torque,
xl_current_pos,
xl_current_speed,
diff --git a/src/dynamixel_registers.h b/src/dynamixel_registers.h
index 2bfbe13d4dfd6b2fcbab947ace5e4b8ecb88e0aa..481a2eba9ec886e7ee0de37385830caa62627f5e 100644
--- a/src/dynamixel_registers.h
+++ b/src/dynamixel_registers.h
@@ -1,9 +1,9 @@
#ifndef _DYNAMIXEL_REGISTERS_H
#define _DYNAMIXEL_REGISTERS_H
-extern unsigned short int std_compl_reg[40];
-extern unsigned short int std_pid_reg[40];
-extern unsigned short int xl_reg[40];
+extern unsigned short int std_compl_reg[39];
+extern unsigned short int std_pid_reg[39];
+extern unsigned short int xl_reg[39];
typedef enum {
// [Access] [Description]
@@ -38,7 +38,6 @@ typedef enum {
goal_pos, // (RW) Goal Position
goal_speed, // (RW) Moving Speed
torque_limit, // (RW) Torque Limit
- goal_torque, // (RW) Torque Limit
current_pos, // (R) Current Position
current_speed, // (R) Current Speed
current_load, // (R) Current Load
diff --git a/src/examples/motor_testbench.cpp b/src/examples/motor_testbench.cpp
index 7f238c3ab6b4aea2520369eb7e35c3585eecf081..44bacebf21ed170548ca8035420fbbbfda29b6e8 100755
--- a/src/examples/motor_testbench.cpp
+++ b/src/examples/motor_testbench.cpp
@@ -14,7 +14,7 @@ int main(int argc, char *argv[])
CDynamixelMotor *cont;
std::string serial="A400gavq";
TDynamixel_info info;
- double angle;
+ double angle,old_angle;
double amplitude,freq;
CTime time;
TDynamixel_pid pid;
@@ -93,13 +93,15 @@ int main(int argc, char *argv[])
cont->turn_led_on();
sleep(1);
cont->turn_led_off();*/
+ angle=cont->get_current_angle();
for(i=0;i<freq;i++)
{
+ old_angle=angle;
angle=amplitude*sin(i*2*3.14159/freq);
cont->move_absolute_angle(angle,0);
time.set();
std::cout << time.getTimeInMicroseconds();
- std::cout << "," << angle;
+ std::cout << "," << old_angle;
std::cout << "," << cont->get_current_angle() << std::endl;
usleep(7800);
}