diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index c729a8d8a6a4e538c92e055670e9381bfc76e87b..d67ac1f5a9b95a03daa1165a53ed8ba1cebe725c 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,28 +21,32 @@ 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;
+  this->info.max_current=-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;
   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;
-  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 +55,12 @@ 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_pwm_limit();
+    try{
+      this->config.max_current=this->get_current_limit();
+    }catch(CDynamixelMotorUnsupportedFeatureException &e){
+      std::cout << "No current limit feature" << std::endl;
+    }
     this->get_compliance_control(this->compliance);
     this->get_pid_control(this->pid);
     this->alarms=this->get_turn_off_alarms();
@@ -80,12 +88,15 @@ unsigned int CDynamixelMotor::from_speeds(double speed)
 
   if(speed>this->info.max_speed)
     speed=this->info.max_speed;
-  counts=fabs(speed)/0.666;
+  if(this->info.ext_memory_map)
+    counts=speed/0.229;
+  else
+    counts=fabs(speed)/0.666;
 
   return counts;
 }
 
-double CDynamixelMotor::to_angles(unsigned short int counts)
+double CDynamixelMotor::to_angles(unsigned int counts)
 {
   double angle;
 
@@ -94,42 +105,45 @@ double CDynamixelMotor::to_angles(unsigned short int counts)
   return angle;
 }
 
