diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index d9d7bd6bcfd66795e58394432a88346a0f43536e..b659e6a90177b2d82ad987901c8d24335d1b8659 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -25,6 +25,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->info.max_angle=-1;
this->info.center_angle=-1;
this->info.max_speed=-1;
+ this->info.max_current=-1;
this->info.baudrate=(unsigned int)-1;
this->info.id=(unsigned char)-1;
this->info.ext_memory_map=false;
@@ -55,6 +56,11 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->config.max_temperature=this->get_temperature_limit();
this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
this->config.max_pwm=this->get_pwm_limit();
+ try{
+ this->config.max_current=this->get_current_limit();
+ }catch(CDynamixelMotorUnsupportedFeatureException &e){
+ std::cout << "No current limit feature" << std::endl;
+ }
this->get_compliance_control(this->compliance);
this->get_pid_control(this->pid);
this->alarms=this->get_turn_off_alarms();
@@ -157,6 +163,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=300.0;
this->info.center_angle=150.0;
this->info.max_speed=354;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=254;
this->registers=ax_reg;
@@ -168,6 +175,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=300.0;
this->info.center_angle=150.0;
this->info.max_speed=2830;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=32;
this->registers=ax_reg;
@@ -179,6 +187,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=300.0;
this->info.center_angle=150.0;
this->info.max_speed=582;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=254;
this->registers=ax_reg;
@@ -190,6 +199,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=2820;
+ this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=32;
this->registers=mx_1_0_reg;
@@ -201,6 +211,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=330;
+ this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=193;
this->registers=mx_1_0_reg;
@@ -212,6 +223,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=378;
+ this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=200;
this->registers=mx_1_0_reg;
@@ -223,6 +235,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=270;
+ this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=225;
this->registers=mx_106_1_0_reg;
@@ -234,6 +247,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=300.0;
this->info.center_angle=150;
this->info.max_speed=684;
+ this->info.max_current=0.0;
this->info.encoder_resolution=1023;
this->info.gear_ratio=238;
this->registers=xl_reg;
@@ -245,6 +259,7 @@ void CDynamixelMotor::get_model(void)
this->info.max_angle=360.0;
this->info.center_angle=180;
this->info.max_speed=4620;
+ this->info.max_current=2.3;
this->info.encoder_resolution=4095;
this->info.gear_ratio=213;
this->registers=xm_reg;
@@ -753,12 +768,21 @@ void CDynamixelMotor::set_pwm_limit(double torque_ratio)
double CDynamixelMotor::get_current_limit(void)
{
+ double current;
+ unsigned int data;
+
+ this->read_register(this->registers[current_limit],data);
+ current=((signed short int)data)*2.69/1000.0;
+ return current;
}
void CDynamixelMotor::set_current_limit(double current)
{
+ unsigned int data;
+ data=(signed short int)(current*1000.0/2.69);
+ this->write_register(this->registers[current_limit],data);
}
double CDynamixelMotor::get_speed_limit(void)
@@ -931,19 +955,42 @@ void CDynamixelMotor::disable(void)
void CDynamixelMotor::move_absolute_angle(double angle,double speed,double current)
{
- unsigned int cmd[2];
+ unsigned int data;
- this->set_control_mode(position_ctrl);
- if(angle>this->info.center_angle)
- angle=this->info.center_angle;
- else if(angle<-this->info.center_angle)
- angle=-this->info.center_angle;
- cmd[0]=this->from_angles(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);
+ 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
+ {
+ 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);
+ 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);
+ }
}
void CDynamixelMotor::move_relative_angle(double angle,double speed,double current)
@@ -996,7 +1043,11 @@ void CDynamixelMotor::move_pwm(double pwm_ratio)
void CDynamixelMotor::move_current(double current)
{
+ unsigned int data;
+ this->set_control_mode(current_ctrl);
+ data=(signed short int)(current*1000.0/2.69);
+ this->write_register(this->registers[goal_current],data);
}
void CDynamixelMotor::stop(void)
@@ -1085,7 +1136,13 @@ double CDynamixelMotor::get_current_pwm(void)
double CDynamixelMotor::get_current_current(void)
{
+ double current;
+ unsigned int data;
+ this->read_register(this->registers[current_load],data);
+ current=((signed short int)data)*2.69/1000.0;
+
+ return current;
}
unsigned int CDynamixelMotor::get_punch(void)
@@ -1109,18 +1166,26 @@ control_mode CDynamixelMotor::get_control_mode(void)
{
unsigned int value;
- if(this->info.model=="XL-320")
+ if(this->info.ext_memory_map)
{
this->read_register(this->registers[op_mode],value);
this->mode=(control_mode)value;
}
else
{
- this->get_position_range(&this->config.min_angle,&this->config.max_angle);
- if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
- this->mode=pwm_ctrl;
+ if(this->info.model=="XL-320")
+ {
+ this->read_register(this->registers[op_mode],value);
+ this->mode=(control_mode)value;
+ }
else
- this->mode=position_ctrl;
+ {
+ this->get_position_range(&this->config.min_angle,&this->config.max_angle);
+ if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
+ this->mode=pwm_ctrl;
+ else
+ this->mode=position_ctrl;
+ }
}
return this->mode;
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index 95347b040f204165c75b6e2f490618dd7e644b93..185a7dc2b06716345e291e052d044b60b7602fc8 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -12,6 +12,7 @@
#include <string>
#include <vector>
#include <memory>
+#include <limits>
typedef enum
{
@@ -35,6 +36,7 @@ typedef struct
double max_angle;
double center_angle;
double max_speed;
+ double max_current;
unsigned int baudrate;
unsigned char id;
bool ext_memory_map;
@@ -63,6 +65,7 @@ typedef struct
double max_voltage;
double min_voltage;
double max_pwm;
+ double max_current;
unsigned short int punch;
}TDynamixel_config;
@@ -366,12 +369,12 @@ class CDynamixelMotor
* \brief
*
*/
- void move_absolute_angle(double angle,double speed,double current=-1.0);
+ void move_absolute_angle(double angle,double speed,double current=std::numeric_limits<double>::infinity());
/**
* \brief
*
*/
- void move_relative_angle(double angle,double speed,double current=-1.0);
+ void move_relative_angle(double angle,double speed,double current=std::numeric_limits<double>::infinity());
/**
* \brief
*
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index cbf916125ab00bb2769f23fedd868d95e745635f..f5ed16a0fe13b4bb458617b0989094fbdd87a121 100644
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -131,25 +131,30 @@ int main(int argc, char *argv[])
/////////////////////////////////////////////////////////////////////////
////MOVE ABSOLUTE ANGLE
/////////////////////////////////////////////////////////////////////////
-/*
+
+ double time_interval = 0.1; //time in secs between checks
+ int max_time_sec = 10.0; //max time to wait until timeout
+ int t;
+ double uperiod = time_interval*1000000.0;
+ double timeout = max_time_sec/(uperiod/1000000.0);
+ double desired_angle,absolute_angle=0.0,desired_speed=1.0,current_angle,max_angle_error=0.5,desired_current=0.1;
+
std::cout << "-----------------------------------------------------------" << std::endl;
- double absolute_angle=0.0;
- double current_abs_angle;
- std::cout << "MOVE ABSOLUTE ANGLE: " << absolute_angle << std::endl;
cont->enable();
- current_abs_angle = cont->get_current_angle();
desired_angle=absolute_angle;
std::cout << "Desired angle: " << desired_angle << std::endl;
- std::cout << "Current angle: " << current_abs_angle << std::endl;
+ current_angle=cont->get_current_angle();
+ std::cout << "Current angle: " << current_angle << std::endl;
- cont->move_absolute_angle(absolute_angle, desired_speed);
+ cont->move_absolute_angle(absolute_angle, desired_speed,desired_current);
std::cout << "Moving..." << std::endl;
t=0;
- while(fabs(current_abs_angle)>max_angle_error && t<timeout)
+ while(fabs(current_angle)>max_angle_error && t<timeout)
{
- current_abs_angle = cont->get_current_angle();
+ current_angle=cont->get_current_angle();
+ std::cout << current_angle << std::endl;
usleep(uperiod);
t++;
}
@@ -158,16 +163,16 @@ int main(int argc, char *argv[])
std::cout << "Reached " << max_time_sec << "sec timeout"<< std::endl;
std::cout << "Desired angle: " << desired_angle << std::endl;
- std::cout << "Reached angle: " << current_abs_angle << std::endl;
- std::cout << "Error angle: " << current_abs_angle-desired_angle << std::endl;
+ std::cout << "Reached angle: " << current_angle << std::endl;
+ std::cout << "Error angle: " << current_angle-desired_angle << std::endl;
std::cout << "Done" << std::endl;
sleep(1);
cont->disable();
-*/
+
/////////////////////////////////////////////////////////////////////////
////MOVE PWM
/////////////////////////////////////////////////////////////////////////
-
+/*
double time_interval = 0.1; //time in secs between checks
int max_time_sec = 10.0; //max time to wait until timeout
int t;
@@ -203,7 +208,7 @@ int main(int argc, char *argv[])
std::cout << "----------------------------" << std::endl;
desired_pwm=-1.0*max_pwm;
- std::cout << "MOVE EFFORT: " << desired_pwm << std::endl;
+ std::cout << "MOVE PWM: " << desired_pwm << std::endl;
cont->move_pwm(desired_pwm);
std::cout << "Moving..." << std::endl;
@@ -221,8 +226,66 @@ int main(int argc, char *argv[])
std::cout << "-----------------------------------------------------------" << std::endl;
sleep(1);
cont->disable();
-
-
+*/
+
+ /////////////////////////////////////////////////////////////////////////
+ ////MOVE Current
+ /////////////////////////////////////////////////////////////////////////
+/*
+ double time_interval = 0.1; //time in secs between checks
+ int max_time_sec = 10.0; //max time to wait until timeout
+ int t;
+ double uperiod = time_interval*1000000.0;
+ double timeout = max_time_sec/(uperiod/1000000.0);
+ double max_current,desired_current;
+
+ cont->enable();
+ std::cout << "-----------------------------------------------------------" << std::endl;
+ std::cout << "Current limit " << cont->get_current_limit() << std::endl;
+ if(argc==2)
+ max_current = atof(argv[1]);
+ else
+ max_current = 0.5;
+
+ desired_current=max_current;
+ std::cout << "MOVE current: " << desired_current << std::endl;
+
+ cont->move_current(desired_current);
+ std::cout << "Moving..." << std::endl;
+
+ t=0;
+ while(t<timeout)
+ {
+ std::cout << "Current current: " << cont->get_current_current() << std::endl;
+ usleep(uperiod);
+ t++;
+ }
+ cont->move_current(0);
+
+ std::cout << "Done" << std::endl;
+ sleep(1);
+ std::cout << "----------------------------" << std::endl;
+
+ desired_current=-1.0*max_current;
+ std::cout << "MOVE current: " << desired_current << std::endl;
+
+ cont->move_current(desired_current);
+ std::cout << "Moving..." << std::endl;
+
+ t=0;
+ while(t<timeout)
+ {
+ std::cout << "Current current: " << cont->get_current_current() << std::endl;
+ usleep(uperiod);
+ t++;
+ }
+ cont->move_current(0);
+
+ std::cout << "Done" << std::endl;
+ std::cout << "-----------------------------------------------------------" << std::endl;
+ sleep(1);
+ cont->disable();
+*/
/////////////////////////////////////////////////////////////////////////
////MOVE RANDOM TORQUES AND OUTPUT SPEEDS
/////////////////////////////////////////////////////////////////////////