Skip to content
GitLab
Commit dec7c6aa authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented the current control and the position and current control modes.

parent 8e7efa29
Hide whitespace changes
Inline Side-by-side
src/dynamixel_motor.cpp
View file @ dec7c6aa
......@@ -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;
......
src/dynamixel_motor.h
View file @ dec7c6aa
......@@ -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
*
......
src/examples/test_dynamixel_motor.cpp
View file @ dec7c6aa
......@@ -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
/////////////////////////////////////////////////////////////////////////
......
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