-double CDynamixelMotor::to_speeds(unsigned short int counts)
+double CDynamixelMotor::to_speeds(unsigned int counts)
 {
   double speed;
 
-  speed = counts*0.666; // Taken from the datasheets (sfoix)
+  if(this->info.ext_memory_map)
+    speed=counts*0.229;
+  else
+    speed=counts*0.666; // Taken from the datasheets (sfoix)
 
   return speed;
 }
 
 void CDynamixelMotor::reset_motor(void)
 {
-  unsigned short int current_position;
-  unsigned short int maximum_torque;
+  unsigned int current_position;
+  unsigned int maximum_pwm;
 
   try{
-    this->dynamixel_dev->read_word_register(this->registers[current_pos],&current_position);
+    this->read_register(this->registers[current_pos],current_position);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
+    this->write_register(this->registers[goal_pos],current_position);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->read_word_register(this->registers[max_torque],&maximum_torque);
+    this->read_register(this->registers[max_pwm],maximum_pwm);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->write_word_register(this->registers[torque_limit],maximum_torque);
+    this->write_register(this->registers[pwm_limit],maximum_pwm);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->write_byte_register(this->registers[led],0x00);
+    this->write_register(this->registers[led],0);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
@@ -138,6 +152,7 @@ void CDynamixelMotor::reset_motor(void)
 void CDynamixelMotor::get_model(void)
 {
   unsigned short int model;
+  unsigned int value;
 
   if(this->dynamixel_dev==NULL)
   {
@@ -147,108 +162,128 @@ 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_word_register(0x0000,&model);
       switch(model)
       {
 	case 0x000C: this->info.model="AX-12A";
 		     this->info.max_angle=300.0;
 		     this->info.center_angle=150.0;
 		     this->info.max_speed=354;
+                     this->info.max_current=0.0;
 		     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;
+                     this->info.ext_memory_map=false;
 		     break;
 	case 0x012C: this->info.model="AX-12W";
 		     this->info.max_angle=300.0;
 		     this->info.center_angle=150.0;
 		     this->info.max_speed=2830;
+                     this->info.max_current=0.0;
 		     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;
+                     this->info.ext_memory_map=false;
 		     break;
 	case 0x0012: this->info.model="AX-18A";
 		     this->info.max_angle=300.0;
 		     this->info.center_angle=150.0;
 		     this->info.max_speed=582;
+                     this->info.max_current=0.0;
 		     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;
+                     this->info.ext_memory_map=false;
 		     break;
-	case 0x001D: this->info.model="MX-28";
-		     this->info.max_angle=360.0;
-		     this->info.center_angle=180.0;
-		     this->info.max_speed=330;
-		     this->info.encoder_resolution=4095;
-		     this->info.gear_ratio=193;
-                     this->registers=std_pid_reg;
-		     this->info.pid_control=true;
-		     break;
-	case 0x0018: this->info.model="RX-24F";
+	case 0x001C: this->info.model="RX-28";
 		     this->info.max_angle=300.0;
 		     this->info.center_angle=150.0;
-		     this->info.max_speed=756;
+		     this->info.max_speed=510;
+                     this->info.max_current=0.0;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=193;
-                     this->registers=std_compl_reg;
+                     this->registers=ax_reg;
 		     this->info.pid_control=false;
+                     this->info.multi_turn=false;
+                     this->info.ext_memory_map=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;
+	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.max_current=0.0;
+		     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;
+                     this->info.ext_memory_map=false;
+		     break;
+	case 0x001D: this->info.model="MX-28";
+		     this->info.max_angle=360.0;
+		     this->info.center_angle=180.0;
+		     this->info.max_speed=330;
+                     this->info.max_current=0.0;
+		     this->info.encoder_resolution=4095;
 		     this->info.gear_ratio=193;
-                     this->registers=std_compl_reg;
-		     this->info.pid_control=false;
+                     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;
 		     this->info.center_angle=180.0;
 		     this->info.max_speed=378;
+                     this->info.max_current=0.0;
 		     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;
+                     this->info.ext_memory_map=false;
 		     break;
 	case 0x0140: this->info.model="MX-106";
 		     this->info.max_angle=360.0;
 		     this->info.center_angle=180.0;
 		     this->info.max_speed=270;
+                     this->info.max_current=0.0;
 		     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;
+                     this->info.ext_memory_map=false;
 		     break;
 	case 0x015E: this->info.model="XL_320";
 		     this->info.max_angle=300.0;
 		     this->info.center_angle=150;
 		     this->info.max_speed=684;
+                     this->info.max_current=0.0;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=238;
                      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.max_current=2.3;
+		     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;
@@ -259,11 +294,57 @@ void CDynamixelMotor::get_model(void)
 	this->reset_motor();
       throw;
     }
+    this->read_register(this->registers[firmware_version],value);
+    this->info.firmware_ver=value;
   }
 }
 
 void CDynamixelMotor::set_control_mode(control_mode mode)
 {
+  unsigned int value;
+  bool was_enabled=this->enabled;
+
+  if(this->mode!=mode)
+  {
+    if(this->enabled)
+      this->disable();
+    if(this->info.ext_memory_map)
+    {
+      value=mode;
+      this->write_register(this->registers[op_mode],value);
+    }
+    else
+    {
+      if(this->info.model=="XL_320")
+      {
+        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
+      {
+        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();
+  }
+}
+
+void CDynamixelMotor::read_register(TDynReg reg, unsigned int &value)
+{
+  unsigned char reg_value[4]={0};
+
   if(this->dynamixel_dev==NULL)
   {
     /* handle exceptions */
@@ -271,37 +352,72 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
   }
   else
   {
-    try{
-      if(this->mode!=mode)
-      {
-        this->disable();
-        if(this->info.model=="XL_320")
-        {
-          this->dynamixel_dev->write_byte_register(this->registers[dyn_control_mode],(unsigned char)mode);
-          usleep(10000);
-          this->mode=mode;
-        }
+    if(reg.address==0xFFFF)
+    {
+      /* handle exceptions */
+      throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"The desired feature is not supported by this servo model.");
+    }
+    else
+    {
+      try{
+        this->dynamixel_dev->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
         {
-          if(mode==angle_ctrl)
-          {
-            this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
-            this->mode=angle_ctrl;
-          }
-          else
-          {
-            this->set_position_range(-this->info.center_angle,-this->info.center_angle);
-            this->mode=torque_ctrl;
-          }
+          /* handle exceptions */
+          throw CDynamixelMotorException(_HERE_,"Invalid register size.");
         }
+      }catch(CDynamixelAlarmException &e){
+        /* handle dynamixel exception */
+        if(e.get_alarm()&this->alarms)
+          this->reset_motor();
+        throw;
       }
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-        this->reset_motor();
-      throw;
     }
-  }  
+  }
+}
+
+void CDynamixelMotor::write_register(TDynReg reg, unsigned int value)
+{
+  if(this->dynamixel_dev==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->write_registers(reg.address,(unsigned char *)&value,reg.size);
+        }catch(CDynamixelAlarmException &e){
+          /* handle dynamixel exception */
+          if(e.get_alarm()&this->alarms)
+            this->reset_motor();
+          throw;
+        }
+      }
+      if(reg.eeprom)
+        usleep(10000);
+    }
+  }
 }
 
 void CDynamixelMotor::load_config(TDynamixel_config &config)
@@ -309,8 +425,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 +445,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 +499,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 +533,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,
@@ -445,105 +561,62 @@ 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)
 {
-  unsigned short int max_pos,min_pos;
+  unsigned int max_pos,min_pos;
 
-  if(this->dynamixel_dev==NULL)
+  max_pos=this->from_angles(max);
+  min_pos=this->from_angles(min);
+  if(min_pos<0.0 || max_pos>(this->info.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(max);
-    min_pos=this->from_angles(min);
-    if(min_pos<0.0 || max_pos>(this->info.encoder_resolution))
-    {
-      /* handle exceptions */
-      throw CDynamixelMotorException(_HERE_,"The desired range is outside the valid range of the servo.");
-    }
+    this->config.max_angle=max;
+    this->config.min_angle=min;
+    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=pwm_ctrl;
     else
-    {
-      try{
-	this->config.max_angle=max;
-	this->config.min_angle=min;
-	this->dynamixel_dev->write_word_register(this->registers[ccw_angle_limit],max_pos);
-        usleep(10000);
-	this->dynamixel_dev->write_word_register(this->registers[cw_angle_limit],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;
-        else
-          this->mode=angle_ctrl;
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
+      this->mode=position_ctrl;
   }
 }
 
 void CDynamixelMotor::get_position_range(double *min, double *max)
 {
-  unsigned short int angle_limit;
+  unsigned int angle_limit;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_word_register(this->registers[ccw_angle_limit],&angle_limit);
-      (*max)=this->to_angles(angle_limit);
-      this->dynamixel_dev->read_word_register(this->registers[cw_angle_limit],&angle_limit);
-      (*min)=this->to_angles(angle_limit);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[max_angle_limit],angle_limit);
+  (*max)=this->to_angles(angle_limit);
+  this->read_register(this->registers[min_angle_limit],angle_limit);
+  (*min)=this->to_angles(angle_limit);
 }
 
 int CDynamixelMotor::get_temperature_limit(void)
 {
-  unsigned char limit;
+  unsigned int limit;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_byte_register(this->registers[temp_limit],&limit);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[temp_limit],limit);
 
   return limit;
 }
 
 void CDynamixelMotor::set_temperature_limit(int limit)
 {
+  unsigned int value;
+
   if(limit<0 && limit>255)
   {
     /* handle exceptions */
@@ -551,123 +624,63 @@ void CDynamixelMotor::set_temperature_limit(int limit)
   }
   else
   {
-    if(this->dynamixel_dev==NULL)
-    {
-      /* handle exceptions */
-      throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-    }
-    else
-    {
-      try{
-	this->config.max_temperature=limit;
-	this->dynamixel_dev->write_byte_register(this->registers[temp_limit],(unsigned char)limit);
-        usleep(10000);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
+    this->config.max_temperature=limit;
+    value=limit;
+    this->write_register(this->registers[temp_limit],value);
   }
 }
 
 void CDynamixelMotor::get_voltage_limits(double *min, double *max)
 {
-  unsigned char min_voltage,max_voltage;
+  unsigned int value;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  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);
-      *min=((double)min_voltage/10.0);
-      *max=((double)max_voltage/10.0);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[min_voltage_limit],value);
+  *min=((double)value/10.0);
+  this->read_register(this->registers[max_voltage_limit],value);
+  *max=((double)value/10.0);
 }
 
 void CDynamixelMotor::set_voltage_limits(double min, double max)
 {
-  unsigned char min_voltage,max_voltage;
+  unsigned int value;
 
-  if(this->dynamixel_dev==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>30.0 || max<5.0 || max>30.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.min_voltage=min;
-	  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);
-          usleep(10000);
-	  this->dynamixel_dev->write_byte_register(this->registers[high_voltage_limit],max_voltage);
-          usleep(10000);
-	}catch(CDynamixelAlarmException &e){
-	  /* handle dynamixel exception */
-	  if(e.get_alarm()&this->alarms)
-	    this->reset_motor();
-	  throw;
-	}
-      }
+      this->config.min_voltage=min;
+      this->config.max_voltage=max;
+      value=min*10;
+      this->write_register(this->registers[min_voltage_limit],value);
+      value=max*10;
+      this->write_register(this->registers[max_voltage_limit],value);
     }
   }
 }
 
 unsigned char CDynamixelMotor::get_led_alarms(void)
 {
-  unsigned char led_alarms=0x00;
+  unsigned int led_alarms;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_byte_register(this->registers[alarm_led],&led_alarms);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[alarm_led],led_alarms);
 
   return led_alarms;
 }
 
 void CDynamixelMotor::set_led_alarms(unsigned char alarms)
 {
+  unsigned int value;
+
   if(alarms&0x80)
   {
     /* handle exceptions */
@@ -675,51 +688,24 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms)
   }
   else
   {
-    if(this->dynamixel_dev==NULL)
-    {
-      /* handle exceptions */
-      throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-    }
-    else
-    {
-      try{
-	this->dynamixel_dev->write_byte_register(this->registers[alarm_led],(unsigned char)alarms);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
+    value=alarms;
+    this->write_register(this->registers[alarm_led],value);
   }
 }
 
 unsigned char CDynamixelMotor::get_turn_off_alarms(void)
 {
-  unsigned char shutdown_alarms=0x00;
+  unsigned int shutdown_alarms;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_byte_register(this->registers[alarm_shtdwn], &shutdown_alarms);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }  
+  this->read_register(this->registers[alarm_shtdwn],shutdown_alarms);
 
   return shutdown_alarms;
 }
 
 void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
 {
+  unsigned int value;
+
   if(alarms&0x80)
   {
     /* handle exceptions */
@@ -727,319 +713,249 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
   }
   else
   {
-    if(this->dynamixel_dev==NULL)
-    {
-      /* handle exceptions */
-      throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-    }
-    else
-    {
-      try{
-	this->alarms=alarms;
-	this->dynamixel_dev->write_byte_register(this->registers[alarm_shtdwn],(unsigned char)alarms);
-        usleep(10000);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
+    this->alarms=alarms;
+    value=alarms;
+    this->write_register(this->registers[alarm_shtdwn],value);
   }
 }
 
-double CDynamixelMotor::get_max_torque(void)
+double CDynamixelMotor::get_max_pwm(void)
 {
-  unsigned short int load;
+  unsigned int load;
   double torque;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
+  this->read_register(this->registers[max_pwm],load);
+  torque=(load&0x3FF)*100.0/1023.0;
+  if(load>0x3FF)
+    torque=-1*torque;
+
+  return torque;
+}
+
+double CDynamixelMotor::get_pwm_limit(void)
+{
+  unsigned int load;
+  double torque;
+
+  this->read_register(this->registers[pwm_limit],load);
+  if(this->info.ext_memory_map)
+    torque=((signed short int)load)*100.0/885.0;
   else
   {
-    try{
-      this->dynamixel_dev->read_word_register(this->registers[max_torque],&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)
-	this->reset_motor();
-      throw;
-    }
+    torque=(load&0x3FF)*100.0/1023.0;
+    if(load>0x3FF)
+      torque=-1*torque;
   }
 
   return torque;
 }
 
-double CDynamixelMotor::get_limit_torque(void)
+void CDynamixelMotor::set_max_pwm(double torque_ratio)
 {
-  unsigned short int load;
-  double torque;
+  unsigned int load;
 
-  if(this->dynamixel_dev==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
   {
-    try{
-      this->dynamixel_dev->read_word_register(this->registers[torque_limit],&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)
-	this->reset_motor();
-      throw;
-    }
+    load=torque_ratio*1023.0/100.0;
+    this->config.max_pwm=torque_ratio;
+    this->write_register(this->registers[max_pwm],load);
   }
-
-  return torque;
 }
 
-void CDynamixelMotor::set_max_torque(double torque_ratio)
+void CDynamixelMotor::set_pwm_limit(double torque_ratio)
 {
-  unsigned short int load;
+  unsigned int load;
 
-  if(this->dynamixel_dev==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.");
-    }
+    if(this->info.ext_memory_map)
+      load=torque_ratio*885.0/100.0;
     else
-    {
-      load=torque_ratio*1023/100.0;
-      try{
-	this->config.max_torque=torque_ratio;
-	this->dynamixel_dev->write_word_register(this->registers[max_torque],load);
-        usleep(10000);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
+      load=torque_ratio*1023.0/100.0;
+    this->write_register(this->registers[pwm_limit],load);
   }
 }
 
-void CDynamixelMotor::set_limit_torque(double torque_ratio)
+double CDynamixelMotor::get_current_limit(void)
 {
-  unsigned short int load;
+  double current;
+  unsigned int data;
+  
+  this->read_register(this->registers[current_limit],data);
+  current=((signed short int)data)*2.69/1000.0;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  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->write_word_register(this->registers[torque_limit],load);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
-  }
+  return current;
+}
+
+void CDynamixelMotor::set_current_limit(double current)
+{
+  unsigned int data;
+
+  data=(signed short int)(current*1000.0/2.69);
+  this->write_register(this->registers[current_limit],data);
+}
+
+double CDynamixelMotor::get_speed_limit(void)
+{
+  unsigned int data;
+  double speed;
+
+  this->read_register(this->registers[velocity_limit],data);
+  speed=this->to_speeds(data);
+
+  return speed;
+}
+
+void CDynamixelMotor::set_speed_limit(double speed)
+{
+  unsigned int data;
+
+  data=this->from_speeds(speed);
+  this->write_register(this->registers[velocity_limit],data);
 }
 
+
 void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
+  unsigned int value;
+
+  if(!this->info.pid_control)
   {
-    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);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
+    this->read_register(this->registers[cw_comp_margin],value);
+    config.cw_compliance_margin=value;
+    this->read_register(this->registers[ccw_comp_margin],value);
+    config.ccw_compliance_margin=value;
+    this->read_register(this->registers[cw_comp_slope],value);
+    config.cw_compliance_slope=value;
+    this->read_register(this->registers[ccw_comp_slope],value);
+    config.ccw_compliance_slope=value;
   }
 }
 
 void CDynamixelMotor::set_compliance_control(TDynamixel_compliance &config)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
+  unsigned int value;
+
+  if(!this->info.pid_control)
   {
-    if(!this->info.pid_control)
+    if(config.cw_compliance_margin>254)
     {
-      if(config.cw_compliance_margin>254)
-      {
-	/* handle exceptions */
-	throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance margin.");
-      }
-      if(config.ccw_compliance_margin>254)
-      {
-	/*  handle exceptions */
-	throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
-      }
-      if(config.cw_compliance_slope>=1 && config.cw_compliance_slope<=5) config.cw_compliance_slope=4;
-      else if(config.cw_compliance_slope>=6 && config.cw_compliance_slope<=11) config.cw_compliance_slope=8;
-      else if(config.cw_compliance_slope>=12 && config.cw_compliance_slope<=23) config.cw_compliance_slope=16;
-      else if(config.cw_compliance_slope>=24 && config.cw_compliance_slope<=47) config.cw_compliance_slope=32;
-      else if(config.cw_compliance_slope>=48 && config.cw_compliance_slope<=95) config.cw_compliance_slope=64;
-      else if(config.cw_compliance_slope>=96 && config.cw_compliance_slope<=191) config.cw_compliance_slope=128;
-      else if(config.cw_compliance_slope>=192 && config.cw_compliance_slope<=254) config.cw_compliance_slope=254;
-      else
-      {
-	/* handle exceptions */
-	throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance slope.");
-      }
-      if(config.ccw_compliance_slope>=1 && config.ccw_compliance_slope<=5) config.ccw_compliance_slope=4;
-      else if(config.ccw_compliance_slope>=6 && config.ccw_compliance_slope<=11) config.ccw_compliance_slope=8;
-      else if(config.ccw_compliance_slope>=12 && config.ccw_compliance_slope<=23) config.ccw_compliance_slope=16;
-      else if(config.ccw_compliance_slope>=24 && config.ccw_compliance_slope<=47) config.ccw_compliance_slope=32;
-      else if(config.ccw_compliance_slope>=48 && config.ccw_compliance_slope<=95) config.ccw_compliance_slope=64;
-      else if(config.ccw_compliance_slope>=96 && config.ccw_compliance_slope<=191) config.ccw_compliance_slope=128;
-      else if(config.ccw_compliance_slope>=192 && config.ccw_compliance_slope<=254) config.ccw_compliance_slope=254;
-      else
-      {
-	/* handle exceptions */
-	throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
-      }
-      try{
-	this->compliance.cw_compliance_margin=config.cw_compliance_margin;
-	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);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
+      /* handle exceptions */
+      throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance margin.");
+    }  
+    if(config.ccw_compliance_margin>254)
+    {
+      /*  handle exceptions */
+      throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
+    }
+    if(config.cw_compliance_slope>=1 && config.cw_compliance_slope<=5) config.cw_compliance_slope=4;
+    else if(config.cw_compliance_slope>=6 && config.cw_compliance_slope<=11) config.cw_compliance_slope=8;
+    else if(config.cw_compliance_slope>=12 && config.cw_compliance_slope<=23) config.cw_compliance_slope=16;
+    else if(config.cw_compliance_slope>=24 && config.cw_compliance_slope<=47) config.cw_compliance_slope=32;
+    else if(config.cw_compliance_slope>=48 && config.cw_compliance_slope<=95) config.cw_compliance_slope=64;
+    else if(config.cw_compliance_slope>=96 && config.cw_compliance_slope<=191) config.cw_compliance_slope=128;
+    else if(config.cw_compliance_slope>=192 && config.cw_compliance_slope<=254) config.cw_compliance_slope=254;
+    else
+    {
+      /* handle exceptions */
+      throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance slope.");
+    }
+    if(config.ccw_compliance_slope>=1 && config.ccw_compliance_slope<=5) config.ccw_compliance_slope=4;
+    else if(config.ccw_compliance_slope>=6 && config.ccw_compliance_slope<=11) config.ccw_compliance_slope=8;
+    else if(config.ccw_compliance_slope>=12 && config.ccw_compliance_slope<=23) config.ccw_compliance_slope=16;
+    else if(config.ccw_compliance_slope>=24 && config.ccw_compliance_slope<=47) config.ccw_compliance_slope=32;
+    else if(config.ccw_compliance_slope>=48 && config.ccw_compliance_slope<=95) config.ccw_compliance_slope=64;
+    else if(config.ccw_compliance_slope>=96 && config.ccw_compliance_slope<=191) config.ccw_compliance_slope=128;
+    else if(config.ccw_compliance_slope>=192 && config.ccw_compliance_slope<=254) config.ccw_compliance_slope=254;
+    else
+    {
+      /* handle exceptions */
+      throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
     }
+    this->compliance.cw_compliance_margin=config.cw_compliance_margin;
+    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;
+    value=config.cw_compliance_margin;
+    this->write_register(this->registers[cw_comp_margin],value);
+    value=config.ccw_compliance_margin;
+    this->write_register(this->registers[ccw_comp_margin],value);
+    value=config.cw_compliance_slope;
+    this->write_register(this->registers[cw_comp_slope],value);
+    value=config.ccw_compliance_slope;
+    this->write_register(this->registers[ccw_comp_slope],value);
   }
 }
 
 void CDynamixelMotor::get_pid_control(TDynamixel_pid &config)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
