From eba1c161e2df4172a03c2a5a714480ccfa8e0779 Mon Sep 17 00:00:00 2001
From: pmlab <pmlab@deltonos.users.iri.prv>
Date: Fri, 6 Sep 2019 15:25:57 +0200
Subject: [PATCH] in the move_abolute_angle and move_relative_angle functions,
configured the speed and current before the target position.
---
src/dynamixel_motor.cpp | 32 ++++++++++++++++----------------
1 file changed, 16 insertions(+), 16 deletions(-)
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index fafd0d4..432550b 100644
--- a/src/dynamixel_motor.cpp
+++ b/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);
}
}
--
GitLab