diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 2889041da856ba9ba94b7c1137697fc10648e52c..d9d7bd6bcfd66795e58394432a88346a0f43536e 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -27,6 +27,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->info.max_speed=-1;
this->info.baudrate=(unsigned int)-1;
this->info.id=(unsigned char)-1;
+ this->info.ext_memory_map=false;
this->compliance.cw_compliance_margin=0x00;
this->compliance.ccw_compliance_margin=0x00;
this->compliance.cw_compliance_slope=0x20;
@@ -43,7 +44,8 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->config.punch=0x20;
this->registers=NULL;
this->dyn_server=dyn_server;
- this->dynamixel_dev=NULL;
+ this->dynamixel_dev=NULL;
+ this->enabled=false;
try{
this->info.baudrate=this->dyn_server->get_baudrate();
this->dynamixel_dev=this->dyn_server->get_device(dev_id,version);
@@ -52,7 +54,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
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_pwm=this->get_max_pwm();
+ this->config.max_pwm=this->get_pwm_limit();
this->get_compliance_control(this->compliance);
this->get_pid_control(this->pid);
this->alarms=this->get_turn_off_alarms();
@@ -160,6 +162,7 @@ void CDynamixelMotor::get_model(void)
this->registers=ax_reg;
this->info.pid_control=false;
this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x012C: this->info.model="AX-12W";
this->info.max_angle=300.0;
@@ -170,6 +173,7 @@ void CDynamixelMotor::get_model(void)
this->registers=ax_reg;
this->info.pid_control=false;
this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x0012: this->info.model="AX-18A";
this->info.max_angle=300.0;
@@ -180,6 +184,7 @@ void CDynamixelMotor::get_model(void)
this->registers=ax_reg;
this->info.pid_control=false;
this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
break;
case 0x0168: this->info.model="MX-12";
this->info.max_angle=360.0;
@@ -190,6 +195,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
break;
case 0x001D: this->info.model="MX-28";
this->info.max_angle=360.0;
@@ -200,6 +206,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
break;
case 0x0136: this->info.model="MX-64";
this->info.max_angle=360.0;
@@ -210,6 +217,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
break;
case 0x0140: this->info.model="MX-106";
this->info.max_angle=360.0;
@@ -220,6 +228,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_106_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
+ this->info.ext_memory_map=false;
break;
case 0x015E: this->info.model="XL_320";
this->info.max_angle=300.0;
@@ -230,6 +239,18 @@ void CDynamixelMotor::get_model(void)
this->registers=xl_reg;
this->info.pid_control=true;
this->info.multi_turn=false;
+ this->info.ext_memory_map=false;
+ break;
+ case 0x0406: this->info.model="XM-430-W210";
+ this->info.max_angle=360.0;
+ this->info.center_angle=180;
+ this->info.max_speed=4620;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=213;
+ this->registers=xm_reg;
+ this->info.pid_control=true;
+ this->info.multi_turn=true;
+ this->info.ext_memory_map=true;
break;
default: this->info.model="unknown";
break;
@@ -248,29 +269,42 @@ void CDynamixelMotor::get_model(void)
void CDynamixelMotor::set_control_mode(control_mode mode)
{
unsigned int value;
+ bool was_enabled=this->enabled;
if(this->mode!=mode)
{
- this->disable();
- if(this->info.model=="XL_320")
+ if(this->enabled)
+ this->disable();
+ if(this->info.ext_memory_map)
{
value=mode;
this->write_register(this->registers[op_mode],value);
- this->mode=mode;
}
else
{
- if(mode==angle_ctrl)
+ if(this->info.model=="XL_320")
{
- this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
- this->mode=angle_ctrl;
+ if(mode==pwm_ctrl)
+ value=1;
+ else if(mode==position_ctrl)
+ value=2;
+ else
+ throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"Unsupported control mode");
+ this->write_register(this->registers[op_mode],value);
}
else
{
- this->set_position_range(-this->info.center_angle,-this->info.center_angle);
- this->mode=torque_ctrl;
+ if(mode==position_ctrl)
+ this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
+ else if(mode==pwm_ctrl)
+ this->set_position_range(-this->info.center_angle,-this->info.center_angle);
+ else
+ throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"Unsupported control mode");
}
+ this->mode=mode;
}
+ if(was_enabled)
+ this->enable();
}
}
@@ -297,9 +331,9 @@ void CDynamixelMotor::read_register(TDynReg reg, unsigned int &value)
if(reg.size==1)
value=reg_value[0];
else if(reg.size==2)
- value=*((unsigned short int *)reg_value);
+ value=reg_value[0]+(reg_value[1]<<8);
else if(reg.size==4)
- value=*((unsigned int *)reg_value);
+ value=reg_value[0]+(reg_value[1]<<8)+(reg_value[2]<<16)+(reg_value[3]<<24);
else
{
/* handle exceptions */
@@ -494,11 +528,13 @@ void CDynamixelMotor::get_servo_info(TDynamixel_info &info)
info.gear_ratio=this->info.gear_ratio;
info.encoder_resolution=this->info.encoder_resolution;
info.pid_control=this->info.pid_control;
+ info.multi_turn=this->info.multi_turn;
info.max_angle=this->info.max_angle;
info.center_angle=this->info.center_angle;
info.max_speed=this->info.max_speed;
info.baudrate=this->info.baudrate;
info.id=this->info.id;
+ info.ext_memory_map=this->info.ext_memory_map;
}
void CDynamixelMotor::set_position_range(double min, double max)
@@ -519,9 +555,9 @@ void CDynamixelMotor::set_position_range(double min, double max)
this->write_register(this->registers[max_angle_limit],max_pos);
this->write_register(this->registers[min_angle_limit],min_pos);
if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
- this->mode=torque_ctrl;
+ this->mode=pwm_ctrl;
else
- this->mode=angle_ctrl;
+ this->mode=position_ctrl;
}
}
@@ -656,7 +692,7 @@ double CDynamixelMotor::get_max_pwm(void)
double torque;
this->read_register(this->registers[max_pwm],load);
- torque=(load&0x3FF)*100.0/1023;
+ torque=(load&0x3FF)*100.0/1023.0;
if(load>0x3FF)
torque=-1*torque;
@@ -669,9 +705,14 @@ double CDynamixelMotor::get_pwm_limit(void)
double torque;
this->read_register(this->registers[pwm_limit],load);
- torque=(load&0x3FF)*100.0/1023;
- if(load>0x3FF)
- torque=-1*torque;
+ if(this->info.ext_memory_map)
+ torque=((signed short int)load)*100.0/885.0;
+ else
+ {
+ torque=(load&0x3FF)*100.0/1023.0;
+ if(load>0x3FF)
+ torque=-1*torque;
+ }
return torque;
}
@@ -686,7 +727,7 @@ void CDynamixelMotor::set_max_pwm(double torque_ratio)
}
else
{
- load=torque_ratio*1023/100.0;
+ load=torque_ratio*1023.0/100.0;
this->config.max_pwm=torque_ratio;
this->write_register(this->registers[max_pwm],load);
}
@@ -702,11 +743,35 @@ void CDynamixelMotor::set_pwm_limit(double torque_ratio)
}
else
{
- load=torque_ratio*1023/100.0;
+ if(this->info.ext_memory_map)
+ load=torque_ratio*885.0/100.0;
+ else
+ load=torque_ratio*1023.0/100.0;
this->write_register(this->registers[pwm_limit],load);
}
}
+double CDynamixelMotor::get_current_limit(void)
+{
+
+}
+
+void CDynamixelMotor::set_current_limit(double current)
+{
+
+}
+
+double CDynamixelMotor::get_speed_limit(void)
+{
+
+}
+
+void CDynamixelMotor::set_speed_limit(double speed)
+{
+
+}
+
+
void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
{
unsigned int value;
@@ -812,6 +877,26 @@ void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
}
}
+void CDynamixelMotor::get_vel_pi_control(TDynamixel_pid &config)
+{
+
+}
+
+void CDynamixelMotor::set_vel_pi_control(TDynamixel_pid &config)
+{
+
+}
+
+void CDynamixelMotor::get_feedfwd_control(double &acc_gain,double &vel_gain)
+{
+
+}
+
+void CDynamixelMotor::set_feedfwd_control(double acc_gain,double vel_gain)
+{
+
+}
+
void CDynamixelMotor::turn_led_on(void)
{
this->write_register(this->registers[led],1);
@@ -835,18 +920,20 @@ void CDynamixelMotor::lock(void)
void CDynamixelMotor::enable(void)
{
this->write_register(this->registers[torque_en],1);
+ this->enabled=true;
}
void CDynamixelMotor::disable(void)
{
this->write_register(this->registers[torque_en],0);
+ this->enabled=false;
}
-void CDynamixelMotor::move_absolute_angle(double angle,double speed)
+void CDynamixelMotor::move_absolute_angle(double angle,double speed,double current)
{
unsigned int cmd[2];
- this->set_control_mode(angle_ctrl);
+ this->set_control_mode(position_ctrl);
if(angle>this->info.center_angle)
angle=this->info.center_angle;
else if(angle<-this->info.center_angle)
@@ -859,12 +946,12 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
this->write_register(this->registers[goal_speed],cmd[1]);
}
-void CDynamixelMotor::move_relative_angle(double angle,double speed)
+void CDynamixelMotor::move_relative_angle(double angle,double speed,double current)
{
unsigned int cmd[2],pos;
double abs_angle;
- this->set_control_mode(angle_ctrl);
+ this->set_control_mode(position_ctrl);
this->read_register(this->registers[current_pos],pos);
abs_angle=angle+this->to_angles(pos);
if(abs_angle>this->info.center_angle)
@@ -879,26 +966,44 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed)
this->write_register(this->registers[goal_speed],cmd[1]);
}
-void CDynamixelMotor::move_torque(double torque_ratio)
+void CDynamixelMotor::move_speed(double speed)
+{
+
+}
+
+void CDynamixelMotor::move_pwm(double pwm_ratio)
{
unsigned int torque=0;
- this->set_control_mode(torque_ctrl);
- if(torque_ratio>100.0)
- torque_ratio=100.0;
- else if(torque_ratio<-100.0)
- torque_ratio=-100.0;
- if(torque_ratio<0.0)
- torque+=0x0400;
- torque+=((unsigned int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
- this->write_register(this->registers[goal_speed],torque);
+ this->set_control_mode(pwm_ctrl);
+ if(pwm_ratio>100.0)
+ pwm_ratio=100.0;
+ else if(pwm_ratio<-100.0)
+ pwm_ratio=-100.0;
+ if(this->info.ext_memory_map)
+ {
+ torque=((unsigned short int)(pwm_ratio*885.0/100.0));
+ this->write_register(this->registers[goal_pwm],torque);
+ }
+ else
+ {
+ if(pwm_ratio<0.0)
+ torque+=0x0400;
+ torque+=((unsigned int)(fabs(pwm_ratio)*1023.0/100.0))&0x03FF;
+ this->write_register(this->registers[goal_speed],torque);
+ }
+}
+
+void CDynamixelMotor::move_current(double current)
+{
+
}
void CDynamixelMotor::stop(void)
{
unsigned int current_position;
- if(this->mode==angle_ctrl)
+ if(this->mode==position_ctrl)
{
this->read_register(this->registers[current_pos],current_position);
if(this->to_angles(current_position)>this->config.max_angle)
@@ -957,19 +1062,32 @@ double CDynamixelMotor::get_current_voltage(void)
return c_voltage;
}
-double CDynamixelMotor::get_current_effort(void)
+double CDynamixelMotor::get_current_pwm(void)
{
unsigned int data;
double c_effort;
- this->read_register(this->registers[current_load],data);
- c_effort = (double)((data&0x3FF)*100.0/1023);
- if(this->get_current_speed() < 0)
- c_effort *= -1.0;
+ if(this->info.ext_memory_map)
+ {
+ this->read_register(this->registers[current_pwm],data);
+ c_effort=100.0*((signed short int)data)/885.0;
+ }
+ else
+ {
+ this->read_register(this->registers[current_load],data);
+ c_effort = (double)((data&0x3FF)*100.0/1023.0);
+ if(this->get_current_speed() < 0)
+ c_effort *= -1.0;
+ }
return c_effort;
}
+double CDynamixelMotor::get_current_current(void)
+{
+
+}
+
unsigned int CDynamixelMotor::get_punch(void)
{
unsigned int value;
@@ -1000,9 +1118,9 @@ control_mode CDynamixelMotor::get_control_mode(void)
{
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;
+ this->mode=pwm_ctrl;
else
- this->mode=angle_ctrl;
+ this->mode=position_ctrl;
}
return this->mode;
@@ -1035,6 +1153,7 @@ CDynamixelMotor::~CDynamixelMotor()
this->info.max_speed=-1;
this->info.baudrate=(unsigned int)-1;
this->info.id=(unsigned char)-1;
+ this->info.ext_memory_map=false;
this->compliance.cw_compliance_margin=0x00;
this->compliance.ccw_compliance_margin=0x00;
this->compliance.cw_compliance_slope=0x20;
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index 9affd4af3505cd07e2a98ff2a42a359871142d88..95347b040f204165c75b6e2f490618dd7e644b93 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -37,6 +37,7 @@ typedef struct
double max_speed;
unsigned int baudrate;
unsigned char id;
+ bool ext_memory_map;
}TDynamixel_info;
typedef struct
@@ -49,9 +50,9 @@ typedef struct
typedef struct
{
- unsigned char p;
- unsigned char i;
- unsigned char d;
+ unsigned short int p;
+ unsigned short int i;
+ unsigned short int d;
}TDynamixel_pid;
typedef struct
@@ -65,7 +66,7 @@ typedef struct
unsigned short int punch;
}TDynamixel_config;
-typedef enum{angle_ctrl=2,torque_ctrl=1} control_mode;
+typedef enum{current_ctrl=0,velocity_ctrl=1,position_ctrl=3,ext_position_ctrl=4,current_pos_ctrl=5,pwm_ctrl=6} control_mode;
/**
* \brief
@@ -115,6 +116,11 @@ class CDynamixelMotor
*
*/
TDynReg *registers;
+ /**
+ * \brief
+ *
+ */
+ bool enabled;
protected:
/**
* \brief
@@ -265,6 +271,26 @@ class CDynamixelMotor
*
*/
void set_pwm_limit(double torque_ratio);
+ /**
+ * \brief
+ *
+ */
+ double get_current_limit(void);
+ /**
+ * \brief
+ *
+ */
+ void set_current_limit(double current);
+ /**
+ * \brief
+ *
+ */
+ double get_speed_limit(void);
+ /**
+ * \brief
+ *
+ */
+ void set_speed_limit(double speed);
/**
* \brief
*
@@ -285,6 +311,26 @@ class CDynamixelMotor
*
*/
void set_pid_control(TDynamixel_pid &config);
+ /**
+ * \brief
+ *
+ */
+ void get_vel_pi_control(TDynamixel_pid &config);
+ /**
+ * \brief
+ *
+ */
+ void set_vel_pi_control(TDynamixel_pid &config);
+ /**
+ * \brief
+ *
+ */
+ void get_feedfwd_control(double &acc_gain,double &vel_gain);
+ /**
+ * \brief
+ *
+ */
+ void set_feedfwd_control(double acc_gain,double vel_gain);
/* control functions */
/**
* \brief
@@ -320,20 +366,30 @@ class CDynamixelMotor
* \brief
*
*/
- void move_absolute_angle(double angle,double speed);
+ void move_absolute_angle(double angle,double speed,double current=-1.0);
+ /**
+ * \brief
+ *
+ */
+ void move_relative_angle(double angle,double speed,double current=-1.0);
+ /**
+ * \brief
+ *
+ */
+ void move_speed(double speed);
/**
* \brief
*
*/
- void move_relative_angle(double angle,double speed);
+ void move_pwm(double pwm_ratio);
/**
* \brief
*
*/
- void move_torque(double torque_ratio);
+ void move_current(double current);
/**
* \brief
- *
+ *
*/
void stop(void);
/**
@@ -350,7 +406,12 @@ class CDynamixelMotor
* \brief
*
*/
- double get_current_effort(void);
+ double get_current_pwm(void);
+ /**
+ * \brief
+ *
+ */
+ double get_current_current(void);
/**
* \brief
*
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 2f14c361ff2159f940f084646bcfd1814c2646af..f29b675e79693922500378f75993cdc573aa2ad9 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -29,7 +29,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,CDynamixelServe
this->group_id=group_id;
this->dyn_server=dyn_server;
this->clear();
- this->mode=angle_ctrl;
+ this->mode=position_ctrl;
}
}
@@ -47,7 +47,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,CDynamixelServe
this->group_id=group_id;
this->dyn_server=dyn_server;
this->clear();
- this->mode=torque_ctrl;
+ this->mode=pwm_ctrl;
if(ids.size()==0)
{
/* handle exceptions */
@@ -56,7 +56,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,CDynamixelServe
try{
for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version);
- this->set_control_mode(angle_ctrl);
+ this->set_control_mode(position_ctrl);
}catch(CException &e){
/* handle exceptions */
this->clear();
@@ -105,31 +105,31 @@ double CDynamixelMotorGroup::to_speeds(unsigned int index,int unsigned counts)
void CDynamixelMotorGroup::reset_motor(unsigned int index)
{
- unsigned short int current_position;
- unsigned short int maximum_torque;
+ unsigned int current_position;
+ unsigned int maximum_pwm;
try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][current_pos].address,¤t_position);
+ this->read_register(index,this->registers[index][current_pos],current_position);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->write_word_register(this->registers[index][goal_pos].address,current_position);
+ this->write_register(index,this->registers[index][goal_pos],current_position);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][max_pwm].address,&maximum_torque);
+ this->read_register(index,this->registers[index][max_pwm],maximum_pwm);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->write_word_register(this->registers[index][pwm_limit].address,maximum_torque);
+ this->write_register(index,this->registers[index][max_pwm],maximum_pwm);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->write_byte_register(this->registers[index][led].address,0x00);
+ this->write_register(index,this->registers[index][led],0);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
@@ -138,6 +138,7 @@ void CDynamixelMotorGroup::reset_motor(unsigned int index)
void CDynamixelMotorGroup::get_model(unsigned int index)
{
unsigned short int model;
+ unsigned int value;
if(this->dynamixel_dev[index]==NULL)
{
@@ -147,8 +148,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
else
{
try{
- this->dynamixel_dev[index]->read_byte_register(firmware_version,&this->info[index].firmware_ver);
- this->dynamixel_dev[index]->read_word_register(model_number,&model);
+ this->dynamixel_dev[index]->read_word_register(0x0000,&model);
switch(model)
{
case 0x000C: this->info[index].model="AX-12A";
@@ -240,69 +240,39 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
this->reset_motor(index);
throw;
}
+ this->read_register(index,this->registers[index][firmware_version],value);
+ this->info[index].firmware_ver=value;
}
}
void CDynamixelMotorGroup::set_position_range(unsigned int index,double min, double max)
{
- unsigned short int max_pos,min_pos;
+ unsigned int max_pos,min_pos;
- if(this->dynamixel_dev[index]==NULL)
+ max_pos=this->from_angles(index,max);
+ min_pos=this->from_angles(index,min);
+ if(min_pos<0.0 || max_pos>(this->info[index].encoder_resolution))
{
/* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ throw CDynamixelMotorException(_HERE_,"The desired range is outside the valid range of the servo.");
}
else
{
- max_pos=this->from_angles(index,max);
- min_pos=this->from_angles(index,min);
- if(min_pos<0.0 || max_pos>(this->info[index].encoder_resolution))
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The desired range is outside the valid range of the servo.");
- }
- else
- {
- try{
- this->config[index].max_angle=max;
- this->config[index].min_angle=min;
- this->dynamixel_dev[index]->write_word_register(this->registers[index][max_angle_limit].address,max_pos);
- usleep(10000);
- this->dynamixel_dev[index]->write_word_register(this->registers[index][min_angle_limit].address,min_pos);
- usleep(10000);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->config[index].max_angle=max;
+ this->config[index].min_angle=min;
+ this->write_register(index,this->registers[index][max_angle_limit],max_pos);
+ this->write_register(index,this->registers[index][min_angle_limit],min_pos);
}
}
void CDynamixelMotorGroup::get_position_range(unsigned int index,double *min, double *max)
{
- unsigned short int angle_limit;
+ unsigned int angle_limit;
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][max_angle_limit].address,&angle_limit);
- (*max)=this->to_angles(index,angle_limit);
- this->dynamixel_dev[index]->read_word_register(this->registers[index][min_angle_limit].address,&angle_limit);
- (*min)=this->to_angles(index,angle_limit);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->read_register(index,this->registers[index][max_angle_limit],angle_limit);
+ (*max)=this->to_angles(index,angle_limit);
+ this->read_register(index,this->registers[index][min_angle_limit],angle_limit);
+ (*min)=this->to_angles(index,angle_limit);
}
void CDynamixelMotorGroup::set_temperature_limit(unsigned int index,int limit)
@@ -314,283 +284,140 @@ void CDynamixelMotorGroup::set_temperature_limit(unsigned int index,int limit)
}
else
{
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->config[index].max_temperature=limit;
- this->dynamixel_dev[index]->write_byte_register(this->registers[index][temp_limit].address,(unsigned char)limit);
- usleep(10000);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->config[index].max_temperature=limit;
+ this->write_register(index,this->registers[index][temp_limit],limit);
}
}
int CDynamixelMotorGroup::get_temperature_limit(unsigned int index)
{
- unsigned char limit;
+ unsigned int limit;
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][temp_limit].address,&limit);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->read_register(index,this->registers[index][temp_limit],limit);
return limit;
}
void CDynamixelMotorGroup::set_voltage_limits(unsigned int index,double min, double max)
{
- unsigned char min_voltage,max_voltage;
+ unsigned int min_voltage,max_voltage;
- if(this->dynamixel_dev[index]==NULL)
+ if(min>max)
{
/* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ throw CDynamixelMotorException(_HERE_,"The maximum voltage is lower than the minimum voltage.");
}
else
{
- if(min>max)
+ if(min<5.0 || min>25.0 || max<5.0 || max>25.0)
{
/* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The maximum voltage is lower than the minimum voltage.");
+ throw CDynamixelMotorException(_HERE_,"The desired voltage range is outside the valid range.");
}
else
{
- if(min<5.0 || min>25.0 || max<5.0 || max>25.0)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The desired voltage range is outside the valid range.");
- }
- else
- {
- try{
- this->config[index].min_voltage=min;
- this->config[index].max_voltage=max;
- min_voltage=min*10;
- max_voltage=max*10;
- this->dynamixel_dev[index]->write_byte_register(this->registers[index][min_voltage_limit].address,min_voltage);
- usleep(10000);
- this->dynamixel_dev[index]->write_byte_register(this->registers[index][max_voltage_limit].address,max_voltage);
- usleep(10000);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->config[index].min_voltage=min;
+ this->config[index].max_voltage=max;
+ min_voltage=min*10;
+ max_voltage=max*10;
+ this->write_register(index,this->registers[index][min_voltage_limit],min_voltage);
+ this->write_register(index,this->registers[index][max_voltage_limit],max_voltage);
}
}
}
void CDynamixelMotorGroup::get_voltage_limits(unsigned int index,double *min, double *max)
{
- unsigned char min_voltage,max_voltage;
+ unsigned int min_voltage,max_voltage;
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][min_voltage_limit].address,&min_voltage);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][max_voltage_limit].address,&max_voltage);
- *min=((double)min_voltage/10.0);
- *max=((double)max_voltage/10.0);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->read_register(index,this->registers[index][min_voltage_limit],min_voltage);
+ this->read_register(index,this->registers[index][max_voltage_limit],max_voltage);
+ *min=((double)min_voltage/10.0);
+ *max=((double)max_voltage/10.0);
}
unsigned char CDynamixelMotorGroup::get_turn_off_alarms(unsigned int index)
{
- unsigned char shutdown_alarms=0x00;
+ unsigned int shutdown_alarms;
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][alarm_shtdwn].address,&shutdown_alarms);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->read_register(index,this->registers[index][alarm_shtdwn],shutdown_alarms);
return shutdown_alarms;
}
void CDynamixelMotorGroup::set_max_pwm(unsigned int index,double torque_ratio)
{
- unsigned short int load;
+ unsigned int load;
- if(this->dynamixel_dev[index]==NULL)
+ if(torque_ratio<0.0 || torque_ratio>100.0)
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
}
else
{
- if(torque_ratio<0.0 || torque_ratio>100.0)
- {
- throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
- }
- else
- {
- load=torque_ratio*1023/100.0;
- try{
- this->config[index].max_pwm=torque_ratio;
- this->dynamixel_dev[index]->write_word_register(this->registers[index][max_pwm].address,load);
- usleep(10000);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ load=torque_ratio*1023/100.0;
+ this->config[index].max_pwm=torque_ratio;
+ this->write_register(index,this->registers[index][max_pwm],load);
}
}
void CDynamixelMotorGroup::set_limit_torque(unsigned int index,double torque_ratio)
{
- unsigned short int load;
+ unsigned int load;
- if(this->dynamixel_dev[index]==NULL)
+ if(torque_ratio<0.0 || torque_ratio>100.0)
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
}
else
{
- if(torque_ratio<0.0 || torque_ratio>100.0)
- {
- throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
- }
- else
- {
- load=torque_ratio*1023/100.0;
- try{
- this->dynamixel_dev[index]->write_word_register(this->registers[index][pwm_limit].address,load);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ load=torque_ratio*1023/100.0;
+ this->write_register(index,this->registers[index][pwm_limit],load);
}
}
double CDynamixelMotorGroup::get_max_pwm(unsigned int index)
{
- unsigned short int load;
+ unsigned int load;
double torque;
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][max_pwm].address,&load);
- torque=(load&0x3FF)*100.0/1023;
- if(load>0x3FF)
- torque=-1*torque;
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->read_register(index,this->registers[index][max_pwm],load);
+ torque=(load&0x3FF)*100.0/1023;
+ if(load>0x3FF)
+ torque=-1*torque;
return torque;
}
void CDynamixelMotorGroup::get_compliance_control(unsigned int index,TDynamixel_compliance &config)
{
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
+ unsigned int value;
+
+ if(!this->info[index].pid_control)
{
- if(!this->info[index].pid_control)
- {
- try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_margin].address,&config.cw_compliance_margin);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_margin].address,&config.ccw_compliance_margin);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_slope].address,&config.cw_compliance_slope);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_slope].address,&config.ccw_compliance_slope);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->read_register(index,this->registers[index][cw_comp_margin],value);
+ config.cw_compliance_margin=value;
+ this->read_register(index,this->registers[index][ccw_comp_margin],value);
+ config.ccw_compliance_margin=value;
+ this->read_register(index,this->registers[index][cw_comp_slope],value);
+ config.cw_compliance_slope=value;
+ this->read_register(index,this->registers[index][ccw_comp_slope],value);
+ config.ccw_compliance_slope=value;
}
}
void CDynamixelMotorGroup::get_pid_control(unsigned int index,TDynamixel_pid &config)
{
- if(this->dynamixel_dev[index]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
+ unsigned int value;
+
+ if(this->info[index].pid_control)
{
- if(this->info[index].pid_control)
- {
- try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][pos_pid_p].address,&config.p);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][pos_pid_i].address,&config.i);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][pos_pid_d].address,&config.d);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[index])
- this->reset_motor(index);
- throw;
- }
- }
+ this->read_register(index,this->registers[index][pos_pid_p],value);
+ config.p=value;
+ this->read_register(index,this->registers[index][pos_pid_i],value);
+ config.i=value;
+ this->read_register(index,this->registers[index][pos_pid_d],value);
+ config.d=value;
}
}
@@ -647,7 +474,7 @@ void CDynamixelMotorGroup::clear(void)
void CDynamixelMotorGroup::set_control_mode(control_mode mode)
{
- unsigned int i=0;
+ unsigned int i=0,value;
if(this->mode!=mode)
{
@@ -655,38 +482,117 @@ void CDynamixelMotorGroup::set_control_mode(control_mode mode)
for(i=0;i<this->servo_id.size();i++)
{
try{
- if(this->dynamixel_dev[i]==NULL)
+ value=mode;
+ this->write_register(i,this->registers[i][op_mode],value);
+ }catch(CDynamixelMotorUnsupportedFeatureException &e){
+ if(this->info[i].model=="XL_320")
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ if(mode==pwm_ctrl)
+ value=1;
+ else if(mode==position_ctrl)
+ value=2;
+ else
+ throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"Unsupported control mode");
+ this->write_register(i,this->registers[i][op_mode],value);
}
else
{
- if(this->info[i].model=="XL_320")
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][op_mode].address,(unsigned char)mode);
+ if(mode==pwm_ctrl)
+ this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
+ else if(mode==position_ctrl)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
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);
- }
+ throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"Unsupported control mode");
+ }
+ }
+ }
+ this->mode=mode;
+ }
+}
+
+void CDynamixelMotorGroup::read_register(unsigned int index,TDynReg reg, unsigned int &value)
+{
+ unsigned char reg_value[4]={0};
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(reg.address==0xFFFF)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"The desired feature is not supported by this servo model.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->read_registers(reg.address,reg_value,reg.size);
+ 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);
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid register size.");
}
- this->mode=mode;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
throw;
}
}
}
}
+void CDynamixelMotorGroup::write_register(unsigned int index,TDynReg reg, unsigned int value)
+{
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(reg.address==0xFFFF)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"The desired feature is not supported by this servo model.");
+ }
+ else
+ {
+ if(reg.size>4)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid register size.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->write_registers(reg.address,(unsigned char *)&value,reg.size);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ if(reg.eeprom)
+ usleep(10000);
+ }
+ }
+}
+
void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
{
std::vector<TDynamixel_compliance> compliance;
- std::vector<unsigned short int> punch;
+ std::vector<unsigned int> punch;
std::vector<TDynamixel_pid> pid;
unsigned int i=0;
@@ -721,13 +627,13 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
}
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;
+ this->mode=pwm_ctrl;
for(i=1;i<this->servo_id.size();i++)
this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
}
else
{
- this->mode=angle_ctrl;
+ this->mode=position_ctrl;
for(i=1;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);
}
@@ -784,7 +690,7 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
{
dyn_motor_group_config_t::dyn_motor_config_iterator iterator;
std::vector<TDynamixel_compliance> compliance;
- std::vector<unsigned short int> punch;
+ std::vector<unsigned int> punch;
std::vector<TDynamixel_pid> pid;
std::string full_path,path;
struct stat buffer;
@@ -834,13 +740,13 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
}
if(cfg->dyn_motor_config().size()>0 && this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
{
- this->mode=torque_ctrl;
+ this->mode=pwm_ctrl;
for(i=1;i<this->servo_id.size();i++)
this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
}
else
{
- this->mode=angle_ctrl;
+ this->mode=position_ctrl;
for(i=1;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);
}
@@ -871,95 +777,79 @@ void CDynamixelMotorGroup::save_config(std::string &filename)
void CDynamixelMotorGroup::set_compliance_control(std::vector<TDynamixel_compliance> &config)
{
- unsigned int i=0;
+ unsigned int i=0,value;
for(i=0;i<this->servo_id.size();i++)
{
- if(this->dynamixel_dev[i]==NULL)
+ if(!this->info[i].pid_control)
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- if(!this->info[i].pid_control)
+ if(config[i].cw_compliance_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance margin.");
+ }
+ if(config[i].ccw_compliance_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
+ }
+ if(config[i].cw_compliance_slope>=1 && config[i].cw_compliance_slope<=5) config[i].cw_compliance_slope=4;
+ else if(config[i].cw_compliance_slope>=6 && config[i].cw_compliance_slope<=11) config[i].cw_compliance_slope=8;
+ else if(config[i].cw_compliance_slope>=12 && config[i].cw_compliance_slope<=23) config[i].cw_compliance_slope=16;
+ else if(config[i].cw_compliance_slope>=24 && config[i].cw_compliance_slope<=47) config[i].cw_compliance_slope=32;
+ else if(config[i].cw_compliance_slope>=48 && config[i].cw_compliance_slope<=95) config[i].cw_compliance_slope=64;
+ else if(config[i].cw_compliance_slope>=96 && config[i].cw_compliance_slope<=191) config[i].cw_compliance_slope=128;
+ else if(config[i].cw_compliance_slope>=192 && config[i].cw_compliance_slope<=254) config[i].cw_compliance_slope=254;
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance slope.");
+ }
+ if(config[i].ccw_compliance_slope>=1 && config[i].ccw_compliance_slope<=5) config[i].ccw_compliance_slope=4;
+ else if(config[i].ccw_compliance_slope>=6 && config[i].ccw_compliance_slope<=11) config[i].ccw_compliance_slope=8;
+ else if(config[i].ccw_compliance_slope>=12 && config[i].ccw_compliance_slope<=23) config[i].ccw_compliance_slope=16;
+ else if(config[i].ccw_compliance_slope>=24 && config[i].ccw_compliance_slope<=47) config[i].ccw_compliance_slope=32;
+ else if(config[i].ccw_compliance_slope>=48 && config[i].ccw_compliance_slope<=95) config[i].ccw_compliance_slope=64;
+ else if(config[i].ccw_compliance_slope>=96 && config[i].ccw_compliance_slope<=191) config[i].ccw_compliance_slope=128;
+ else if(config[i].ccw_compliance_slope>=192 && config[i].ccw_compliance_slope<=254) config[i].ccw_compliance_slope=254;
+ else
{
- if(config[i].cw_compliance_margin>254)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance margin.");
- }
- if(config[i].ccw_compliance_margin>254)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
- }
- if(config[i].cw_compliance_slope>=1 && config[i].cw_compliance_slope<=5) config[i].cw_compliance_slope=4;
- else if(config[i].cw_compliance_slope>=6 && config[i].cw_compliance_slope<=11) config[i].cw_compliance_slope=8;
- else if(config[i].cw_compliance_slope>=12 && config[i].cw_compliance_slope<=23) config[i].cw_compliance_slope=16;
- else if(config[i].cw_compliance_slope>=24 && config[i].cw_compliance_slope<=47) config[i].cw_compliance_slope=32;
- else if(config[i].cw_compliance_slope>=48 && config[i].cw_compliance_slope<=95) config[i].cw_compliance_slope=64;
- else if(config[i].cw_compliance_slope>=96 && config[i].cw_compliance_slope<=191) config[i].cw_compliance_slope=128;
- else if(config[i].cw_compliance_slope>=192 && config[i].cw_compliance_slope<=254) config[i].cw_compliance_slope=254;
- else
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance slope.");
- }
- if(config[i].ccw_compliance_slope>=1 && config[i].ccw_compliance_slope<=5) config[i].ccw_compliance_slope=4;
- else if(config[i].ccw_compliance_slope>=6 && config[i].ccw_compliance_slope<=11) config[i].ccw_compliance_slope=8;
- else if(config[i].ccw_compliance_slope>=12 && config[i].ccw_compliance_slope<=23) config[i].ccw_compliance_slope=16;
- else if(config[i].ccw_compliance_slope>=24 && config[i].ccw_compliance_slope<=47) config[i].ccw_compliance_slope=32;
- else if(config[i].ccw_compliance_slope>=48 && config[i].ccw_compliance_slope<=95) config[i].ccw_compliance_slope=64;
- else if(config[i].ccw_compliance_slope>=96 && config[i].ccw_compliance_slope<=191) config[i].ccw_compliance_slope=128;
- else if(config[i].ccw_compliance_slope>=192 && config[i].ccw_compliance_slope<=254) config[i].ccw_compliance_slope=254;
- else
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
- }
- try{
- this->compliance[i].cw_compliance_margin=config[i].cw_compliance_margin;
- this->compliance[i].ccw_compliance_margin=config[i].ccw_compliance_margin;
- this->compliance[i].cw_compliance_slope=config[i].cw_compliance_slope;
- this->compliance[i].ccw_compliance_slope=config[i].ccw_compliance_slope;
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][cw_comp_margin].address,config[i].cw_compliance_margin);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_margin].address,config[i].ccw_compliance_margin);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][cw_comp_slope].address,config[i].cw_compliance_slope);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_slope].address,config[i].ccw_compliance_slope);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
}
+ this->compliance[i].cw_compliance_margin=config[i].cw_compliance_margin;
+ this->compliance[i].ccw_compliance_margin=config[i].ccw_compliance_margin;
+ this->compliance[i].cw_compliance_slope=config[i].cw_compliance_slope;
+ this->compliance[i].ccw_compliance_slope=config[i].ccw_compliance_slope;
+ value=config[i].cw_compliance_margin;
+ this->write_register(i,this->registers[i][cw_comp_margin],value);
+ value=config[i].ccw_compliance_margin;
+ this->write_register(i,this->registers[i][ccw_comp_margin],value);
+ value=config[i].cw_compliance_slope;
+ this->write_register(i,this->registers[i][cw_comp_slope],value);
+ value=config[i].ccw_compliance_slope;
+ this->write_register(i,this->registers[i][ccw_comp_slope],value);
}
}
}
void CDynamixelMotorGroup::set_pid_control(std::vector<TDynamixel_pid> &config)
{
- unsigned int i=0;
+ unsigned int i=0,value;
for(i=0;i<this->servo_id.size();i++)
{
- if(this->dynamixel_dev[i]==NULL)
+ if(this->info[i].pid_control)
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- if(this->info[i].pid_control)
- {
- this->pid[i].p=config[i].p;
- this->pid[i].i=config[i].i;
- this->pid[i].d=config[i].d;
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][pos_pid_p].address,config[i].p);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][pos_pid_i].address,config[i].i);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][pos_pid_d].address,config[i].d);
- }
+ this->pid[i].p=config[i].p;
+ this->pid[i].i=config[i].i;
+ this->pid[i].d=config[i].d;
+ value=config[i].p;
+ this->write_register(i,this->registers[i][pos_pid_p],value);
+ value=config[i].i;
+ this->write_register(i,this->registers[i][pos_pid_i],value);
+ value=config[i].d;
+ this->write_register(i,this->registers[i][pos_pid_d],value);
}
}
}
@@ -969,24 +859,7 @@ void CDynamixelMotorGroup::enable(void)
unsigned int i=0;
for(i=0;i<this->servo_id.size();i++)
- {
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en].address,1);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
- }
+ this->write_register(i,this->registers[i][torque_en],1);
}
void CDynamixelMotorGroup::disable(void)
@@ -994,24 +867,7 @@ void CDynamixelMotorGroup::disable(void)
unsigned int i=0;
for(i=0;i<this->servo_id.size();i++)
- {
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en].address,0);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
- }
+ this->write_register(i,this->registers[i][torque_en],0);
}
void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std::vector<double> &speeds)
@@ -1020,62 +876,42 @@ void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std::
unsigned short int cmd[2];
unsigned int i=0;
- try{
- this->set_control_mode(angle_ctrl);
- data.resize(this->servo_id.size());
- for(i=0;i<this->servo_id.size();i++)
- {
- data[i].resize(4);
- cmd[0]=this->from_angles(i,angles[i]);
- cmd[1]=this->from_speeds(i,speeds[i]);
- data[i][0]=((int)(cmd[0])%256);
- data[i][1]=(int)(cmd[0]/256);
- data[i][2]=((int)(cmd[1])%256);
- data[i][3]=(int)(cmd[1]/256);
- }
- this->dyn_server->write_sync(this->servo_id, this->registers[0][goal_pos].address, data);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
+ // !!!!!!!!!!!!!!!!!!!! DOES NOT WORK for new servos !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+ this->set_control_mode(position_ctrl);
+ data.resize(this->servo_id.size());
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ data[i].resize(4);
+ cmd[0]=this->from_angles(i,angles[i]);
+ cmd[1]=this->from_speeds(i,speeds[i]);
+ data[i][0]=((int)(cmd[0])%256);
+ data[i][1]=(int)(cmd[0]/256);
+ data[i][2]=((int)(cmd[1])%256);
+ data[i][3]=(int)(cmd[1]/256);
}
+ this->dyn_server->write_sync(this->servo_id, this->registers[0][goal_pos].address, data);
}
void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::vector<double> &speeds)
{
std::vector< std::vector<unsigned char> > data;
- unsigned short int cmd[2],pos;
- unsigned int i=0;
+ unsigned short int cmd[2];
+ unsigned int i=0,pos;
- try{
- this->set_control_mode(angle_ctrl);
- data.resize(this->servo_id.size());
- for(i=0;i<this->servo_id.size();i++)
- {
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos].address,&pos);
- data[i].resize(4);
- cmd[0]=this->from_angles(i,angles[i]+this->to_angles(i,pos));
- cmd[1]=this->from_speeds(i,speeds[i]);
- data[i][0]=((int)(cmd[0])%256);
- data[i][1]=(int)(cmd[0]/256);
- data[i][2]=((int)(cmd[1])%256);
- data[i][3]=(int)(cmd[1]/256);
- }
- }
+ // !!!!!!!!!!!!!!!!!!!! DOES NOT WORK for new servos !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+ this->set_control_mode(position_ctrl);
+ data.resize(this->servo_id.size());
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ this->read_register(i,this->registers[i][current_pos],pos);
+ data[i].resize(4);
+ cmd[0]=this->from_angles(i,angles[i]+this->to_angles(i,pos));
+ cmd[1]=this->from_speeds(i,speeds[i]);
+ data[i][0]=((int)(cmd[0])%256);
+ data[i][1]=(int)(cmd[0]/256);
+ data[i][2]=((int)(cmd[1])%256);
+ data[i][3]=(int)(cmd[1]/256);
this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_pos].address,data);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
}
}
@@ -1085,214 +921,109 @@ void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios)
unsigned short int torque=0;
unsigned int i=0;
- try{
- this->set_control_mode(torque_ctrl);
- data.resize(this->servo_id.size());
- for(i=0;i<this->servo_id.size();i++)
- {
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- torque=0;
- data[i].resize(2);
- if(torque_ratios[i]<0.0)
- torque+=0x0400;
- torque+=((unsigned short int)(fabs(torque_ratios[i])*1023.0/100.0))&0x03FF;
- data[i][0]=((int)(torque)%256);
- data[i][1]=(int)(torque/256);
- }
- }
+ this->set_control_mode(pwm_ctrl);
+ data.resize(this->servo_id.size());
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ torque=0;
+ data[i].resize(2);
+ if(torque_ratios[i]<0.0)
+ torque+=0x0400;
+ torque+=((unsigned short int)(fabs(torque_ratios[i])*1023.0/100.0))&0x03FF;
+ data[i][0]=((int)(torque)%256);
+ data[i][1]=(int)(torque/256);
this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_speed].address,data);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
}
}
void CDynamixelMotorGroup::stop(void)
{
- unsigned short int current_position;
+ unsigned int current_position;
unsigned int i=0;
- try{
- for(i=0;i<this->servo_id.size();i++)
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->mode==position_ctrl)
{
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- if(this->mode==angle_ctrl)
- {
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos].address,¤t_position);
- if(current_position>this->to_angles(i,this->config[i].max_angle))
- current_position=this->from_angles(i,this->config[i].max_angle);
- else if(current_position<this->to_angles(i,this->config[i].min_angle))
- current_position=this->from_angles(i,this->config[i].min_angle);
- this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_pos].address,current_position);
- }
- else
- this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_speed].address,0);
- }
+ this->read_register(i,this->registers[i][current_pos],current_position);
+ if(current_position>this->to_angles(i,this->config[i].max_angle))
+ current_position=this->from_angles(i,this->config[i].max_angle);
+ else if(current_position<this->to_angles(i,this->config[i].min_angle))
+ current_position=this->from_angles(i,this->config[i].min_angle);
+ this->write_register(i,this->registers[i][goal_pos],current_position);
+ }
+ else
+ this->write_register(i,this->registers[i][goal_speed],0);
}
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
}
void CDynamixelMotorGroup::get_current_angle(std::vector<double> &angles)
{
- unsigned short int data;
+ unsigned int data;
angles.resize(this->servo_id.size());
for(unsigned int i=0; i<this->servo_id.size(); i++)
{
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos].address,&data);
- angles[i] = this->to_angles(i, data);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
+ this->read_register(i,this->registers[i][current_pos],data);
+ angles[i] = this->to_angles(i, data);
}
}
void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds)
{
- unsigned short int data;
+ unsigned int data;
speeds.resize(this->servo_id.size());
for(unsigned int i=0; i<this->servo_id.size(); i++)
{
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed].address,&data);
- speeds[i] = this->to_speeds(i, data&0x03FF);
- if(data&0x0400)
- speeds[i]*=-1.0;
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
+ this->read_register(i,this->registers[i][current_speed],data);
+ speeds[i] = this->to_speeds(i, data&0x03FF);
+ if(data&0x0400)
+ speeds[i]*=-1.0;
}
}
void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
{
- unsigned short int c_effort, c_speed;
+ unsigned int c_effort,c_speed;
efforts.resize(this->servo_id.size());
for(unsigned int i=0; i<this->servo_id.size(); i++)
{
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_load].address,&c_effort);
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed].address,&c_speed);
- efforts[i] = (double)((c_effort&0x3FF)*100.0/1023);
- if (c_speed&0x0400)
- efforts[i] *= -1.0;
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
+ this->read_register(i,this->registers[i][current_load],c_effort);
+ this->read_register(i,this->registers[i][current_speed],c_speed);
+ efforts[i] = (double)((c_effort&0x3FF)*100.0/1023);
+ if (c_speed&0x0400)
+ efforts[i] *= -1.0;
}
}
void CDynamixelMotorGroup::get_current_voltage(std::vector<double> &voltages)
{
- unsigned char data;
+ unsigned int data;
voltages.resize(this->servo_id.size());
for(unsigned int i=0; i<this->servo_id.size(); i++)
{
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_voltage].address,&data);
- voltages[i] = (double)data/10;
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
+ this->read_register(i,this->registers[i][current_voltage],data);
+ voltages[i] = (double)data/10;
}
}
void CDynamixelMotorGroup::get_current_temperature(std::vector<double> &temperatures)
{
- unsigned char data;
+ unsigned int data;
temperatures.resize(this->servo_id.size());
for(unsigned int i=0; i<this->servo_id.size(); i++)
{
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_temp].address,&data);
- temperatures[i] = (double)data;
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
+ this->read_register(i,this->registers[i][current_temp],data);
+ temperatures[i] = (double)data;
}
}
@@ -1301,55 +1032,23 @@ unsigned int CDynamixelMotorGroup::get_num_motors(void)
return this->servo_id.size();
}
-void CDynamixelMotorGroup::get_punch(std::vector<unsigned short int> &punches)
+void CDynamixelMotorGroup::get_punch(std::vector<unsigned int> &punches)
{
punches.resize(this->servo_id.size());
for(unsigned int i=0; i<this->servo_id.size(); i++)
- {
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][punch].address,&punches[i]);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
- }
+ this->read_register(i,this->registers[i][punch],punches[i]);
}
-void CDynamixelMotorGroup::set_punch(std::vector<unsigned short int> &punches)
+void CDynamixelMotorGroup::set_punch(std::vector<unsigned int> &punches)
{
punches.resize(this->servo_id.size());
for(unsigned int i=0; i<this->servo_id.size(); i++)
{
- if(this->dynamixel_dev[i]==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- if(punches[i]<0x0020 || punches[i]>0x03FF)
- throw CDynamixelMotorException(_HERE_,"Invalid punch value");
- this->dynamixel_dev[i]->write_word_register(this->registers[i][punch].address,punches[i]);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms[i])
- this->reset_motor(i);
- throw;
- }
- }
+ if(punches[i]<0x0020 || punches[i]>0x03FF)
+ throw CDynamixelMotorException(_HERE_,"Invalid punch value");
+ this->write_register(i,this->registers[i][punch],punches[i]);
}
}
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index 930b3e1c03ab6668563ff9bb943aae7b75106b2a..537a257abbe0cd1a80c279468df074177d371f4d 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -178,6 +178,16 @@ class CDynamixelMotorGroup
*
*/
void set_control_mode(control_mode mode);
+ /**
+ * \brief
+ *
+ */
+ void read_register(unsigned int index,TDynReg reg, unsigned int &value);
+ /**
+ * \brief
+ *
+ */
+ void write_register(unsigned int index,TDynReg reg, unsigned int value);
public:
/**
* \brief
@@ -286,12 +296,12 @@ class CDynamixelMotorGroup
* \brief
*
*/
- void get_punch(std::vector<unsigned short int> &punches);
+ void get_punch(std::vector<unsigned int> &punches);
/**
* \brief
*
*/
- void set_punch(std::vector<unsigned short int> &punches);
+ void set_punch(std::vector<unsigned int> &punches);
/**
* \brief
*
diff --git a/src/dynamixel_registers.cpp b/src/dynamixel_registers.cpp
index 71c94b5636d406ef4c18c174b27bcda05a13f02c..cabd2938a8cb49acb9edefc6f7144b1b17ed8325 100644
--- a/src/dynamixel_registers.cpp
+++ b/src/dynamixel_registers.cpp
@@ -112,7 +112,7 @@ TDynReg mx_1_0_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0x001C,1,false},
{0x001B,1,false},
- {0x001A,0,false},
+ {0x001A,1,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
@@ -181,7 +181,7 @@ TDynReg mx_106_1_0_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0x001C,1,false},
{0x001B,1,false},
- {0x001A,0,false},
+ {0x001A,1,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
@@ -250,7 +250,7 @@ TDynReg xl_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0x001D,1,false},
{0x001C,1,false},
- {0x001B,0,false},
+ {0x001B,1,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
@@ -276,3 +276,72 @@ TDynReg xl_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0xFFFF,0,false}};
+TDynReg xm_reg[NUM_REG]={// Info
+ {0x0000,2,true},
+ {0x0002,4,true},
+ {0x0006,1,true},
+ {0x0007,1,true},
+ {0x0008,1,true},
+ {0x0009,1,true},
+ {0x000C,1,true},
+ {0x000D,1,true},
+ {0x0044,1,false},
+ // Configuration
+ {0x000A,1,true},
+ {0x000B,1,true},
+ {0x0014,4,true},
+ {0x0018,4,true},
+ {0x0034,4,true},
+ {0x0030,4,true},
+ {0x001F,1,true},
+ {0x0022,2,true},
+ {0x0020,2,true},
+ {0x0024,2,true},
+ {0xFFFF,0,false},
+ {0x0026,2,true},
+ {0xFFFF,0,false},
+ {0x002C,4,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ // Status
+ {0x003F,1,true},
+ {0x003F,1,true},
+ {0x0041,1,false},
+ {0x0062,1,false},
+ {0x007A,1,false},
+ {0x007B,1,false},
+ {0xFFFF,0,false},
+ {0x0045,1,false},
+ {0x0046,1,false},
+ // Control
+ {0x0040,1,false},
+ {0x004C,2,false},
+ {0x004E,2,false},
+ {0x0054,2,false},
+ {0x0052,2,false},
+ {0x0050,2,false},
+ {0x0058,2,false},
+ {0x005A,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ // Set point
+ {0x0074,4,false},
+ {0x0068,4,false},
+ {0x0064,2,false},
+ {0x0066,2,false},
+ {0x006C,4,false},
+ {0x0070,4,false},
+ // Feedback
+ {0x0084,4,false},
+ {0x0080,4,false},
+ {0x007E,2,false},
+ {0x0090,2,false},
+ {0x0092,1,false},
+ {0x007C,2,false},
+ {0x0078,2,false},
+ {0x0088,5,false},
+ {0x008C,4,false}};
+
diff --git a/src/dynamixel_registers.h b/src/dynamixel_registers.h
index 159356e9d9d32db28017ea0bccbc50116c6427af..5e32a305a56317005c53c21cba49e1e5d767e499 100644
--- a/src/dynamixel_registers.h
+++ b/src/dynamixel_registers.h
@@ -13,6 +13,7 @@ extern TDynReg ax_reg[NUM_REG];
extern TDynReg mx_1_0_reg[NUM_REG];
extern TDynReg mx_106_1_0_reg[NUM_REG];
extern TDynReg xl_reg[NUM_REG];
+extern TDynReg xm_reg[NUM_REG];
typedef enum {
// [Access] [Description]
@@ -70,7 +71,7 @@ typedef enum {
// Set point
goal_pos, // (RW) Goal Position
goal_speed, // (RW) Moving Speed
- goal_pwn, // (RW)
+ goal_pwm, // (RW)
goal_current, // (RW)
profile_accel, // (RW)
profile_vel, // (RW)
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index 58914a15e87d6667202808c0102d274cc5e73e7a..cbf916125ab00bb2769f23fedd868d95e745635f 100644
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -11,9 +11,9 @@ std::string config_file="../src/xml/dyn_servo_config.xml";
int main(int argc, char *argv[])
{
- std::string serial="A400gavm"; //extracted from dynamixel library's test_dynamixel_server_no_scan or with 'dmesg' command: "SerialNumber: A600cVjj"
- int baudrate = 1000000; //57600 or 1000000
- int device = 1; //extracted from dynamixel library's test_dynamixel_server_no_scan
+ std::string serial="FT3M9M97"; //extracted from dynamixel library's test_dynamixel_server_no_scan or with 'dmesg' command: "SerialNumber: A600cVjj"
+ int baudrate = 57600; //57600 or 1000000
+ int device = 2; //extracted from dynamixel library's test_dynamixel_server_no_scan
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CDynamixelMotor *cont = NULL;
@@ -29,7 +29,7 @@ int main(int argc, char *argv[])
if(dyn_server->get_num_buses()>0)
{
dyn_server->config_bus(serial,baudrate);
- cont = new CDynamixelMotor(cont_name, dyn_server, device);
+ cont = new CDynamixelMotor(cont_name, dyn_server, device,dyn_version2);
/////////////////////////////////////////////////////////////////////////
////INFO
@@ -44,10 +44,6 @@ int main(int argc, char *argv[])
if(info.pid_control)
{
std::cout << "[INFO]: PID control capable" << std::endl;
- pid.p=10;
- pid.i=20;
- pid.d=30;
- cont->set_pid_control(pid);
cont->get_pid_control(pid);
std::cout << "[INFO]: PID control: (" << (int)pid.p << "," << (int)pid.i << "," << (int)pid.d << ")" << std::endl;
}
@@ -76,15 +72,15 @@ int main(int argc, char *argv[])
std::cout << "[INFO]: - Instruction Error:\t" << bool(sd_alarms&0x40) << std::endl;
led_alarms=cont->get_led_alarms();
std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl;
- std::cout << "[INFO]: max_pwm: " << std::dec << cont->get_max_pwm() << std::endl;
+ //std::cout << "[INFO]: max_pwm: " << std::dec << cont->get_max_pwm() << std::endl;
std::cout << "[INFO]: pwm_limit: " << std::dec << cont->get_pwm_limit() << std::endl;
- std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl;
+ //std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl;
cont->turn_led_on();
sleep(1);
cont->turn_led_off();
*/
-
+/*
double desired_speed = 100.0; //chosen speed when moving angle
double max_angle_error = 0.5; //max angle error permitted
double time_interval = 0.1; //time in secs between checks
@@ -94,7 +90,7 @@ int main(int argc, char *argv[])
int t;
double uperiod = time_interval*1000000.0;
double timeout = max_time_sec/(uperiod/1000000.0);
-
+*/
/////////////////////////////////////////////////////////////////////////
@@ -141,6 +137,7 @@ int main(int argc, char *argv[])
double current_abs_angle;
std::cout << "MOVE ABSOLUTE ANGLE: " << absolute_angle << std::endl;
+ cont->enable();
current_abs_angle = cont->get_current_angle();
desired_angle=absolute_angle;
std::cout << "Desired angle: " << desired_angle << std::endl;
@@ -165,63 +162,72 @@ int main(int argc, char *argv[])
std::cout << "Error angle: " << current_abs_angle-desired_angle << std::endl;
std::cout << "Done" << std::endl;
sleep(1);
+ cont->disable();
*/
/////////////////////////////////////////////////////////////////////////
- ////MOVE TORQUE
+ ////MOVE PWM
/////////////////////////////////////////////////////////////////////////
-/*
+
+ double time_interval = 0.1; //time in secs between checks
+ int max_time_sec = 10.0; //max time to wait until timeout
+ int t;
+ double uperiod = time_interval*1000000.0;
+ double timeout = max_time_sec/(uperiod/1000000.0);
+ double max_pwm,desired_pwm;
+
+ cont->enable();
std::cout << "-----------------------------------------------------------" << std::endl;
- double max_effort;
+ std::cout << "PWM limit " << cont->get_pwm_limit() << std::endl;
if(argc==2)
- max_effort = atoi(argv[1]);
+ max_pwm = atoi(argv[1]);
else
- max_effort = 50;
+ max_pwm = 50;
- double desired_effort=max_effort;
- std::cout << "MOVE EFFORT: " << desired_effort << std::endl;
+ desired_pwm=max_pwm;
+ std::cout << "MOVE PWM: " << desired_pwm << std::endl;
- cont->move_torque(desired_effort);
+ cont->move_pwm(desired_pwm);
std::cout << "Moving..." << std::endl;
t=0;
while(t<timeout)
{
- std::cout << "Current speed: " << cont->get_current_speed() << std::endl;
+ std::cout << "Current pwm: " << cont->get_current_pwm() << std::endl;
usleep(uperiod);
t++;
}
- cont->move_torque(0);
+ cont->move_pwm(0);
std::cout << "Done" << std::endl;
sleep(1);
std::cout << "----------------------------" << std::endl;
- desired_effort=-1.0*max_effort;
- std::cout << "MOVE EFFORT: " << desired_effort << std::endl;
+ desired_pwm=-1.0*max_pwm;
+ std::cout << "MOVE EFFORT: " << desired_pwm << std::endl;
- cont->move_torque(desired_effort);
+ cont->move_pwm(desired_pwm);
std::cout << "Moving..." << std::endl;
t=0;
while(t<timeout)
{
- std::cout << "Current speed: " << cont->get_current_speed() << std::endl;
+ std::cout << "Current pwm: " << cont->get_current_pwm() << std::endl;
usleep(uperiod);
t++;
}
- cont->move_torque(0);
+ cont->move_pwm(0);
std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
-*/
+ cont->disable();
/////////////////////////////////////////////////////////////////////////
////MOVE RANDOM TORQUES AND OUTPUT SPEEDS
/////////////////////////////////////////////////////////////////////////
///*
-
+/*
std::cout << "MOVE RANDOM EFFORTS" << std::endl;
int min=-100;
int max=100;
@@ -250,7 +256,7 @@ int main(int argc, char *argv[])
std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
-
+*/
}