in the move_abolute_angle and move_relative_angle functions, configured the...

in the move_abolute_angle and move_relative_angle functions, configured the speed and current before the target position.

src/dynamixel_motor.cpp

@@ -975,16 +975,16 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed,double curre
   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);
-    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
   {
@@ -995,16 +995,16 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed,double curre
       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);
-    if(speed>this->info.max_speed)
-      speed=this->info.max_speed;
-    data=this->from_speeds(speed);
-    this->write_register(this->registers[profile_vel],data);
   }
 }
 
@@ -1016,6 +1016,10 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed,double curre
   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)
@@ -1024,10 +1028,6 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed,double curre
       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
   {
@@ -1038,6 +1038,10 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed,double curre
       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)
@@ -1046,10 +1050,6 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed,double curre
       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);
   } 
 }