+  unsigned int value;
+
+  if(this->info.pid_control)
   {
-    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);
-      }catch(CDynamixelAlarmException &e){
-	/* handle dynamixel exception */
-	if(e.get_alarm()&this->alarms)
-	  this->reset_motor();
-	throw;
-      }
-    }
+    this->read_register(this->registers[pos_pid_p],value);
+    config.p=value;
+    this->read_register(this->registers[pos_pid_i],value);
+    config.i=value;
+    this->read_register(this->registers[pos_pid_d],value);
+    config.d=value;
   }
 }
 
 void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
+  unsigned int value;
+
+  if(this->info.pid_control)
   {
-    if(this->info.pid_control)
-    {
-      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->pid.p=config.p;
+    this->pid.i=config.i;
+    this->pid.d=config.d;
+    value=config.p;
+    this->write_register(this->registers[pos_pid_p],value);
+    value=config.i;
+    this->write_register(this->registers[pos_pid_i],value);
+    value=config.d;
+    this->write_register(this->registers[pos_pid_d],value);
   }
 }
 
+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)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->write_byte_register(this->registers[led],1);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->write_register(this->registers[led],1);
 }
 
 void CDynamixelMotor::turn_led_off(void)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->write_byte_register(this->registers[led],0);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->write_register(this->registers[led],0);
 }
 
 bool CDynamixelMotor::is_locked(void)
