Commit eba1c161 authored by pmlab's avatar pmlab
Browse files

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.
parent 5e3a6eb2
......@@ -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);
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment