From fdecbe2605f979c63409ebd807a9e39a687e176d Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 2 Sep 2019 17:58:54 +0200
Subject: [PATCH] Implemented the velcoity control mode.
---
src/dynamixel_motor.cpp | 94 ++++++++++++++++++++++++++++++++---------
1 file changed, 75 insertions(+), 19 deletions(-)
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index b659e6a..08a7a72 100644
--- a/src/dynamixel_motor.cpp
+++ b/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)
--
GitLab