diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index c729a8d8a6a4e538c92e055670e9381bfc76e87b..d188a135ed842a818c5dfd488c65a5d186b8dfbf 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1,5 +1,4 @@
#include "dynamixel_motor_exceptions.h"
-#include "dynamixel_registers.h"
#include "dynamixelexceptions.h"
#include "eventexceptions.h"
#include "dynamixel_motor.h"
@@ -22,6 +21,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->info.gear_ratio=(unsigned int)-1;
this->info.encoder_resolution=(unsigned int)-1;
this->info.pid_control=false;
+ this->info.multi_turn=false;
this->info.max_angle=-1;
this->info.center_angle=-1;
this->info.max_speed=-1;
@@ -33,13 +33,13 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->compliance.ccw_compliance_slope=0x20;
this->pid.p=0x00;
this->pid.i=0x00;
- this->pid.d=0x00;
+ this->pid.d=0x20;
this->config.max_angle=0.0;
this->config.min_angle=0.0;
this->config.max_temperature=0.0;
this->config.max_voltage=0.0;
this->config.min_voltage=0.0;
- this->config.max_torque=0.0;
+ this->config.max_pwm=0.0;
this->config.punch=0x20;
this->registers=NULL;
this->dyn_server=dyn_server;
@@ -52,7 +52,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_torque=this->get_max_torque();
+ this->config.max_pwm=this->get_max_pwm();
this->get_compliance_control(this->compliance);
this->get_pid_control(this->pid);
this->alarms=this->get_turn_off_alarms();
@@ -109,27 +109,27 @@ void CDynamixelMotor::reset_motor(void)
unsigned short int maximum_torque;
try{
- this->dynamixel_dev->read_word_register(this->registers[current_pos],¤t_position);
+ this->dynamixel_dev->read_word_register(this->registers[current_pos].address,¤t_position);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
+ this->dynamixel_dev->write_word_register(this->registers[goal_pos].address,current_position);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev->read_word_register(this->registers[max_torque],&maximum_torque);
+ this->dynamixel_dev->read_word_register(this->registers[max_pwm].address,&maximum_torque);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev->write_word_register(this->registers[torque_limit],maximum_torque);
+ this->dynamixel_dev->write_word_register(this->registers[pwm_limit].address,maximum_torque);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev->write_byte_register(this->registers[led],0x00);
+ this->dynamixel_dev->write_byte_register(this->registers[led].address,0x00);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
@@ -147,8 +147,8 @@ void CDynamixelMotor::get_model(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(std_firmware_version,&this->info.firmware_ver);
- this->dynamixel_dev->read_word_register(std_model_number,&model);
+ this->dynamixel_dev->read_byte_register(firmware_version,&this->info.firmware_ver);
+ this->dynamixel_dev->read_word_register(model_number,&model);
switch(model)
{
case 0x000C: this->info.model="AX-12A";
@@ -157,8 +157,9 @@ void CDynamixelMotor::get_model(void)
this->info.max_speed=354;
this->info.encoder_resolution=1023;
this->info.gear_ratio=254;
- this->registers=std_compl_reg;
+ this->registers=ax_reg;
this->info.pid_control=false;
+ this->info.multi_turn=false;
break;
case 0x012C: this->info.model="AX-12W";
this->info.max_angle=300.0;
@@ -166,8 +167,9 @@ void CDynamixelMotor::get_model(void)
this->info.max_speed=2830;
this->info.encoder_resolution=1023;
this->info.gear_ratio=32;
- this->registers=std_compl_reg;
+ this->registers=ax_reg;
this->info.pid_control=false;
+ this->info.multi_turn=false;
break;
case 0x0012: this->info.model="AX-18A";
this->info.max_angle=300.0;
@@ -175,8 +177,19 @@ void CDynamixelMotor::get_model(void)
this->info.max_speed=582;
this->info.encoder_resolution=1023;
this->info.gear_ratio=254;
- this->registers=std_compl_reg;
+ this->registers=ax_reg;
this->info.pid_control=false;
+ this->info.multi_turn=false;
+ break;
+ case 0x0168: this->info.model="MX-12";
+ this->info.max_angle=360.0;
+ this->info.center_angle=180.0;
+ this->info.max_speed=2820;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=32;
+ this->registers=mx_1_0_reg;
+ this->info.pid_control=true;
+ this->info.multi_turn=true;
break;
case 0x001D: this->info.model="MX-28";
this->info.max_angle=360.0;
@@ -184,26 +197,9 @@ void CDynamixelMotor::get_model(void)
this->info.max_speed=330;
this->info.encoder_resolution=4095;
this->info.gear_ratio=193;
- this->registers=std_pid_reg;
+ this->registers=mx_1_0_reg;
this->info.pid_control=true;
- break;
- case 0x0018: this->info.model="RX-24F";
- this->info.max_angle=300.0;
- this->info.center_angle=150.0;
- this->info.max_speed=756;
- this->info.encoder_resolution=1023;
- this->info.gear_ratio=193;
- this->registers=std_compl_reg;
- this->info.pid_control=false;
- break;
- case 0x001C: this->info.model="RX-28";
- this->info.max_angle=300.0;
- this->info.center_angle=150.0;
- this->info.max_speed=402;
- this->info.encoder_resolution=1023;
- this->info.gear_ratio=193;
- this->registers=std_compl_reg;
- this->info.pid_control=false;
+ this->info.multi_turn=true;
break;
case 0x0136: this->info.model="MX-64";
this->info.max_angle=360.0;
@@ -211,17 +207,9 @@ void CDynamixelMotor::get_model(void)
this->info.max_speed=378;
this->info.encoder_resolution=4095;
this->info.gear_ratio=200;
- this->registers=std_pid_reg;
+ this->registers=mx_1_0_reg;
this->info.pid_control=true;
- break;
- case 0x0040: this->info.model="Rx-64";
- this->info.max_angle=300.0;
- this->info.center_angle=150.0;
- this->info.max_speed=294;
- this->info.encoder_resolution=1023;
- this->info.gear_ratio=200;
- this->registers=std_compl_reg;
- this->info.pid_control=false;
+ this->info.multi_turn=true;
break;
case 0x0140: this->info.model="MX-106";
this->info.max_angle=360.0;
@@ -229,17 +217,9 @@ void CDynamixelMotor::get_model(void)
this->info.max_speed=270;
this->info.encoder_resolution=4095;
this->info.gear_ratio=225;
- this->registers=std_pid_reg;
+ this->registers=mx_106_1_0_reg;
this->info.pid_control=true;
- break;
- case 0x006B: this->info.model="Ex-106+";
- this->info.max_angle=250.0;
- this->info.center_angle=125;
- this->info.max_speed=414;
- this->info.encoder_resolution=4095;
- this->info.gear_ratio=184;
- this->registers=std_compl_reg;
- this->info.pid_control=false;
+ this->info.multi_turn=true;
break;
case 0x015E: this->info.model="XL_320";
this->info.max_angle=300.0;
@@ -249,6 +229,7 @@ void CDynamixelMotor::get_model(void)
this->info.gear_ratio=238;
this->registers=xl_reg;
this->info.pid_control=true;
+ this->info.multi_turn=false;
break;
default: this->info.model="unknown";
break;
@@ -277,7 +258,7 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
this->disable();
if(this->info.model=="XL_320")
{
- this->dynamixel_dev->write_byte_register(this->registers[dyn_control_mode],(unsigned char)mode);
+ this->dynamixel_dev->write_byte_register(this->registers[op_mode].address,(unsigned char)mode);
usleep(10000);
this->mode=mode;
}
@@ -309,8 +290,8 @@ void CDynamixelMotor::load_config(TDynamixel_config &config)
this->set_position_range(config.min_angle,config.max_angle);
this->set_temperature_limit(config.max_temperature);
this->set_voltage_limits(config.min_voltage,config.max_voltage);
- this->set_max_torque(config.max_torque);
- this->set_limit_torque(config.max_torque);
+ this->set_max_pwm(config.max_pwm);
+ this->set_pwm_limit(config.max_pwm);
}
#ifdef _HAVE_XSD
@@ -329,7 +310,7 @@ void CDynamixelMotor::read_config(std::string &filename,TDynamixel_config &confi
config.max_temperature=cfg->temp_limit();
config.max_voltage=cfg->max_voltage();
config.min_voltage=cfg->min_voltage();
- config.max_torque=cfg->max_torque();
+ config.max_pwm=cfg->max_pwm();
config.punch=cfg->punch();
/* pid control */
pid.p=cfg->kp();
@@ -383,8 +364,8 @@ void CDynamixelMotor::load_config(std::string &filename)
this->set_compliance_control(compliance);
}
this->set_punch(cfg->punch());
- this->set_max_torque(cfg->max_torque());
- this->set_limit_torque(cfg->max_torque());
+ this->set_max_pwm(cfg->max_pwm());
+ this->set_pwm_limit(cfg->max_pwm());
}catch (const xml_schema::exception& e){
std::ostringstream os;
os << e;
@@ -417,7 +398,7 @@ void CDynamixelMotor::save_config(std::string &filename)
this->get_temperature_limit(),
max_voltage,
min_voltage,
- this->get_max_torque(),
+ this->get_max_pwm(),
compliance.cw_compliance_margin,
compliance.ccw_compliance_margin,
compliance.cw_compliance_slope,
@@ -475,9 +456,9 @@ void CDynamixelMotor::set_position_range(double min, double max)
try{
this->config.max_angle=max;
this->config.min_angle=min;
- this->dynamixel_dev->write_word_register(this->registers[ccw_angle_limit],max_pos);
+ this->dynamixel_dev->write_word_register(this->registers[max_angle_limit].address,max_pos);
usleep(10000);
- this->dynamixel_dev->write_word_register(this->registers[cw_angle_limit],min_pos);
+ this->dynamixel_dev->write_word_register(this->registers[min_angle_limit].address,min_pos);
usleep(10000);
if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
this->mode=torque_ctrl;
@@ -505,9 +486,9 @@ void CDynamixelMotor::get_position_range(double *min, double *max)
else
{
try{
- this->dynamixel_dev->read_word_register(this->registers[ccw_angle_limit],&angle_limit);
+ this->dynamixel_dev->read_word_register(this->registers[max_angle_limit].address,&angle_limit);
(*max)=this->to_angles(angle_limit);
- this->dynamixel_dev->read_word_register(this->registers[cw_angle_limit],&angle_limit);
+ this->dynamixel_dev->read_word_register(this->registers[min_angle_limit].address,&angle_limit);
(*min)=this->to_angles(angle_limit);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -530,7 +511,7 @@ int CDynamixelMotor::get_temperature_limit(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[temp_limit],&limit);
+ this->dynamixel_dev->read_byte_register(this->registers[temp_limit].address,&limit);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -560,7 +541,7 @@ void CDynamixelMotor::set_temperature_limit(int limit)
{
try{
this->config.max_temperature=limit;
- this->dynamixel_dev->write_byte_register(this->registers[temp_limit],(unsigned char)limit);
+ this->dynamixel_dev->write_byte_register(this->registers[temp_limit].address,(unsigned char)limit);
usleep(10000);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -584,8 +565,8 @@ void CDynamixelMotor::get_voltage_limits(double *min, double *max)
else
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[low_voltage_limit],&min_voltage);
- this->dynamixel_dev->read_byte_register(this->registers[high_voltage_limit],&max_voltage);
+ this->dynamixel_dev->read_byte_register(this->registers[min_voltage_limit].address,&min_voltage);
+ this->dynamixel_dev->read_byte_register(this->registers[max_voltage_limit].address,&max_voltage);
*min=((double)min_voltage/10.0);
*max=((double)max_voltage/10.0);
}catch(CDynamixelAlarmException &e){
@@ -627,9 +608,9 @@ void CDynamixelMotor::set_voltage_limits(double min, double max)
this->config.max_voltage=max;
min_voltage=min*10;
max_voltage=max*10;
- this->dynamixel_dev->write_byte_register(this->registers[low_voltage_limit],min_voltage);
+ this->dynamixel_dev->write_byte_register(this->registers[min_voltage_limit].address,min_voltage);
usleep(10000);
- this->dynamixel_dev->write_byte_register(this->registers[high_voltage_limit],max_voltage);
+ this->dynamixel_dev->write_byte_register(this->registers[max_voltage_limit].address,max_voltage);
usleep(10000);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -654,7 +635,7 @@ unsigned char CDynamixelMotor::get_led_alarms(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[alarm_led],&led_alarms);
+ this->dynamixel_dev->read_byte_register(this->registers[alarm_led].address,&led_alarms);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -683,7 +664,7 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms)
else
{
try{
- this->dynamixel_dev->write_byte_register(this->registers[alarm_led],(unsigned char)alarms);
+ this->dynamixel_dev->write_byte_register(this->registers[alarm_led].address,(unsigned char)alarms);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -706,7 +687,7 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[alarm_shtdwn], &shutdown_alarms);
+ this->dynamixel_dev->read_byte_register(this->registers[alarm_shtdwn].address, &shutdown_alarms);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -736,7 +717,7 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
{
try{
this->alarms=alarms;
- this->dynamixel_dev->write_byte_register(this->registers[alarm_shtdwn],(unsigned char)alarms);
+ this->dynamixel_dev->write_byte_register(this->registers[alarm_shtdwn].address,(unsigned char)alarms);
usleep(10000);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -748,7 +729,7 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
}
}
-double CDynamixelMotor::get_max_torque(void)
+double CDynamixelMotor::get_max_pwm(void)
{
unsigned short int load;
double torque;
@@ -761,7 +742,7 @@ double CDynamixelMotor::get_max_torque(void)
else
{
try{
- this->dynamixel_dev->read_word_register(this->registers[max_torque],&load);
+ this->dynamixel_dev->read_word_register(this->registers[max_pwm].address,&load);
torque=(load&0x3FF)*100.0/1023;
if(load>0x3FF)
torque=-1*torque;
@@ -776,7 +757,7 @@ double CDynamixelMotor::get_max_torque(void)
return torque;
}
-double CDynamixelMotor::get_limit_torque(void)
+double CDynamixelMotor::get_pwm_limit(void)
{
unsigned short int load;
double torque;
@@ -789,7 +770,7 @@ double CDynamixelMotor::get_limit_torque(void)
else
{
try{
- this->dynamixel_dev->read_word_register(this->registers[torque_limit],&load);
+ this->dynamixel_dev->read_word_register(this->registers[pwm_limit].address,&load);
torque=(load&0x3FF)*100.0/1023;
if(load>0x3FF)
torque=-1*torque;
@@ -804,7 +785,7 @@ double CDynamixelMotor::get_limit_torque(void)
return torque;
}
-void CDynamixelMotor::set_max_torque(double torque_ratio)
+void CDynamixelMotor::set_max_pwm(double torque_ratio)
{
unsigned short int load;
@@ -823,8 +804,8 @@ void CDynamixelMotor::set_max_torque(double torque_ratio)
{
load=torque_ratio*1023/100.0;
try{
- this->config.max_torque=torque_ratio;
- this->dynamixel_dev->write_word_register(this->registers[max_torque],load);
+ this->config.max_pwm=torque_ratio;
+ this->dynamixel_dev->write_word_register(this->registers[max_pwm].address,load);
usleep(10000);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -836,7 +817,7 @@ void CDynamixelMotor::set_max_torque(double torque_ratio)
}
}
-void CDynamixelMotor::set_limit_torque(double torque_ratio)
+void CDynamixelMotor::set_pwm_limit(double torque_ratio)
{
unsigned short int load;
@@ -855,7 +836,7 @@ void CDynamixelMotor::set_limit_torque(double torque_ratio)
{
load=torque_ratio*1023/100.0;
try{
- this->dynamixel_dev->write_word_register(this->registers[torque_limit],load);
+ this->dynamixel_dev->write_word_register(this->registers[pwm_limit].address,load);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -878,10 +859,10 @@ void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
if(!this->info.pid_control)
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[cw_comp_margin],&config.cw_compliance_margin);
- this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_margin],&config.ccw_compliance_margin);
- this->dynamixel_dev->read_byte_register(this->registers[cw_comp_slope],&config.cw_compliance_slope);
- this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_slope],&config.ccw_compliance_slope);
+ this->dynamixel_dev->read_byte_register(this->registers[cw_comp_margin].address,&config.cw_compliance_margin);
+ this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_margin].address,&config.ccw_compliance_margin);
+ this->dynamixel_dev->read_byte_register(this->registers[cw_comp_slope].address,&config.cw_compliance_slope);
+ this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_slope].address,&config.ccw_compliance_slope);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -942,10 +923,10 @@ void CDynamixelMotor::set_compliance_control(TDynamixel_compliance &config)
this->compliance.ccw_compliance_margin=config.ccw_compliance_margin;
this->compliance.cw_compliance_slope=config.cw_compliance_slope;
this->compliance.ccw_compliance_slope=config.ccw_compliance_slope;
- this->dynamixel_dev->write_byte_register(this->registers[cw_comp_margin],config.cw_compliance_margin);
- this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_margin],config.ccw_compliance_margin);
- this->dynamixel_dev->write_byte_register(this->registers[cw_comp_slope],config.cw_compliance_slope);
- this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_slope],config.ccw_compliance_slope);
+ this->dynamixel_dev->write_byte_register(this->registers[cw_comp_margin].address,config.cw_compliance_margin);
+ this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_margin].address,config.ccw_compliance_margin);
+ this->dynamixel_dev->write_byte_register(this->registers[cw_comp_slope].address,config.cw_compliance_slope);
+ this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_slope].address,config.ccw_compliance_slope);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -968,9 +949,9 @@ void CDynamixelMotor::get_pid_control(TDynamixel_pid &config)
if(this->info.pid_control)
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[pid_p],&config.p);
- this->dynamixel_dev->read_byte_register(this->registers[pid_i],&config.i);
- this->dynamixel_dev->read_byte_register(this->registers[pid_d],&config.d);
+ this->dynamixel_dev->read_byte_register(this->registers[pos_pid_p].address,&config.p);
+ this->dynamixel_dev->read_byte_register(this->registers[pos_pid_i].address,&config.i);
+ this->dynamixel_dev->read_byte_register(this->registers[pos_pid_d].address,&config.d);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -995,9 +976,9 @@ void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
this->pid.p=config.p;
this->pid.i=config.i;
this->pid.d=config.d;
- this->dynamixel_dev->write_byte_register(this->registers[pid_p],config.p);
- this->dynamixel_dev->write_byte_register(this->registers[pid_i],config.i);
- this->dynamixel_dev->write_byte_register(this->registers[pid_d],config.d);
+ this->dynamixel_dev->write_byte_register(this->registers[pos_pid_p].address,config.p);
+ this->dynamixel_dev->write_byte_register(this->registers[pos_pid_i].address,config.i);
+ this->dynamixel_dev->write_byte_register(this->registers[pos_pid_d].address,config.d);
}
}
}
@@ -1012,7 +993,7 @@ void CDynamixelMotor::turn_led_on(void)
else
{
try{
- this->dynamixel_dev->write_byte_register(this->registers[led],1);
+ this->dynamixel_dev->write_byte_register(this->registers[led].address,1);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1032,7 +1013,7 @@ void CDynamixelMotor::turn_led_off(void)
else
{
try{
- this->dynamixel_dev->write_byte_register(this->registers[led],0);
+ this->dynamixel_dev->write_byte_register(this->registers[led].address,0);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1062,7 +1043,7 @@ void CDynamixelMotor::enable(void)
else
{
try{
- this->dynamixel_dev->write_byte_register(this->registers[torque_en],1);
+ this->dynamixel_dev->write_byte_register(this->registers[torque_en].address,1);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1082,7 +1063,7 @@ void CDynamixelMotor::disable(void)
else
{
try{
- this->dynamixel_dev->write_byte_register(this->registers[torque_en],0);
+ this->dynamixel_dev->write_byte_register(this->registers[torque_en].address,0);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1113,7 +1094,7 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
if(speed>this->info.max_speed)
speed=this->info.max_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].address,(unsigned char *)cmd,4);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1137,7 +1118,7 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed)
{
try{
this->set_control_mode(angle_ctrl);
- this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos);
+ this->dynamixel_dev->read_word_register(this->registers[current_pos].address,&pos);
abs_angle=angle+this->to_angles(pos);
if(abs_angle>this->info.center_angle)
abs_angle=this->info.center_angle;
@@ -1147,7 +1128,7 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed)
if(speed>this->info.max_speed)
speed=this->info.max_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].address,(unsigned char *)cmd,4);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1177,7 +1158,7 @@ void CDynamixelMotor::move_torque(double torque_ratio)
if(torque_ratio<0.0)
torque+=0x0400;
torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
- this->dynamixel_dev->write_word_register(this->registers[goal_speed],torque);
+ this->dynamixel_dev->write_word_register(this->registers[goal_speed].address,torque);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1201,15 +1182,15 @@ void CDynamixelMotor::stop(void)
try{
if(this->mode==angle_ctrl)
{
- this->dynamixel_dev->read_word_register(this->registers[current_pos],¤t_position);
+ this->dynamixel_dev->read_word_register(this->registers[current_pos].address,¤t_position);
if(this->to_angles(current_position)>this->config.max_angle)
current_position=from_angles(this->config.max_angle);
else if(this->to_angles(current_position)<this->config.min_angle)
current_position=from_angles(this->config.min_angle);
- this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
+ this->dynamixel_dev->write_word_register(this->registers[goal_pos].address,current_position);
}
else
- this->dynamixel_dev->write_word_register(this->registers[goal_speed],0);
+ this->dynamixel_dev->write_word_register(this->registers[goal_speed].address,0);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1232,7 +1213,7 @@ double CDynamixelMotor::get_current_angle(void)
else
{
try{
- this->dynamixel_dev->read_word_register(this->registers[current_pos],&data);
+ this->dynamixel_dev->read_word_register(this->registers[current_pos].address,&data);
current_position = this->to_angles(data);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -1258,7 +1239,7 @@ double CDynamixelMotor::get_current_speed(void)
else
{
try{
- this->dynamixel_dev->read_word_register(this->registers[current_speed],&data);
+ this->dynamixel_dev->read_word_register(this->registers[current_speed].address,&data);
c_speed = this->to_speeds(data&0x03FF);
if(data&0x0400)
c_speed*=-1.0;
@@ -1286,7 +1267,7 @@ double CDynamixelMotor::get_current_temperature(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[current_temp],&data);
+ this->dynamixel_dev->read_byte_register(this->registers[current_temp].address,&data);
c_temp = (double)data;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -1312,7 +1293,7 @@ double CDynamixelMotor::get_current_voltage(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(this->registers[current_voltage],&data);
+ this->dynamixel_dev->read_byte_register(this->registers[current_voltage].address,&data);
c_voltage = (double)data/10;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -1339,7 +1320,7 @@ double CDynamixelMotor::get_current_effort(void)
{
try
{
- this->dynamixel_dev->read_word_register(this->registers[current_load],&data);
+ this->dynamixel_dev->read_word_register(this->registers[current_load].address,&data);
c_effort = (double)((data&0x3FF)*100.0/1023);
if(this->get_current_speed() < 0)
c_effort *= -1.0;
@@ -1368,7 +1349,7 @@ unsigned short int CDynamixelMotor::get_punch(void)
else
{
try{
- this->dynamixel_dev->read_word_register(this->registers[punch],&value);
+ this->dynamixel_dev->read_word_register(this->registers[punch].address,&value);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1393,7 +1374,7 @@ void CDynamixelMotor::set_punch(unsigned short int punch_value)
if(punch_value<0x0020 || punch_value>0x03FF)
throw CDynamixelMotorException(_HERE_,"Invalid Punch value");
this->config.punch=punch_value;
- this->dynamixel_dev->write_word_register(this->registers[punch],punch_value);
+ this->dynamixel_dev->write_word_register(this->registers[punch].address,punch_value);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1415,7 +1396,7 @@ control_mode CDynamixelMotor::get_control_mode(void)
try{
if(this->info.model=="XL-320")
{
- this->dynamixel_dev->read_byte_register(this->registers[dyn_control_mode],(unsigned char *)&this->mode);
+ this->dynamixel_dev->read_byte_register(this->registers[op_mode].address,(unsigned char *)&this->mode);
}
else
{
@@ -1457,6 +1438,7 @@ CDynamixelMotor::~CDynamixelMotor()
this->info.gear_ratio=(unsigned int)-1;
this->info.encoder_resolution=(unsigned int)-1;
this->info.pid_control=false;
+ this->info.multi_turn=false;
this->info.max_angle=-1;
this->info.center_angle=-1;
this->info.max_speed=-1;
@@ -1474,6 +1456,6 @@ CDynamixelMotor::~CDynamixelMotor()
this->config.max_temperature=0.0;
this->config.max_voltage=0.0;
this->config.min_voltage=0.0;
- this->config.max_torque=0.0;
+ this->config.max_pwm=0.0;
}
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index d466a2ac165d968ab42ead5964a66d6e94a85874..3f75e205716ccd3c38b8d4a893eebda29aec2f27 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -2,6 +2,7 @@
#define _DYNAMIXEL_MOTOR_H
#include "dynamixelserver.h"
+#include "dynamixel_registers.h"
#include "threadserver.h"
#include "eventserver.h"
#include "ftdimodule.h"
@@ -30,6 +31,7 @@ typedef struct
unsigned int gear_ratio;
unsigned int encoder_resolution;
bool pid_control;
+ bool multi_turn;
double max_angle;
double center_angle;
double max_speed;
@@ -59,7 +61,7 @@ typedef struct
double max_temperature;
double max_voltage;
double min_voltage;
- double max_torque;
+ double max_pwm;
unsigned short int punch;
}TDynamixel_config;
@@ -112,7 +114,7 @@ class CDynamixelMotor
* \brief
*
*/
- unsigned short int *registers;
+ TDynReg *registers;
protected:
/**
* \brief
@@ -237,22 +239,22 @@ class CDynamixelMotor
* \brief
*
*/
- double get_max_torque(void);
+ double get_max_pwm(void);
/**
* \brief
*
*/
- double get_limit_torque(void);
+ double get_pwm_limit(void);
/**
* \brief
*
*/
- void set_max_torque(double torque_ratio);
+ void set_max_pwm(double torque_ratio);
/**
* \brief
*
*/
- void set_limit_torque(double torque_ratio);
+ void set_pwm_limit(double torque_ratio);
/**
* \brief
*
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 202f59cdc16640c8e952a8a0646052e824f94468..2f14c361ff2159f940f084646bcfd1814c2646af 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -109,27 +109,27 @@ void CDynamixelMotorGroup::reset_motor(unsigned int index)
unsigned short int maximum_torque;
try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][current_pos],¤t_position);
+ this->dynamixel_dev[index]->read_word_register(this->registers[index][current_pos].address,¤t_position);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->write_word_register(this->registers[index][goal_pos],current_position);
+ this->dynamixel_dev[index]->write_word_register(this->registers[index][goal_pos].address,current_position);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][max_torque],&maximum_torque);
+ this->dynamixel_dev[index]->read_word_register(this->registers[index][max_pwm].address,&maximum_torque);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->write_word_register(this->registers[index][torque_limit],maximum_torque);
+ this->dynamixel_dev[index]->write_word_register(this->registers[index][pwm_limit].address,maximum_torque);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
try{
- this->dynamixel_dev[index]->write_byte_register(this->registers[index][led],0x00);
+ this->dynamixel_dev[index]->write_byte_register(this->registers[index][led].address,0x00);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
@@ -147,99 +147,79 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
else
{
try{
- this->dynamixel_dev[index]->read_byte_register(std_firmware_version,&this->info[index].firmware_ver);
- this->dynamixel_dev[index]->read_word_register(std_model_number,&model);
+ this->dynamixel_dev[index]->read_byte_register(firmware_version,&this->info[index].firmware_ver);
+ this->dynamixel_dev[index]->read_word_register(model_number,&model);
switch(model)
{
- case 0x000C: this->info[index].model="AX-12A";
- this->info[index].max_angle=300.0;
- this->info[index].center_angle=150.0;
- this->info[index].max_speed=354.0;
- this->info[index].encoder_resolution=1023;
- this->info[index].gear_ratio=254;
- this->registers[index]=std_compl_reg;
- this->info[index].pid_control=false;
- break;
- case 0x012C: this->info[index].model="AX-12W";
- this->info[index].max_angle=300.0;
- this->info[index].center_angle=150.0;
- this->info[index].max_speed=2830;
- this->info[index].encoder_resolution=1023;
- this->info[index].gear_ratio=32;
- this->registers[index]=std_compl_reg;
- this->info[index].pid_control=false;
- break;
- case 0x0012: this->info[index].model="AX-18A";
- this->info[index].max_angle=300.0;
- this->info[index].center_angle=150.0;
- this->info[index].max_speed=582;
- this->info[index].encoder_resolution=1023;
- this->info[index].gear_ratio=254;
- this->registers[index]=std_compl_reg;
- this->info[index].pid_control=false;
- break;
- case 0x001D: this->info[index].model="MX-28";
- this->info[index].max_angle=360.0;
- this->info[index].center_angle=180.0;
- this->info[index].max_speed=330;
- this->info[index].encoder_resolution=4095;
- this->info[index].gear_ratio=193;
- this->registers[index]=std_pid_reg;
- this->info[index].pid_control=true;
- break;
- case 0x0018: this->info[index].model="RX-24F";
- this->info[index].max_angle=300.0;
- this->info[index].center_angle=150.0;
- this->info[index].max_speed=756;
- this->info[index].encoder_resolution=1023;
- this->info[index].gear_ratio=193;
- this->registers[index]=std_compl_reg;
- this->info[index].pid_control=false;
- break;
- case 0x001C: this->info[index].model="RX-28";
- this->info[index].max_angle=300.0;
- this->info[index].center_angle=150.0;
- this->info[index].max_speed=402;
- this->info[index].encoder_resolution=1023;
- this->info[index].gear_ratio=193;
- this->registers[index]=std_compl_reg;
- this->info[index].pid_control=false;
- break;
- case 0x0136: this->info[index].model="MX-64";
- this->info[index].max_angle=360.0;
- this->info[index].center_angle=180.0;
- this->info[index].max_speed=378;
- this->info[index].encoder_resolution=4095;
- this->info[index].gear_ratio=200;
- this->registers[index]=std_pid_reg;
- this->info[index].pid_control=true;
- break;
- case 0x0040: this->info[index].model="Rx-64";
- this->info[index].max_angle=300.0;
- this->info[index].center_angle=150.0;
- this->info[index].max_speed=294;
- this->info[index].encoder_resolution=1023;
- this->info[index].gear_ratio=200;
- this->registers[index]=std_compl_reg;
- this->info[index].pid_control=false;
- break;
- case 0x0140: this->info[index].model="MX-106";
- this->info[index].max_angle=360.0;
- this->info[index].center_angle=180.0;
- this->info[index].max_speed=270;
- this->info[index].encoder_resolution=4095;
- this->info[index].gear_ratio=225;
- this->registers[index]=std_pid_reg;
- this->info[index].pid_control=true;
- break;
- case 0x006B: this->info[index].model="Ex-106+";
- this->info[index].max_angle=251.0;
- this->info[index].center_angle=125.5;
- this->info[index].max_speed=414;
- this->info[index].encoder_resolution=4095;
- this->info[index].gear_ratio=184;
- this->registers[index]=std_compl_reg;
- this->info[index].pid_control=false;
+ case 0x000C: this->info[index].model="AX-12A";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=354;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=254;
+ this->registers[index]=ax_reg;
+ this->info[index].pid_control=false;
+ this->info[index].multi_turn=false;
+ break;
+ case 0x012C: this->info[index].model="AX-12W";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=2830;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=32;
+ this->registers[index]=ax_reg;
+ this->info[index].pid_control=false;
+ this->info[index].multi_turn=false;
+ break;
+ case 0x0012: this->info[index].model="AX-18A";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=582;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=254;
+ this->registers[index]=ax_reg;
+ this->info[index].pid_control=false;
+ this->info[index].multi_turn=false;
+ break;
+ case 0x0168: this->info[index].model="MX-12";
+ this->info[index].max_angle=360.0;
+ this->info[index].center_angle=180.0;
+ this->info[index].max_speed=2820;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=32;
+ this->registers[index]=mx_1_0_reg;
+ this->info[index].pid_control=true;
+ this->info[index].multi_turn=true;
+ break;
+ case 0x001D: this->info[index].model="MX-28";
+ this->info[index].max_angle=360.0;
+ this->info[index].center_angle=180.0;
+ this->info[index].max_speed=330;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=193;
+ this->registers[index]=mx_1_0_reg;
+ this->info[index].pid_control=true;
+ this->info[index].multi_turn=true;
+ break;
+ case 0x0136: this->info[index].model="MX-64";
+ this->info[index].max_angle=360.0;
+ this->info[index].center_angle=180.0;
+ this->info[index].max_speed=378;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=200;
+ this->registers[index]=mx_1_0_reg;
+ this->info[index].pid_control=true;
+ this->info[index].multi_turn=true;
+ break;
+ case 0x0140: this->info[index].model="MX-106";
+ this->info[index].max_angle=360.0;
+ this->info[index].center_angle=180.0;
+ this->info[index].max_speed=270;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=225;
+ this->registers[index]=mx_106_1_0_reg;
+ this->info[index].pid_control=true;
+ this->info[index].multi_turn=true;
break;
case 0x015E: this->info[index].model="XL_320";
this->info[index].max_angle=300.0;
@@ -249,7 +229,8 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
this->info[index].gear_ratio=238;
this->registers[index]=xl_reg;
this->info[index].pid_control=true;
- break;
+ this->info[index].multi_turn=false;
+ break;
default: this->info[index].model="unknown";
break;
}
@@ -285,9 +266,9 @@ void CDynamixelMotorGroup::set_position_range(unsigned int index,double min, dou
try{
this->config[index].max_angle=max;
this->config[index].min_angle=min;
- this->dynamixel_dev[index]->write_word_register(this->registers[index][ccw_angle_limit],max_pos);
+ 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][cw_angle_limit],min_pos);
+ this->dynamixel_dev[index]->write_word_register(this->registers[index][min_angle_limit].address,min_pos);
usleep(10000);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -311,9 +292,9 @@ void CDynamixelMotorGroup::get_position_range(unsigned int index,double *min, do
else
{
try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][ccw_angle_limit],&angle_limit);
+ 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][cw_angle_limit],&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 */
@@ -342,7 +323,7 @@ void CDynamixelMotorGroup::set_temperature_limit(unsigned int index,int limit)
{
try{
this->config[index].max_temperature=limit;
- this->dynamixel_dev[index]->write_byte_register(this->registers[index][temp_limit],(unsigned char)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 */
@@ -366,7 +347,7 @@ int CDynamixelMotorGroup::get_temperature_limit(unsigned int index)
else
{
try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][temp_limit],&limit);
+ 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])
@@ -408,9 +389,9 @@ void CDynamixelMotorGroup::set_voltage_limits(unsigned int index,double min, dou
this->config[index].max_voltage=max;
min_voltage=min*10;
max_voltage=max*10;
- this->dynamixel_dev[index]->write_byte_register(this->registers[index][low_voltage_limit],min_voltage);
+ 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][high_voltage_limit],max_voltage);
+ this->dynamixel_dev[index]->write_byte_register(this->registers[index][max_voltage_limit].address,max_voltage);
usleep(10000);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -435,8 +416,8 @@ void CDynamixelMotorGroup::get_voltage_limits(unsigned int index,double *min, do
else
{
try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][low_voltage_limit],&min_voltage);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][high_voltage_limit],&max_voltage);
+ 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){
@@ -460,7 +441,7 @@ unsigned char CDynamixelMotorGroup::get_turn_off_alarms(unsigned int index)
else
{
try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][alarm_shtdwn],&shutdown_alarms);
+ 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])
@@ -472,7 +453,7 @@ unsigned char CDynamixelMotorGroup::get_turn_off_alarms(unsigned int index)
return shutdown_alarms;
}
-void CDynamixelMotorGroup::set_max_torque(unsigned int index,double torque_ratio)
+void CDynamixelMotorGroup::set_max_pwm(unsigned int index,double torque_ratio)
{
unsigned short int load;
@@ -491,8 +472,8 @@ void CDynamixelMotorGroup::set_max_torque(unsigned int index,double torque_ratio
{
load=torque_ratio*1023/100.0;
try{
- this->config[index].max_torque=torque_ratio;
- this->dynamixel_dev[index]->write_word_register(this->registers[index][max_torque],load);
+ 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 */
@@ -523,7 +504,7 @@ void CDynamixelMotorGroup::set_limit_torque(unsigned int index,double torque_rat
{
load=torque_ratio*1023/100.0;
try{
- this->dynamixel_dev[index]->write_word_register(this->registers[index][torque_limit],load);
+ 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])
@@ -534,7 +515,7 @@ void CDynamixelMotorGroup::set_limit_torque(unsigned int index,double torque_rat
}
}
-double CDynamixelMotorGroup::get_max_torque(unsigned int index)
+double CDynamixelMotorGroup::get_max_pwm(unsigned int index)
{
unsigned short int load;
double torque;
@@ -547,7 +528,7 @@ double CDynamixelMotorGroup::get_max_torque(unsigned int index)
else
{
try{
- this->dynamixel_dev[index]->read_word_register(this->registers[index][max_torque],&load);
+ 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;
@@ -574,10 +555,10 @@ void CDynamixelMotorGroup::get_compliance_control(unsigned int index,TDynamixel_
if(!this->info[index].pid_control)
{
try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_margin],&config.cw_compliance_margin);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_margin],&config.ccw_compliance_margin);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_slope],&config.cw_compliance_slope);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_slope],&config.ccw_compliance_slope);
+ 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])
@@ -600,9 +581,9 @@ void CDynamixelMotorGroup::get_pid_control(unsigned int index,TDynamixel_pid &co
if(this->info[index].pid_control)
{
try{
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_p],&config.p);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_i],&config.i);
- this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_d],&config.d);
+ 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])
@@ -631,7 +612,7 @@ void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version)
this->get_position_range(this->servo_id.size(),&config.min_angle,&config.max_angle);
config.max_temperature=this->get_temperature_limit(this->servo_id.size());
this->get_voltage_limits(this->servo_id.size(),&config.min_voltage,&config.max_voltage);
- config.max_torque=this->get_max_torque(this->servo_id.size());
+ config.max_pwm=this->get_max_pwm(this->servo_id.size());
this->config.push_back(config);
this->get_compliance_control(this->servo_id.size(),compliance);
this->compliance.push_back(compliance);
@@ -682,7 +663,7 @@ void CDynamixelMotorGroup::set_control_mode(control_mode mode)
else
{
if(this->info[i].model=="XL_320")
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][dyn_control_mode],(unsigned char)mode);
+ this->dynamixel_dev[i]->write_byte_register(this->registers[i][op_mode].address,(unsigned char)mode);
else
{
if(mode==torque_ctrl)
@@ -735,8 +716,8 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
compliance[i].ccw_compliance_slope=config.compliance_control[i].ccw_compliance_slope;
}
punch[i]=config.dyn_config[i].punch;
- this->set_max_torque(i,config.dyn_config[i].max_torque);
- this->set_limit_torque(i,config.dyn_config[i].max_torque);
+ this->set_max_pwm(i,config.dyn_config[i].max_pwm);
+ this->set_limit_torque(i,config.dyn_config[i].max_pwm);
}
if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
{
@@ -848,8 +829,8 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
compliance[i].ccw_compliance_slope=motor->ccw_comp_slope();
}
punch[i]=motor->punch();
- this->set_max_torque(i,motor->max_torque());
- this->set_limit_torque(i,motor->max_torque());
+ this->set_max_pwm(i,motor->max_pwm());
+ this->set_limit_torque(i,motor->max_pwm());
}
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)
{
@@ -942,10 +923,10 @@ void CDynamixelMotorGroup::set_compliance_control(std::vector<TDynamixel_complia
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],config[i].cw_compliance_margin);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_margin],config[i].ccw_compliance_margin);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][cw_comp_slope],config[i].cw_compliance_slope);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_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])
@@ -975,9 +956,9 @@ void CDynamixelMotorGroup::set_pid_control(std::vector<TDynamixel_pid> &config)
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][pid_p],config[i].p);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_i],config[i].i);
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_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);
}
}
}
@@ -997,7 +978,7 @@ void CDynamixelMotorGroup::enable(void)
else
{
try{
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en],1);
+ 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])
@@ -1022,7 +1003,7 @@ void CDynamixelMotorGroup::disable(void)
else
{
try{
- this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en],0);
+ 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])
@@ -1052,7 +1033,7 @@ void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std::
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], data);
+ 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])
@@ -1079,7 +1060,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::
}
else
{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&pos);
+ 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]);
@@ -1089,7 +1070,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::
data[i][3]=(int)(cmd[1]/256);
}
}
- this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_pos],data);
+ 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])
@@ -1125,7 +1106,7 @@ void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios)
data[i][1]=(int)(torque/256);
}
}
- this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_speed],data);
+ 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])
@@ -1151,15 +1132,15 @@ void CDynamixelMotorGroup::stop(void)
{
if(this->mode==angle_ctrl)
{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],¤t_position);
+ 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],current_position);
+ 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],0);
+ this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_speed].address,0);
}
}
}catch(CDynamixelAlarmException &e){
@@ -1186,7 +1167,7 @@ void CDynamixelMotorGroup::get_current_angle(std::vector<double> &angles)
else
{
try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&data);
+ 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 */
@@ -1214,7 +1195,7 @@ void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds)
else
{
try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&data);
+ 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;
@@ -1244,8 +1225,8 @@ void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
else
{
try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_load],&c_effort);
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&c_speed);
+ 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;
@@ -1275,7 +1256,7 @@ void CDynamixelMotorGroup::get_current_voltage(std::vector<double> &voltages)
else
{
try{
- this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_voltage],&data);
+ 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 */
@@ -1303,7 +1284,7 @@ void CDynamixelMotorGroup::get_current_temperature(std::vector<double> &temperat
else
{
try{
- this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_temp],&data);
+ this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_temp].address,&data);
temperatures[i] = (double)data;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
@@ -1334,7 +1315,7 @@ void CDynamixelMotorGroup::get_punch(std::vector<unsigned short int> &punches)
else
{
try{
- this->dynamixel_dev[i]->read_word_register(this->registers[i][punch],&punches[i]);
+ 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])
@@ -1361,7 +1342,7 @@ void CDynamixelMotorGroup::set_punch(std::vector<unsigned short int> &punches)
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],punches[i]);
+ 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])
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index a79f87a36f3055413e3d3b72a0f3ebc791616a67..930b3e1c03ab6668563ff9bb943aae7b75106b2a 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -66,7 +66,7 @@ class CDynamixelMotorGroup
* \brief
*
*/
- std::vector<unsigned short int *>registers;
+ std::vector<TDynReg *>registers;
protected:
/**
* \brief
@@ -142,7 +142,7 @@ class CDynamixelMotorGroup
* \brief
*
*/
- void set_max_torque(unsigned int index,double torque_ratio);
+ void set_max_pwm(unsigned int index,double torque_ratio);
/**
* \brief
*
@@ -152,7 +152,7 @@ class CDynamixelMotorGroup
* \brief
*
*/
- double get_max_torque(unsigned int index);
+ double get_max_pwm(unsigned int index);
/**
* \brief
*
diff --git a/src/dynamixel_registers.cpp b/src/dynamixel_registers.cpp
index 5619600f6d3bac8832db5a677ca4ccde0a29aee0..71c94b5636d406ef4c18c174b27bcda05a13f02c 100644
--- a/src/dynamixel_registers.cpp
+++ b/src/dynamixel_registers.cpp
@@ -1,122 +1,278 @@
#include "dynamixel_registers.h"
-unsigned short int std_compl_reg[39]={std_model_number,
- std_firmware_version,
- std_id,
- std_baudrate,
- std_return_delay_time,
- std_cw_angle_limit,
- std_ccw_angle_limit,
- (unsigned short int)-1,
- std_temp_limit,
- std_low_voltage_limit,
- std_high_voltage_limit,
- std_max_torque,
- std_return_level,
- std_alarm_led,
- std_alarm_shtdwn,
- std_down_cal,
- std_up_cal,
- std_torque_en,
- std_led,
- (unsigned short int)-1,
- (unsigned short int)-1,
- (unsigned short int)-1,
- std_cw_comp_margin,
- std_ccw_comp_margin,
- std_cw_comp_slope,
- std_ccw_comp_slope,
- std_goal_pos,
- std_goal_speed,
- std_torque_limit,
- std_current_pos,
- std_current_speed,
- std_current_load,
- std_current_voltage,
- std_current_temp,
- std_reg_inst,
- (unsigned short int)-1,
- std_moving,
- std_lock,
- std_punch};
+TDynReg ax_reg[NUM_REG]={// Info
+ {0x0000,2,true},
+ {0xFFFF,0,false},
+ {0x0002,1,true},
+ {0x0003,1,true},
+ {0x0004,1,true},
+ {0x0005,1,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0010,1,true},
+ // Configuration
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0006,2,true},
+ {0x0008,2,true},
+ {0x000B,1,true},
+ {0x000C,1,true},
+ {0x000D,1,true},
+ {0x000E,2,true},
+ {0x0022,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ // Status
+ {0x0011,1,true},
+ {0x0012,1,true},
+ {0x0019,1,false},
+ {0xFFFF,0,false},
+ {0x002E,1,false},
+ {0xFFFF,0,false},
+ {0x002F,1,false},
+ {0x002C,1,false},
+ {0xFFFF,0,false},
+ // Control
+ {0x0018,1,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x001A,1,false},
+ {0x001B,1,false},
+ {0x001C,1,false},
+ {0x001D,1,false},
+ {0x0030,2,false},
+ // Set point
+ {0x001E,2,false},
+ {0x0020,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ // Feedback
+ {0x0024,2,false},
+ {0x0026,2,false},
+ {0x0028,2,false},
+ {0x002A,1,false},
+ {0x002B,1,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false}};
-unsigned short int std_pid_reg[39]={std_model_number,
- std_firmware_version,
- std_id,
- std_baudrate,
- std_return_delay_time,
- std_cw_angle_limit,
- std_ccw_angle_limit,
- (unsigned short int)-1,
- std_temp_limit,
- std_low_voltage_limit,
- std_high_voltage_limit,
- std_max_torque,
- std_return_level,
- std_alarm_led,
- std_alarm_shtdwn,
- std_down_cal,
- std_up_cal,
- std_torque_en,
- std_led,
- std_pid_p,
- std_pid_i,
- std_pid_d,
- (unsigned short int)-1,
- (unsigned short int)-1,
- (unsigned short int)-1,
- (unsigned short int)-1,
- std_goal_pos,
- std_goal_speed,
- std_torque_limit,
- std_current_pos,
- std_current_speed,
- std_current_load,
- std_current_voltage,
- std_current_temp,
- std_reg_inst,
- (unsigned short int)-1,
- std_moving,
- std_lock,
- std_punch};
+TDynReg mx_1_0_reg[NUM_REG]={// Info
+ {0x0000,2,true},
+ {0xFFFF,0,false},
+ {0x0002,1,true},
+ {0x0003,1,true},
+ {0x0004,1,true},
+ {0x0005,1,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0010,1,true},
+ // Configuration
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0006,2,true},
+ {0x0008,2,true},
+ {0x000B,1,true},
+ {0x000C,1,true},
+ {0x000D,1,true},
+ {0x000E,2,true},
+ {0x0022,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0014,2,true},
+ {0x0016,1,true},
+ // Status
+ {0x0011,1,true},
+ {0x0012,1,true},
+ {0x0019,1,false},
+ {0xFFFF,0,false},
+ {0x002E,1,false},
+ {0xFFFF,0,false},
+ {0x002F,1,false},
+ {0x002C,1,false},
+ {0xFFFF,0,false},
+ // Control
+ {0x0018,1,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x001C,1,false},
+ {0x001B,1,false},
+ {0x001A,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0030,2,false},
+ // Set point
+ {0x001E,2,false},
+ {0x0020,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0049,1,false},
+ {0xFFFF,0,false},
+ // Feedback
+ {0x0024,2,false},
+ {0x0026,2,false},
+ {0x0028,2,false},
+ {0x002A,1,false},
+ {0x002B,1,false},
+ {0xFFFF,0,false},
+ {0x0032,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false}};
-unsigned short int xl_reg[39]={xl_model_number,
- xl_firmware_version,
- xl_id,
- xl_baudrate,
- xl_return_delay_time,
- xl_cw_angle_limit,
- xl_ccw_angle_limit,
- xl_control_mode,
- xl_temp_limit,
- xl_low_voltage_limit,
- xl_high_voltage_limit,
- xl_max_torque,
- xl_return_level,
- xl_alarm_shtdwn,
- xl_alarm_shtdwn,
- xl_down_cal,
- xl_up_cal,
- xl_torque_en,
- xl_led,
- xl_pid_p,
- xl_pid_i,
- xl_pid_d,
- (unsigned short int)-1,
- (unsigned short int)-1,
- (unsigned short int)-1,
- (unsigned short int)-1,
- xl_goal_pos,
- xl_goal_speed,
- xl_goal_torque,
- xl_current_pos,
- xl_current_speed,
- xl_current_load,
- xl_current_voltage,
- xl_current_temp,
- xl_reg_inst,
- xl_moving,
- (unsigned short int)-1,
- xl_hardware_error,
- xl_punch};
+TDynReg mx_106_1_0_reg[NUM_REG]={// Info
+ {0x0000,2,true},
+ {0xFFFF,0,false},
+ {0x0002,1,true},
+ {0x0003,1,true},
+ {0x0004,1,true},
+ {0x0005,1,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0010,1,true},
+ // Configuration
+ {0x000A,1,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0006,2,true},
+ {0x0008,2,true},
+ {0x000B,1,true},
+ {0x000C,1,true},
+ {0x000D,1,true},
+ {0x000E,2,true},
+ {0x0022,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0014,2,true},
+ {0x0016,1,true},
+ // Status
+ {0x0011,1,true},
+ {0x0012,1,true},
+ {0x0019,1,false},
+ {0xFFFF,0,false},
+ {0x002E,1,false},
+ {0xFFFF,0,false},
+ {0x002F,1,false},
+ {0x002C,1,false},
+ {0xFFFF,0,false},
+ // Control
+ {0x0018,1,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x001C,1,false},
+ {0x001B,1,false},
+ {0x001A,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0030,2,false},
+ // Set point
+ {0x001E,2,false},
+ {0x0020,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0049,1,false},
+ {0xFFFF,0,false},
+ // Feedback
+ {0x0024,2,false},
+ {0x0026,2,false},
+ {0x0028,2,false},
+ {0x002A,1,false},
+ {0x002B,1,false},
+ {0xFFFF,0,false},
+ {0x0032,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false}};
+
+TDynReg xl_reg[NUM_REG]={// Info
+ {0x0000,2,true},
+ {0xFFFF,0,false},
+ {0x0002,1,true},
+ {0x0003,1,true},
+ {0x0004,1,true},
+ {0x0005,1,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0011,1,true},
+ // Configuration
+ {0xFFFF,0,false},
+ {0x000B,1,true},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0006,2,true},
+ {0x0008,2,true},
+ {0x000C,1,true},
+ {0x000D,1,true},
+ {0x000E,1,true},
+ {0x000F,2,true},
+ {0x0023,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ // Status
+ {0x0012,1,true},
+ {0x0012,1,true},
+ {0x0019,1,false},
+ {0xFFFF,0,false},
+ {0x0031,1,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x002F,1,false},
+ {0x0032,1,false},
+ // Control
+ {0x0018,1,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x001D,1,false},
+ {0x001C,1,false},
+ {0x001B,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0033,2,false},
+ // Set point
+ {0x001E,2,false},
+ {0x0020,2,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0x0049,1,false},
+ {0xFFFF,0,false},
+ // Feedback
+ {0x0025,2,false},
+ {0x0027,2,false},
+ {0x0029,2,false},
+ {0x002D,1,false},
+ {0x002E,1,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false},
+ {0xFFFF,0,false}};
diff --git a/src/dynamixel_registers.h b/src/dynamixel_registers.h
index 481a2eba9ec886e7ee0de37385830caa62627f5e..159356e9d9d32db28017ea0bccbc50116c6427af 100644
--- a/src/dynamixel_registers.h
+++ b/src/dynamixel_registers.h
@@ -1,137 +1,90 @@
#ifndef _DYNAMIXEL_REGISTERS_H
#define _DYNAMIXEL_REGISTERS_H
-extern unsigned short int std_compl_reg[39];
-extern unsigned short int std_pid_reg[39];
-extern unsigned short int xl_reg[39];
+#define NUM_REG 62
+
+typedef struct{
+ unsigned short int address;
+ unsigned char size;
+ bool eeprom;
+}TDynReg;
+
+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];
typedef enum {
// [Access] [Description]
- // EEPROM Area
- model_number=0, // (R) Model Number
+ // Info
+ model_number=0, // (R) Model Number
+ model_info, // (R)
firmware_version, // (R) Version of the Firmware
id, // (RW) ID of Dynamixel
baudrate, // (RW) Baud Rate of Dynamixel
return_delay_time, // (RW) Return Delay Time
- cw_angle_limit, // (RW) Clockwise Angle Limit
- ccw_angle_limit, // (RW) Counterclockwise Angle Limit
- dyn_control_mode, // (RW) Joint or wheel mode
- temp_limit, // (RW) Internal Temperature Limit
- low_voltage_limit, // (RW) Lowest Voltage Limit
- high_voltage_limit, // (RW) Highest Voltage Limit
- max_torque, // (RW) Maximum Torque
+ second_id, // (RW)
+ protocol, // (RW)
return_level, // (RW) Status Return Level
+ // Configuration
+ drive_mode, // (RW) Drive mode
+ op_mode, // (RW) Joint or wheel mode
+ homming_offset, // (RW)
+ moving_threshold, // (RW)
+ min_angle_limit, // (RW) Clockwise Angle Limit
+ max_angle_limit, // (RW) Counterclockwise Angle Limit
+ temp_limit, // (RW) Internal Temperature Limit
+ min_voltage_limit, // (RW) Lowest Voltage Limit
+ max_voltage_limit, // (RW) Highest Voltage Limit
+ pwm_limit, // (RW) Maximum Torque
+ max_pwm, // (RW) Maximum Torque
+ current_limit, // (RW)
+ max_current, // (RW)
+ velocity_limit, // (RW)
+ multi_turn_off, // (RW) Multi turn offset position
+ resolution_div, // (RW) Resolution divider
+ // Status
alarm_led, // (RW) LED for Alarm
alarm_shtdwn, // (RW) Shutdown for Alarm
- down_cal, // (RW)
- up_cal, // (RW)
- // RAM Area
- torque_en, // (RW) Torque On/Off
led, // (RW) LED On/Off
- pid_p, // (RW)
- pid_i, // (RW)
- pid_d, // (RW)
+ bus_watchdog, // (RW)
+ moving, // (R) Means if there is any movement
+ moving_status, // (R)
+ lock, // (RW) Locking EEPROM
+ reg_inst, // (RW) Means if Instruction is registered
+ hardware_error, // (R) Means if there is any movement
+ // control
+ torque_en, // (RW) Torque On/Off
+ vel_pid_i, // (RW)
+ vel_pid_p, // (RW)
+ pos_pid_p, // (RW)
+ pos_pid_i, // (RW)
+ pos_pid_d, // (RW)
+ feedfwd_gain_2, // (RW)
+ feedfwd_gain_1, // (RW)
cw_comp_margin, // (RW) CW Compliance Margin
ccw_comp_margin, // (RW) CCW Compliance Margin
cw_comp_slope, // (RW) CW Compliance Slope
ccw_comp_slope, // (RW) CCW Compliance Slope
+ punch, // (RW) Punch
+ // Set point
goal_pos, // (RW) Goal Position
goal_speed, // (RW) Moving Speed
- torque_limit, // (RW) Torque Limit
+ goal_pwn, // (RW)
+ goal_current, // (RW)
+ profile_accel, // (RW)
+ profile_vel, // (RW)
+ // Feedback
current_pos, // (R) Current Position
current_speed, // (R) Current Speed
current_load, // (R) Current Load
current_voltage, // (R) Current Voltage
- current_temp, // (RW) Current Temperature
- reg_inst, // (RW) Means if Instruction is registered
- moving, // (R) Means if there is any movement
- lock, // (RW) Locking EEPROM
- hardware_error, // (R) Means if there is any movement
- punch // (RW) Punch
+ current_temp, // (R) Current Temperature
+ current_pwm, // (R)
+ realtime_tick, // (R) Realtime tick
+ velocity_traj, // (R)
+ pos_traj // (R)
} reg_id;
-typedef enum {
- // [Access] [Description]
- // EEPROM Area
- std_model_number=0x00, // (R) Model Number
- std_firmware_version=0x02, // (R) Version of the Firmware
- std_id=0x03, // (RW) ID of Dynamixel
- std_baudrate=0x04, // (RW) Baud Rate of Dynamixel
- std_return_delay_time=0x05, // (RW) Return Delay Time
- std_cw_angle_limit=0x06, // (RW) Clockwise Angle Limit
- std_ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit
- std_temp_limit=0x0B, // (RW) Internal Temperature Limit
- std_low_voltage_limit=0x0C, // (RW) Lowest Voltage Limit
- std_high_voltage_limit=0x0D, // (RW) Highest Voltage Limit
- std_max_torque=0x0E, // (RW) Maximum Torque
- std_return_level=0x10, // (RW) Status Return Level
- std_alarm_led=0x11, // (RW) LED for Alarm
- std_alarm_shtdwn=0x12, // (RW) Shutdown for Alarm
- std_down_cal=0x14, // (RW)
- std_up_cal=0x16, // (RW)
- // RAM Area
- std_torque_en=0x18, // (RW) Torque On/Off
- std_led=0x19, // (RW) LED On/Off
- std_pid_p=0x1A, // (RW)
- std_pid_i=0x1B, // (RW)
- std_pid_d=0x1C, // (RW)
- std_cw_comp_margin=0x1A, // (RW) CW Compliance Margin
- std_ccw_comp_margin=0x1B, // (RW) CCW Compliance Margin
- std_cw_comp_slope=0x1C, // (RW) CW Compliance Slope
- std_ccw_comp_slope=0x1D, // (RW) CCW Compliance Slope
- std_goal_pos=0x1E, // (RW) Goal Position
- std_goal_speed=0x20, // (RW) Moving Speed
- std_torque_limit=0x22, // (RW) Torque Limit
- std_current_pos=0x24, // (R) Current Position
- std_current_speed=0x26, // (R) Current Speed
- std_current_load=0x28, // (R) Current Load
- std_current_voltage=0x2A, // (R) Current Voltage
- std_current_temp=0x2B, // (RW) Current Temperature
- std_reg_inst=0x2C, // (RW) Means if Instruction is registered
- std_moving=0x2E, // (R) Means if there is any movement
- std_lock=0x2F, // (RW) Locking EEPROM
- std_punch=0x30 // (RW) Punch
-} std_reg_id;
-
-typedef enum {
- // [Access] [Description]
- // EEPROM Area
- xl_model_number=0x00, // (R) Model Number
- xl_firmware_version=0x02, // (R) Version of the Firmware
- xl_id=0x03, // (RW) ID of Dynamixel
- xl_baudrate=0x04, // (RW) Baud Rate of Dynamixel
- xl_return_delay_time=0x05, // (RW) Return Delay Time
- xl_cw_angle_limit=0x06, // (RW) Clockwise Angle Limit
- xl_ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit
- xl_control_mode=0x0B, // (RW) Joint or wheel mode
- xl_temp_limit=0x0C, // (RW) Internal Temperature Limit
- xl_low_voltage_limit=0x0D, // (RW) Lowest Voltage Limit
- xl_high_voltage_limit=0x0E, // (RW) Highest Voltage Limit
- xl_max_torque=0x0F, // (RW) Maximum Torque
- xl_return_level=0x11, // (RW) Status Return Level
- xl_alarm_shtdwn=0x12, // (RW) Shutdown for Alarm
- xl_down_cal=0x14, // (RW)
- xl_up_cal=0x16, // (RW)
- // RAM Area
- xl_torque_en=0x18, // (RW) Torque On/Off
- xl_led=0x19, // (RW) LED On/Off
- xl_pid_d=0x1B, // (RW)
- xl_pid_i=0x1C, // (RW)
- xl_pid_p=0x1D, // (RW)
- xl_goal_pos=0x1E, // (RW) Goal Position
- xl_goal_speed=0x20, // (RW) Moving Speed
- xl_goal_torque=0x23, // (RW) Torque Limit
- xl_current_pos=0x25, // (R) Current Position
- xl_current_speed=0x27, // (R) Current Speed
- xl_current_load=0x29, // (R) Current Load
- xl_current_voltage=0x2D, // (R) Current Voltage
- xl_current_temp=0x2E, // (RW) Current Temperature
- xl_reg_inst=0x2F, // (RW) Means if Instruction is registered
- xl_moving=0x31, // (R) Means if there is any movement
- xl_hardware_error=0x32, // (R) Means if there is any movement
- xl_punch=0x33 // (RW) Punch
-} xl_reg_id;
-
-
#endif
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index 533af63ce4189d29abb467978b3050c0cc03aeb0..58914a15e87d6667202808c0102d274cc5e73e7a 100644
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -11,8 +11,8 @@ std::string config_file="../src/xml/dyn_servo_config.xml";
int main(int argc, char *argv[])
{
- std::string serial="A600cVjj"; //extracted from dynamixel library's test_dynamixel_server_no_scan or with 'dmesg' command: "SerialNumber: A600cVjj"
- int baudrate = 57600; //57600 or 1000000
+ 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
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
@@ -34,7 +34,7 @@ int main(int argc, char *argv[])
/////////////////////////////////////////////////////////////////////////
////INFO
/////////////////////////////////////////////////////////////////////////
- /*
+/*
std::cout << "-----------------------------------------------------------" << std::endl;
cont->get_servo_info(info);
std::cout << "[INFO]: Servo model: " << info.model << std::endl;
@@ -76,13 +76,13 @@ 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_torque: " << std::dec << cont->get_max_torque() << std::endl;
- std::cout << "[INFO]: limit_torque: " << std::dec << cont->get_limit_torque() << 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;
cont->turn_led_on();
sleep(1);
cont->turn_led_off();
- //*/
+*/
double desired_speed = 100.0; //chosen speed when moving angle
@@ -94,13 +94,13 @@ int main(int argc, char *argv[])
int t;
double uperiod = time_interval*1000000.0;
double timeout = max_time_sec/(uperiod/1000000.0);
-
+
/////////////////////////////////////////////////////////////////////////
/////MOVE RELATIVE ANGLE
/////////////////////////////////////////////////////////////////////////
- /*
+/*
std::cout << "-----------------------------------------------------------" << std::endl;
double relative_angle = -45;
double current_rel_angle;
@@ -130,12 +130,12 @@ int main(int argc, char *argv[])
std::cout << "Error angle: " << current_rel_angle-desired_angle << std::endl;
std::cout << "Done" << std::endl;
sleep(1);
- //*/
+*/
/////////////////////////////////////////////////////////////////////////
////MOVE ABSOLUTE ANGLE
/////////////////////////////////////////////////////////////////////////
- /*
+/*
std::cout << "-----------------------------------------------------------" << std::endl;
double absolute_angle=0.0;
double current_abs_angle;
@@ -165,12 +165,11 @@ int main(int argc, char *argv[])
std::cout << "Error angle: " << current_abs_angle-desired_angle << std::endl;
std::cout << "Done" << std::endl;
sleep(1);
- //*/
-
+*/
/////////////////////////////////////////////////////////////////////////
////MOVE TORQUE
/////////////////////////////////////////////////////////////////////////
- /*
+/*
std::cout << "-----------------------------------------------------------" << std::endl;
double max_effort;
if(argc==2)
@@ -215,13 +214,14 @@ int main(int argc, char *argv[])
std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
- //*/
+*/
/////////////////////////////////////////////////////////////////////////
////MOVE RANDOM TORQUES AND OUTPUT SPEEDS
/////////////////////////////////////////////////////////////////////////
///*
+
std::cout << "MOVE RANDOM EFFORTS" << std::endl;
int min=-100;
int max=100;
@@ -250,7 +250,6 @@ int main(int argc, char *argv[])
std::cout << "Done" << std::endl;
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
- //*/
diff --git a/src/xml/dynamixel_motor_cfg_file.xsd b/src/xml/dynamixel_motor_cfg_file.xsd
index d813b05bce3f0162a932da2ec32705ae08f1229b..7ae1430be0b9d2c3cd5259edbba04d0359fceb01 100644
--- a/src/xml/dynamixel_motor_cfg_file.xsd
+++ b/src/xml/dynamixel_motor_cfg_file.xsd
@@ -36,7 +36,7 @@ copyright : not copyrighted - public domain
</xsd:element>
<xsd:element name="min_voltage" type="xsd:float">
</xsd:element>
- <xsd:element name="max_torque" type="xsd:float">
+ <xsd:element name="max_pwm" type="xsd:float">
</xsd:element>
<xsd:element name="cw_comp_margin" type="xsd:unsignedByte">
</xsd:element>