@@ -1054,382 +970,289 @@ void CDynamixelMotor::lock(void)
 
 void CDynamixelMotor::enable(void)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->write_byte_register(this->registers[torque_en],1);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->write_register(this->registers[torque_en],1);
+  this->enabled=true;
 }
 
 void CDynamixelMotor::disable(void)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->write_byte_register(this->registers[torque_en],0);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  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 short int cmd[2];
-
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+  unsigned int data;
+
+  if(current==std::numeric_limits<double>::infinity())
+  {
+    this->set_control_mode(position_ctrl);
+    if(speed>this->info.max_speed)
+      speed=this->info.max_speed;
+    data=this->from_speeds(speed);
+    this->write_register(this->registers[goal_speed],data);
+    if(angle>this->info.center_angle)
+      angle=this->info.center_angle;
+    else if(angle<-this->info.center_angle)
+      angle=-this->info.center_angle;
+    data=this->from_angles(angle);
+    this->write_register(this->registers[goal_pos],data);
   }
   else
   {
-    try{
-      this->set_control_mode(angle_ctrl);
-      if(angle>this->info.center_angle)
-        angle=this->info.center_angle;
-      else if(angle<-this->info.center_angle)
-        angle=-this->info.center_angle;
-      cmd[0]=this->from_angles(angle);
-      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);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
+    this->set_control_mode(current_pos_ctrl);
+    if(current>this->info.max_current)
+      current=this->info.max_current;
+    else if(current<-this->info.max_current)
+      current=-this->info.max_current;
+    data=(signed short int)(current*1000.0/2.69);
+    this->write_register(this->registers[goal_current],data);
+    if(speed>this->info.max_speed)
+      speed=this->info.max_speed;
+    data=this->from_speeds(speed);
+    this->write_register(this->registers[profile_vel],data);
+    if(angle>this->info.center_angle)
+      angle=this->info.center_angle;
+    else if(angle<-this->info.center_angle)
+      angle=-this->info.center_angle;
+    data=this->from_angles(angle);
+    this->write_register(this->registers[goal_pos],data);
   }
 }
 
