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
......@@ -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;
......
......@@ -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
*
......
......@@ -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