Implemented the velcoity control mode.

src/dynamixel_motor.cpp

@@ -88,7 +88,10 @@ 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;
 }
@@ -106,7 +109,10 @@ 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;
 }
@@ -787,12 +793,21 @@ void CDynamixelMotor::set_current_limit(double current)
 
 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);
 }
 
 
@@ -995,27 +1010,60 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed,double curre
 
 void CDynamixelMotor::move_relative_angle(double angle,double speed,double current)
 {
-  unsigned int cmd[2],pos;
+  unsigned int data,pos;
   double abs_angle;
 
-  this->set_control_mode(position_ctrl);
-  this->read_register(this->registers[current_pos],pos);
-  abs_angle=angle+this->to_angles(pos);
-  if(abs_angle>this->info.center_angle)
-    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);
-  this->write_register(this->registers[goal_pos],cmd[0]);
-  if(speed>this->info.max_speed)
-    speed=this->info.max_speed;
-  cmd[1]=this->from_speeds(speed);
-  this->write_register(this->registers[goal_speed],cmd[1]);
+  if(current==std::numeric_limits<double>::infinity())
+  {
+    this->set_control_mode(position_ctrl);
+    this->read_register(this->registers[current_pos],pos);
+    abs_angle=angle+this->to_angles(pos);
+    if(abs_angle>this->info.center_angle)
+      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);
+    if(speed>this->info.max_speed)
+      speed=this->info.max_speed;
+    data=this->from_speeds(speed);
+    this->write_register(this->registers[goal_speed],data);
+  }
+  else
+  {
+    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);
+    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);
+    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::move_speed(double speed)
 {
+  unsigned int data;
 
+  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::move_pwm(double pwm_ratio)
@@ -1046,6 +1094,10 @@ 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);
 }
@@ -1054,7 +1106,7 @@ void CDynamixelMotor::stop(void)
 {
   unsigned int current_position;
 
-  if(this->mode==position_ctrl)
+  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)
@@ -1063,8 +1115,12 @@ void CDynamixelMotor::stop(void)
       current_position=from_angles(this->config.min_angle);
     this->write_register(this->registers[goal_pos],current_position);
   }
-  else
-    this->write_register(this->registers[goal_speed],0);
+  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)