-void CDynamixelMotor::move_relative_angle(double angle,double speed)
+void CDynamixelMotor::move_relative_angle(double angle,double speed,double current)
 {
-  unsigned short int cmd[2],pos;
+  unsigned int data,pos;
   double abs_angle;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+  if(current==std::numeric_limits<double>::infinity())
+  {
+    this->set_control_mode(position_ctrl);
+    if(speed>this->info.max_speed)
+      speed=this->info.max_speed;
+    data=this->from_speeds(speed);
+    this->write_register(this->registers[goal_speed],data);
+    this->read_register(this->registers[current_pos],pos);
+    abs_angle=angle+this->to_angles(pos);
+    if(abs_angle>this->info.center_angle)
+      abs_angle=this->info.center_angle;
+    else if(abs_angle<-this->info.center_angle)
+      abs_angle=-this->info.center_angle;
+    data=this->from_angles(abs_angle);
+    this->write_register(this->registers[goal_pos],data);
   }
   else
   {
-    try{
-      this->set_control_mode(angle_ctrl);
-      this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos);
-      abs_angle=angle+this->to_angles(pos);
-      if(abs_angle>this->info.center_angle)
-        abs_angle=this->info.center_angle;
-      else if(abs_angle<-this->info.center_angle)
-        abs_angle=-this->info.center_angle;
-      cmd[0]=this->from_angles(abs_angle);
-      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);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+    this->set_control_mode(current_pos_ctrl);
+    if(current>this->info.max_current)
+      current=this->info.max_current;
+    else if(current<-this->info.max_current)
+      current=-this->info.max_current;
+    data=(signed short int)(current*1000.0/2.69);
+    this->write_register(this->registers[goal_current],data);
+    if(speed>this->info.max_speed)
+      speed=this->info.max_speed;
+    data=this->from_speeds(speed);
+    this->write_register(this->registers[profile_vel],data);
+    this->read_register(this->registers[current_pos],pos);
+    abs_angle=angle+this->to_angles(pos);
+    if(abs_angle>this->info.center_angle)
+      abs_angle=this->info.center_angle;
+    else if(abs_angle<-this->info.center_angle)
+      abs_angle=-this->info.center_angle;
+    data=this->from_angles(abs_angle);
+    this->write_register(this->registers[goal_pos],data);
+  } 
 }
 
-void CDynamixelMotor::move_torque(double torque_ratio)
+void CDynamixelMotor::move_speed(double speed)
 {
-  unsigned short int torque=0;
+  unsigned int data;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      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 short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
-      this->dynamixel_dev->write_word_register(this->registers[goal_speed],torque);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->set_control_mode(velocity_ctrl);
+  if(speed>this->info.max_speed)
+    speed=this->info.max_speed;
+  else if(speed<-this->info.max_speed)
+    speed=-this->info.max_speed;
+  data=this->from_speeds(speed);
+  this->write_register(this->registers[goal_speed],data);
 }
 
-void CDynamixelMotor::stop(void)
+void CDynamixelMotor::move_pwm(double pwm_ratio)
 {
-  unsigned short int current_position;
+  unsigned int torque=0;
 
-  if(this->dynamixel_dev==NULL)
+  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)
   {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+    torque=((unsigned short int)(pwm_ratio*885.0/100.0));
+    this->write_register(this->registers[goal_pwm],torque);
   }
   else
   {
-    try{
-      if(this->mode==angle_ctrl)
-      {
-	this->dynamixel_dev->read_word_register(this->registers[current_pos],&current_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);
-      }
-      else
-	this->dynamixel_dev->write_word_register(this->registers[goal_speed],0);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
+    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)
+{
+  unsigned int data;
+
+  this->set_control_mode(current_ctrl);
+  if(current>this->info.max_current)
+    current=this->info.max_current;
+  else if(current<-this->info.max_current)
+    current=-this->info.max_current;
+  data=(signed short int)(current*1000.0/2.69);
+  this->write_register(this->registers[goal_current],data);
+}
+
+void CDynamixelMotor::stop(void)
+{
+  unsigned int current_position;
+
+  if(this->mode==position_ctrl || this->mode==ext_position_ctrl || this->mode==current_pos_ctrl)
+  {
+    this->read_register(this->registers[current_pos],current_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->write_register(this->registers[goal_pos],current_position);
+  }
+  else if(this->mode==current_ctrl)
+    this->move_current(0.0);
+  else if(this->mode==velocity_ctrl)
+    this->move_speed(0.0);
+  else if(this->mode==pwm_ctrl)
+    this->move_pwm(0.0);
+}
+
 double CDynamixelMotor::get_current_angle(void)
 {
-  unsigned short int data;
+  unsigned int data;
   double current_position;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_word_register(this->registers[current_pos],&data);
-      current_position = this->to_angles(data);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[current_pos],data);
+  current_position = this->to_angles(data);
 
   return current_position;
 }
 
 double CDynamixelMotor::get_current_speed(void)
 {
-  unsigned short int data;
+  unsigned int data;
   double c_speed;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_word_register(this->registers[current_speed],&data);
-      c_speed = this->to_speeds(data&0x03FF);
-      if(data&0x0400)
-        c_speed*=-1.0;
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[current_speed],data);
+  c_speed = this->to_speeds(data&0x03FF);
+  if(data&0x0400)
+    c_speed*=-1.0;
 
   return c_speed;
 }
 
 double CDynamixelMotor::get_current_temperature(void)
 {
-  unsigned char data;
+  unsigned int data;
   double c_temp;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_byte_register(this->registers[current_temp],&data);
-      c_temp = (double)data;
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[current_temp],data);
+  c_temp = (double)data;
 
   return c_temp;
 }
 
 double CDynamixelMotor::get_current_voltage(void)
 {
-  unsigned char data;
+  unsigned int data;
   double c_voltage;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_byte_register(this->registers[current_voltage],&data);
-      c_voltage = (double)data/10;
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[current_voltage],data);
+  c_voltage = (double)data/10.0;
 
   return c_voltage;
 }
 
-double CDynamixelMotor::get_current_effort(void)
+double CDynamixelMotor::get_current_pwm(void)
 {
-  unsigned short int data;
+  unsigned int data;
   double c_effort;
 
-  if(this->dynamixel_dev==NULL)
+  if(this->info.ext_memory_map)
   {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+    this->read_register(this->registers[current_pwm],data);
+    c_effort=100.0*((signed short int)data)/885.0;
   }
   else
   {
-    try
-    {
-      this->dynamixel_dev->read_word_register(this->registers[current_load],&data);
-      c_effort = (double)((data&0x3FF)*100.0/1023);
-      if(this->get_current_speed() < 0)
-	c_effort *= -1.0;
-    }
-    catch(CDynamixelAlarmException &e)
-    {
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
+    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;
 }
 
-unsigned short int CDynamixelMotor::get_punch(void)
+double CDynamixelMotor::get_current_current(void)
+{
+  double current;
+  unsigned int data;
+
+  this->read_register(this->registers[current_load],data);
+  current=((signed short int)data)*2.69/1000.0;
+
+  return current;
+}
+
+unsigned int CDynamixelMotor::get_punch(void)
 {  
-  unsigned short int value;
+  unsigned int value;
 
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      this->dynamixel_dev->read_word_register(this->registers[punch],&value);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  this->read_register(this->registers[punch],value);
 
   return value;
 }
 
-void CDynamixelMotor::set_punch(unsigned short int punch_value)
+void CDynamixelMotor::set_punch(unsigned int punch_value)
 {
-  if(this->dynamixel_dev==NULL)
-  {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    try{
-      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);
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
-    }
-  }
+  if(punch_value<0x0020 || punch_value>0x03FF) 
+    throw CDynamixelMotorException(_HERE_,"Invalid Punch value");
+  this->config.punch=punch_value;
+  this->write_register(this->registers[punch],punch_value);
 }
 
 control_mode CDynamixelMotor::get_control_mode(void)
 {
-  if(this->dynamixel_dev==NULL)
+  unsigned int value;
+
+  if(this->info.ext_memory_map)
   {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+    this->read_register(this->registers[op_mode],value);
+    this->mode=(control_mode)value;
   }
   else
   {
-    try{
-      if(this->info.model=="XL-320")
-      {
-        this->dynamixel_dev->read_byte_register(this->registers[dyn_control_mode],(unsigned char *)&this->mode);
-      }
+    if(this->info.model=="XL-320")
+    {
+      this->read_register(this->registers[op_mode],value);
+      this->mode=(control_mode)value;
+    }
+    else
+    {
+      this->get_position_range(&this->config.min_angle,&this->config.max_angle);
+      if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
+        this->mode=pwm_ctrl;
       else
-      {
-        this->get_position_range(&this->config.min_angle,&this->config.max_angle);
-        if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
-          this->mode=torque_ctrl;
-        else
-          this->mode=angle_ctrl;
-      }
-    }catch(CDynamixelAlarmException &e){
-      /* handle dynamixel exception */
-      if(e.get_alarm()&this->alarms)
-	this->reset_motor();
-      throw;
+        this->mode=position_ctrl;
     }
   }
  
@@ -1457,11 +1280,13 @@ 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;
   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;
@@ -1474,6 +1299,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..185a7dc2b06716345e291e052d044b60b7602fc8 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"
@@ -11,6 +12,7 @@
 #include <string>
 #include <vector>
 #include <memory>
+#include <limits>
 
 typedef enum
 {
@@ -30,11 +32,14 @@ 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;
+  double max_current;
   unsigned int baudrate;
   unsigned char id;
+  bool ext_memory_map;
 }TDynamixel_info;
 
 typedef struct
@@ -47,9 +52,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
@@ -59,11 +64,12 @@ typedef struct
   double max_temperature;
   double max_voltage;
   double min_voltage;
-  double max_torque;
+  double max_pwm;
+  double max_current;
   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 
@@ -112,7 +118,12 @@ class CDynamixelMotor
      * \brief
      *
      */
-    unsigned short int *registers;
+    TDynReg *registers;
+    /**
+     * \brief
+     *
+     */
+    bool enabled;
   protected:
     /**
      * \brief
@@ -128,12 +139,12 @@ class CDynamixelMotor
      * \brief
      *
      */
-    double to_angles(unsigned short int counts); 
+    double to_angles(unsigned int counts); 
     /**
      * \brief
      *
      */
-    double to_speeds(unsigned short int counts); 
+    double to_speeds(unsigned int counts); 
     /**
      * \brief 
      *
@@ -149,6 +160,16 @@ class CDynamixelMotor
      *
      */
     void set_control_mode(control_mode mode);
+    /**
+     * \brief 
+     *
+     */
+    void read_register(TDynReg reg, unsigned int &value);
+    /**
+     * \brief 
+     *
+     */
+    void write_register(TDynReg reg, unsigned int value);
   public:
     /**
      * \brief
@@ -237,22 +258,42 @@ 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 
+     *  
+     */ 
+    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 
      *  
@@ -273,6 +314,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 
@@ -308,20 +369,30 @@ class CDynamixelMotor
      * \brief 
      *
      */
-    void move_absolute_angle(double angle,double speed);
+    void move_absolute_angle(double angle,double speed,double current=std::numeric_limits<double>::infinity());
+    /**
+     * \brief 
+     *
+     */
+    void move_relative_angle(double angle,double speed,double current=std::numeric_limits<double>::infinity());
+    /**
+     * \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);
     /**
@@ -338,7 +409,12 @@ class CDynamixelMotor
      * \brief 
      *  
      */ 
-    double get_current_effort(void);
+    double get_current_pwm(void);
+    /**
+     * \brief 
+     *  
+     */ 
+    double get_current_current(void);
     /**
      * \brief 
      *  
@@ -353,12 +429,12 @@ class CDynamixelMotor
      * \brief 
      *  
      */ 
-    unsigned short int get_punch(void);
+    unsigned int get_punch(void);
     /**
      * \brief 
      *  
      */ 
-    void set_punch(unsigned short int punch_value);
+    void set_punch(unsigned int punch_value);
     /**
      * \brief 
      *
diff --git a/src/dynamixel_motor_exceptions.cpp b/src/dynamixel_motor_exceptions.cpp
index 00402baa89c395af55945aa47a0f5712c6aa7aaa..06a1bc330bcf2aa31f3b307454313c210d2894bc 100644
--- a/src/dynamixel_motor_exceptions.cpp
+++ b/src/dynamixel_motor_exceptions.cpp
@@ -5,6 +5,7 @@
 
 const std::string dynamixel_motor_error_message="DynamixelMotor error - ";
 const std::string dynamixel_motor_group_error_message="DynamixelMotorGroup error - ";
+const std::string dynamixel_motor_unsupported_feature_error_message="DynamixelMotorUnsupportedFeature error - ";
 const std::string dynamixel_motion_sequence_error_message="DynamixelMotionSeq error - ";
 
 CDynamixelMotorException::CDynamixelMotorException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_error_message)
@@ -21,6 +22,13 @@ CDynamixelMotorGroupException::CDynamixelMotorGroupException(const std::string&
   this->error_msg+=error_msg;
 }
 
+CDynamixelMotorUnsupportedFeatureException::CDynamixelMotorUnsupportedFeatureException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_unsupported_feature_error_message)
+{
+  std::stringstream text;
+ 
+  this->error_msg+=error_msg;
+}
+
 CDynamixelMotionSequenceException::CDynamixelMotionSequenceException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motion_sequence_error_message)
 {
   std::stringstream text;
diff --git a/src/dynamixel_motor_exceptions.h b/src/dynamixel_motor_exceptions.h
index 25487eedb5b253853b257fb4eafc4f371757ce67..aca7ee2ca74a4f019f8f26dd0bc31f991c9821eb 100644
--- a/src/dynamixel_motor_exceptions.h
+++ b/src/dynamixel_motor_exceptions.h
@@ -33,6 +33,19 @@ class CDynamixelMotorGroupException : public CException
     CDynamixelMotorGroupException(const std::string& where,const std::string &error_msg);
 };
 
+/**
+ * \brief 
+ */
+class CDynamixelMotorUnsupportedFeatureException : public CException
+{
+  public:
+    /**
+     * \brief
+     *
+     */
+    CDynamixelMotorUnsupportedFeatureException(const std::string& where,const std::string &error_msg);
+};
+
 /**
  * \brief 
  */
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 202f59cdc16640c8e952a8a0646052e824f94468..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],&current_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],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_torque],&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][torque_limit],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],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,99 +148,78 @@ 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_word_register(0x0000,&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;
       }
@@ -259,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][ccw_angle_limit],max_pos);
-        usleep(10000);
-	this->dynamixel_dev[index]->write_word_register(this->registers[index][cw_angle_limit],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][ccw_angle_limit],&angle_limit);
-      (*max)=this->to_angles(index,angle_limit);
-      this->dynamixel_dev[index]->read_word_register(this->registers[index][cw_angle_limit],&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)
@@ -333,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],(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],&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][low_voltage_limit],min_voltage);
-          usleep(10000);
-	  this->dynamixel_dev[index]->write_byte_register(this->registers[index][high_voltage_limit],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][low_voltage_limit],&min_voltage);
-      this->dynamixel_dev[index]->read_byte_register(this->registers[index][high_voltage_limit],&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],&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_torque(unsigned int index,double torque_ratio)
+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_torque=torque_ratio;
-	this->dynamixel_dev[index]->write_word_register(this->registers[index][max_torque],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][torque_limit],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_torque(unsigned int index)
+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_torque],&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)
+  unsigned int value;
+
+  if(!this->info[index].pid_control)
   {
-    /* handle exceptions */
-    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-  }
-  else
-  {
-    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);
-      }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][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);
-      }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;
   }
 }
 
@@ -631,7 +439,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);
@@ -666,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)
   {
@@ -674,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][dyn_control_mode],(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;
 
@@ -735,18 +622,18 @@ 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)
     {
-      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);
     }
@@ -803,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;
@@ -848,18 +735,18 @@ 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)
       {
-        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);
       }
@@ -890,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],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);
-	}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)
-    {
-      /* handle exceptions */
-      throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
-    }
-    else
+    if(this->info[i].pid_control)
     {
-      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][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->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);
     }
   }
 }
@@ -988,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],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)
@@ -1013,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],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)
@@ -1039,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], 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],&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],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++)
+  {
+    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);
   }
 }
 
@@ -1104,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->dyn_server->write_sync(this->servo_id,this->registers[0][goal_speed],data);
-  }catch(CDynamixelAlarmException &e){
-    /* handle dynamixel exception */
-    if(e.get_alarm()&this->alarms[i])
-      this->reset_motor(i);
-    throw;
+  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);
   }
 }
 
 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],&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->dynamixel_dev[i]->write_word_register(this->registers[i][goal_pos],current_position);
-	}
-	else
-	  this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_speed],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],&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],&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],&c_effort);
-        this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&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],&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],&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;
   }
 }
 
@@ -1320,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],&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],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 a79f87a36f3055413e3d3b72a0f3ebc791616a67..537a257abbe0cd1a80c279468df074177d371f4d 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 
      *  
@@ -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 5619600f6d3bac8832db5a677ca4ccde0a29aee0..576f611d5d8a3d409a83a6baaa02f3d974329806 100644
--- a/src/dynamixel_registers.cpp
+++ b/src/dynamixel_registers.cpp
@@ -1,122 +1,347 @@
 #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},
+                         {0x0020,2,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,1,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,1,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,1,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}};
+
+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 481a2eba9ec886e7ee0de37385830caa62627f5e..5e32a305a56317005c53c21cba49e1e5d767e499 100644
--- a/src/dynamixel_registers.h
+++ b/src/dynamixel_registers.h
@@ -1,137 +1,91 @@
 #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];
+extern TDynReg xm_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_pwm,            //   (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..f5ed16a0fe13b4bb458617b0989094fbdd87a121 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="A600cVjj"; //extracted from dynamixel library's test_dynamixel_server_no_scan or with 'dmesg' command: "SerialNumber: A600cVjj"
+  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 = 1;                //extracted from dynamixel library's test_dynamixel_server_no_scan
+  int device = 2;                //extracted from dynamixel library's test_dynamixel_server_no_scan
   
   CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
   CDynamixelMotor *cont = NULL;
@@ -29,12 +29,12 @@ 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
       /////////////////////////////////////////////////////////////////////////
-      /*
+/*      
       std::cout << "-----------------------------------------------------------" <<  std::endl;
       cont->get_servo_info(info);
       std::cout << "[INFO]: Servo model: "        << info.model << std::endl;
@@ -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_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]: Punch: " << std::dec << cont->get_punch() << 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
       double max_angle_error =   0.5; //max angle error permitted
       double time_interval   =   0.1; //time in secs between checks
@@ -94,13 +90,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,29 +126,35 @@ 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
       /////////////////////////////////////////////////////////////////////////
-      /*
+      
+      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 desired_angle,absolute_angle=0.0,desired_speed=1.0,current_angle,max_angle_error=0.5,desired_current=0.1;
+
       std::cout << "-----------------------------------------------------------" <<  std::endl;
-      double absolute_angle=0.0;
-      double current_abs_angle;
-      std::cout << "MOVE ABSOLUTE ANGLE: " << absolute_angle << std::endl;
 
-      current_abs_angle = cont->get_current_angle();
+      cont->enable();
       desired_angle=absolute_angle;
       std::cout << "Desired angle: " << desired_angle << std::endl;
-      std::cout << "Current angle: " << current_abs_angle << std::endl;
+      current_angle=cont->get_current_angle();
+      std::cout << "Current angle: " << current_angle << std::endl;
 
-      cont->move_absolute_angle(absolute_angle, desired_speed);
+      cont->move_absolute_angle(absolute_angle, desired_speed,desired_current);
       std::cout << "Moving..." << std::endl;
 
       t=0;
-      while(fabs(current_abs_angle)>max_angle_error && t<timeout)
+      while(fabs(current_angle)>max_angle_error && t<timeout)
       {
-        current_abs_angle = cont->get_current_angle();
+        current_angle=cont->get_current_angle();
+        std::cout << current_angle << std::endl;
         usleep(uperiod);
         t++;
       }
@@ -161,67 +163,134 @@ int main(int argc, char *argv[])
         std::cout << "Reached " << max_time_sec << "sec timeout"<< std::endl;
 
       std::cout << "Desired angle: " << desired_angle << std::endl;
-      std::cout << "Reached angle: " << current_abs_angle << std::endl;
-      std::cout << "Error angle: " << current_abs_angle-desired_angle << std::endl;
+      std::cout << "Reached angle: " << current_angle << std::endl;
+      std::cout << "Error angle: " << current_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 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;
       std::cout << "-----------------------------------------------------------" <<  std::endl;
       sleep(1);
-      //*/
-      
-      
+      cont->disable();
+*/      
+  
+      /////////////////////////////////////////////////////////////////////////
+      ////MOVE Current
+      /////////////////////////////////////////////////////////////////////////
+/*
+      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_current,desired_current;
+
+      cont->enable();
+      std::cout << "-----------------------------------------------------------" <<  std::endl;
+      std::cout << "Current limit " << cont->get_current_limit() << std::endl;
+      if(argc==2)
+        max_current = atof(argv[1]);
+      else
+        max_current = 0.5;
+
+      desired_current=max_current;
+      std::cout << "MOVE current: " << desired_current << std::endl;
+
+      cont->move_current(desired_current);
+      std::cout << "Moving..." << std::endl;
+
+      t=0;
+      while(t<timeout)
+      {
+        std::cout << "Current current: " << cont->get_current_current() << std::endl;
+        usleep(uperiod);
+        t++;
+      }
+      cont->move_current(0);
+
+      std::cout << "Done" << std::endl;
+      sleep(1);
+      std::cout << "----------------------------" << std::endl;
+
+      desired_current=-1.0*max_current;
+      std::cout << "MOVE current: " << desired_current << std::endl;
+
+      cont->move_current(desired_current);
+      std::cout << "Moving..." << std::endl;
+
+      t=0;
+      while(t<timeout)
+      {
+        std::cout << "Current current: " << cont->get_current_current() << std::endl;
+        usleep(uperiod);
+        t++;
+      }
+      cont->move_current(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,8 +319,7 @@ 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>