Commit 8e7efa29 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented and tested the PWM control.

parent 0309e944
......@@ -27,6 +27,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->info.max_speed=-1;
this->info.baudrate=(unsigned int)-1;
this->info.id=(unsigned char)-1;
this->info.ext_memory_map=false;
this->compliance.cw_compliance_margin=0x00;
this->compliance.ccw_compliance_margin=0x00;
this->compliance.cw_compliance_slope=0x20;
......@@ -43,7 +44,8 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->config.punch=0x20;
this->registers=NULL;
this->dyn_server=dyn_server;
this->dynamixel_dev=NULL;
this->dynamixel_dev=NULL;
this->enabled=false;
try{
this->info.baudrate=this->dyn_server->get_baudrate();
this->dynamixel_dev=this->dyn_server->get_device(dev_id,version);
......@@ -52,7 +54,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_serv
this->get_control_mode();
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_max_pwm();
this->config.max_pwm=this->get_pwm_limit();
this->get_compliance_control(this->compliance);
this->get_pid_control(this->pid);
this->alarms=this->get_turn_off_alarms();
......@@ -160,6 +162,7 @@ void CDynamixelMotor::get_model(void)
this->registers=ax_reg;
this->info.pid_control=false;
this->info.multi_turn=false;
this->info.ext_memory_map=false;
break;
case 0x012C: this->info.model="AX-12W";
this->info.max_angle=300.0;
......@@ -170,6 +173,7 @@ void CDynamixelMotor::get_model(void)
this->registers=ax_reg;
this->info.pid_control=false;
this->info.multi_turn=false;
this->info.ext_memory_map=false;
break;
case 0x0012: this->info.model="AX-18A";
this->info.max_angle=300.0;
......@@ -180,6 +184,7 @@ void CDynamixelMotor::get_model(void)
this->registers=ax_reg;
this->info.pid_control=false;
this->info.multi_turn=false;
this->info.ext_memory_map=false;
break;
case 0x0168: this->info.model="MX-12";
this->info.max_angle=360.0;
......@@ -190,6 +195,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
this->info.ext_memory_map=false;
break;
case 0x001D: this->info.model="MX-28";
this->info.max_angle=360.0;
......@@ -200,6 +206,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
this->info.ext_memory_map=false;
break;
case 0x0136: this->info.model="MX-64";
this->info.max_angle=360.0;
......@@ -210,6 +217,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
this->info.ext_memory_map=false;
break;
case 0x0140: this->info.model="MX-106";
this->info.max_angle=360.0;
......@@ -220,6 +228,7 @@ void CDynamixelMotor::get_model(void)
this->registers=mx_106_1_0_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
this->info.ext_memory_map=false;
break;
case 0x015E: this->info.model="XL_320";
this->info.max_angle=300.0;
......@@ -230,6 +239,18 @@ void CDynamixelMotor::get_model(void)
this->registers=xl_reg;
this->info.pid_control=true;
this->info.multi_turn=false;
this->info.ext_memory_map=false;
break;
case 0x0406: this->info.model="XM-430-W210";
this->info.max_angle=360.0;
this->info.center_angle=180;
this->info.max_speed=4620;
this->info.encoder_resolution=4095;
this->info.gear_ratio=213;
this->registers=xm_reg;
this->info.pid_control=true;
this->info.multi_turn=true;
this->info.ext_memory_map=true;
break;
default: this->info.model="unknown";
break;
......@@ -248,29 +269,42 @@ void CDynamixelMotor::get_model(void)
void CDynamixelMotor::set_control_mode(control_mode mode)
{
unsigned int value;
bool was_enabled=this->enabled;
if(this->mode!=mode)
{
this->disable();
if(this->info.model=="XL_320")
if(this->enabled)
this->disable();
if(this->info.ext_memory_map)
{
value=mode;
this->write_register(this->registers[op_mode],value);
this->mode=mode;
}
else
{
if(mode==angle_ctrl)
if(this->info.model=="XL_320")
{
this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
this->mode=angle_ctrl;
if(mode==pwm_ctrl)
value=1;
else if(mode==position_ctrl)
value=2;
else
throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"Unsupported control mode");
this->write_register(this->registers[op_mode],value);
}
else
{
this->set_position_range(-this->info.center_angle,-this->info.center_angle);
this->mode=torque_ctrl;
if(mode==position_ctrl)
this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
else if(mode==pwm_ctrl)
this->set_position_range(-this->info.center_angle,-this->info.center_angle);
else
throw CDynamixelMotorUnsupportedFeatureException(_HERE_,"Unsupported control mode");
}
this->mode=mode;
}
if(was_enabled)
this->enable();
}
}
......@@ -297,9 +331,9 @@ void CDynamixelMotor::read_register(TDynReg reg, unsigned int &value)
if(reg.size==1)
value=reg_value[0];
else if(reg.size==2)
value=*((unsigned short int *)reg_value);
value=reg_value[0]+(reg_value[1]<<8);
else if(reg.size==4)
value=*((unsigned int *)reg_value);
value=reg_value[0]+(reg_value[1]<<8)+(reg_value[2]<<16)+(reg_value[3]<<24);
else
{
/* handle exceptions */
......@@ -494,11 +528,13 @@ void CDynamixelMotor::get_servo_info(TDynamixel_info &info)
info.gear_ratio=this->info.gear_ratio;
info.encoder_resolution=this->info.encoder_resolution;
info.pid_control=this->info.pid_control;
info.multi_turn=this->info.multi_turn;
info.max_angle=this->info.max_angle;
info.center_angle=this->info.center_angle;
info.max_speed=this->info.max_speed;
info.baudrate=this->info.baudrate;
info.id=this->info.id;
info.ext_memory_map=this->info.ext_memory_map;
}
void CDynamixelMotor::set_position_range(double min, double max)
......@@ -519,9 +555,9 @@ void CDynamixelMotor::set_position_range(double min, double max)
this->write_register(this->registers[max_angle_limit],max_pos);
this->write_register(this->registers[min_angle_limit],min_pos);
if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
this->mode=torque_ctrl;
this->mode=pwm_ctrl;
else
this->mode=angle_ctrl;
this->mode=position_ctrl;
}
}
......@@ -656,7 +692,7 @@ double CDynamixelMotor::get_max_pwm(void)
double torque;
this->read_register(this->registers[max_pwm],load);
torque=(load&0x3FF)*100.0/1023;
torque=(load&0x3FF)*100.0/1023.0;
if(load>0x3FF)
torque=-1*torque;
......@@ -669,9 +705,14 @@ double CDynamixelMotor::get_pwm_limit(void)
double torque;
this->read_register(this->registers[pwm_limit],load);
torque=(load&0x3FF)*100.0/1023;
if(load>0x3FF)
torque=-1*torque;
if(this->info.ext_memory_map)
torque=((signed short int)load)*100.0/885.0;
else
{
torque=(load&0x3FF)*100.0/1023.0;
if(load>0x3FF)
torque=-1*torque;
}
return torque;
}
......@@ -686,7 +727,7 @@ void CDynamixelMotor::set_max_pwm(double torque_ratio)
}
else
{
load=torque_ratio*1023/100.0;
load=torque_ratio*1023.0/100.0;
this->config.max_pwm=torque_ratio;
this->write_register(this->registers[max_pwm],load);
}
......@@ -702,11 +743,35 @@ void CDynamixelMotor::set_pwm_limit(double torque_ratio)
}
else
{
load=torque_ratio*1023/100.0;
if(this->info.ext_memory_map)
load=torque_ratio*885.0/100.0;
else
load=torque_ratio*1023.0/100.0;
this->write_register(this->registers[pwm_limit],load);
}
}
double CDynamixelMotor::get_current_limit(void)
{
}
void CDynamixelMotor::set_current_limit(double current)
{
}
double CDynamixelMotor::get_speed_limit(void)
{
}
void CDynamixelMotor::set_speed_limit(double speed)
{
}
void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
{
unsigned int value;
......@@ -812,6 +877,26 @@ void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
}
}
void CDynamixelMotor::get_vel_pi_control(TDynamixel_pid &config)
{
}
void CDynamixelMotor::set_vel_pi_control(TDynamixel_pid &config)
{
}
void CDynamixelMotor::get_feedfwd_control(double &acc_gain,double &vel_gain)
{
}
void CDynamixelMotor::set_feedfwd_control(double acc_gain,double vel_gain)
{
}
void CDynamixelMotor::turn_led_on(void)
{
this->write_register(this->registers[led],1);
......@@ -835,18 +920,20 @@ void CDynamixelMotor::lock(void)
void CDynamixelMotor::enable(void)
{
this->write_register(this->registers[torque_en],1);
this->enabled=true;
}
void CDynamixelMotor::disable(void)
{
this->write_register(this->registers[torque_en],0);
this->enabled=false;
}
void CDynamixelMotor::move_absolute_angle(double angle,double speed)
void CDynamixelMotor::move_absolute_angle(double angle,double speed,double current)
{
unsigned int cmd[2];
this->set_control_mode(angle_ctrl);
this->set_control_mode(position_ctrl);
if(angle>this->info.center_angle)
angle=this->info.center_angle;
else if(angle<-this->info.center_angle)
......@@ -859,12 +946,12 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
this->write_register(this->registers[goal_speed],cmd[1]);
}
void CDynamixelMotor::move_relative_angle(double angle,double speed)
void CDynamixelMotor::move_relative_angle(double angle,double speed,double current)
{
unsigned int cmd[2],pos;
double abs_angle;
this->set_control_mode(angle_ctrl);
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)
......@@ -879,26 +966,44 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed)
this->write_register(this->registers[goal_speed],cmd[1]);
}
void CDynamixelMotor::move_torque(double torque_ratio)
void CDynamixelMotor::move_speed(double speed)
{
}
void CDynamixelMotor::move_pwm(double pwm_ratio)
{
unsigned int torque=0;
this->set_control_mode(torque_ctrl);
if(torque_ratio>100.0)
torque_ratio=100.0;
else if(torque_ratio<-100.0)
torque_ratio=-100.0;
if(torque_ratio<0.0)
torque+=0x0400;
torque+=((unsigned int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
this->write_register(this->registers[goal_speed],torque);
this->set_control_mode(pwm_ctrl);
if(pwm_ratio>100.0)
pwm_ratio=100.0;
else if(pwm_ratio<-100.0)
pwm_ratio=-100.0;
if(this->info.ext_memory_map)
{
torque=((unsigned short int)(pwm_ratio*885.0/100.0));
this->write_register(this->registers[goal_pwm],torque);
}
else
{
if(pwm_ratio<0.0)
torque+=0x0400;
torque+=((unsigned int)(fabs(pwm_ratio)*1023.0/100.0))&0x03FF;
this->write_register(this->registers[goal_speed],torque);
}
}
void CDynamixelMotor::move_current(double current)
{
}
void CDynamixelMotor::stop(void)
{
unsigned int current_position;
if(this->mode==angle_ctrl)
if(this->mode==position_ctrl)
{
this->read_register(this->registers[current_pos],current_position);
if(this->to_angles(current_position)>this->config.max_angle)
......@@ -957,19 +1062,32 @@ double CDynamixelMotor::get_current_voltage(void)
return c_voltage;
}
double CDynamixelMotor::get_current_effort(void)
double CDynamixelMotor::get_current_pwm(void)
{
unsigned int data;
double c_effort;
this->read_register(this->registers[current_load],data);
c_effort = (double)((data&0x3FF)*100.0/1023);
if(this->get_current_speed() < 0)
c_effort *= -1.0;
if(this->info.ext_memory_map)
{
this->read_register(this->registers[current_pwm],data);
c_effort=100.0*((signed short int)data)/885.0;
}
else
{
this->read_register(this->registers[current_load],data);
c_effort = (double)((data&0x3FF)*100.0/1023.0);
if(this->get_current_speed() < 0)
c_effort *= -1.0;
}
return c_effort;
}
double CDynamixelMotor::get_current_current(void)
{
}
unsigned int CDynamixelMotor::get_punch(void)
{
unsigned int value;
......@@ -1000,9 +1118,9 @@ control_mode CDynamixelMotor::get_control_mode(void)
{
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=torque_ctrl;
this->mode=pwm_ctrl;
else
this->mode=angle_ctrl;
this->mode=position_ctrl;
}
return this->mode;
......@@ -1035,6 +1153,7 @@ CDynamixelMotor::~CDynamixelMotor()
this->info.max_speed=-1;
this->info.baudrate=(unsigned int)-1;
this->info.id=(unsigned char)-1;
this->info.ext_memory_map=false;
this->compliance.cw_compliance_margin=0x00;
this->compliance.ccw_compliance_margin=0x00;
this->compliance.cw_compliance_slope=0x20;
......
......@@ -37,6 +37,7 @@ typedef struct
double max_speed;
unsigned int baudrate;
unsigned char id;
bool ext_memory_map;
}TDynamixel_info;
typedef struct
......@@ -49,9 +50,9 @@ typedef struct
typedef struct
{
unsigned char p;
unsigned char i;
unsigned char d;
unsigned short int p;
unsigned short int i;
unsigned short int d;
}TDynamixel_pid;
typedef struct
......@@ -65,7 +66,7 @@ typedef struct
unsigned short int punch;
}TDynamixel_config;
typedef enum{angle_ctrl=2,torque_ctrl=1} control_mode;
typedef enum{current_ctrl=0,velocity_ctrl=1,position_ctrl=3,ext_position_ctrl=4,current_pos_ctrl=5,pwm_ctrl=6} control_mode;
/**
* \brief
......@@ -115,6 +116,11 @@ class CDynamixelMotor
*
*/
TDynReg *registers;
/**
* \brief
*
*/
bool enabled;
protected:
/**
* \brief
......@@ -265,6 +271,26 @@ class CDynamixelMotor
*
*/
void set_pwm_limit(double torque_ratio);
/**
* \brief
*
*/
double get_current_limit(void);
/**
* \brief
*
*/
void set_current_limit(double current);
/**
* \brief
*
*/
double get_speed_limit(void);
/**
* \brief
*
*/
void set_speed_limit(double speed);
/**
* \brief
*
......@@ -285,6 +311,26 @@ class CDynamixelMotor
*
*/
void set_pid_control(TDynamixel_pid &config);
/**
* \brief
*
*/
void get_vel_pi_control(TDynamixel_pid &config);
/**
* \brief
*
*/
void set_vel_pi_control(TDynamixel_pid &config);
/**
* \brief
*
*/
void get_feedfwd_control(double &acc_gain,double &vel_gain);
/**
* \brief
*
*/
void set_feedfwd_control(double acc_gain,double vel_gain);
/* control functions */
/**
* \brief
......@@ -320,20 +366,30 @@ class CDynamixelMotor
* \brief
*
*/
void move_absolute_angle(double angle,double speed);
void move_absolute_angle(double angle,double speed,double current=-1.0);
/**
* \brief
*
*/
void move_relative_angle(double angle,double speed,double current=-1.0);
/**
* \brief
*
*/
void move_speed(double speed);
/**
* \brief
*
*/
void move_relative_angle(double angle,double speed);
void move_pwm(double pwm_ratio);
/**
* \brief
*
*/
void move_torque(double torque_ratio);
void move_current(double current);
/**
* \brief
*
*
*/
void stop(void);
/**
......@@ -350,7 +406,12 @@ class CDynamixelMotor
* \brief
*
*/
double get_current_effort(void);
double get_current_pwm(void);
/**
* \brief
*
*/
double get_current_current(void);
/**
* \brief
*
......
This diff is collapsed.
......@@ -178,6 +178,16 @@ class CDynamixelMotorGroup
*
*/
void set_control_mode(control_mode mode);
/**
* \brief
*
*/
void read_register(unsigned int index,TDynReg reg, unsigned int &value);
/**
* \brief
*
*/
void write_register(unsigned int index,TDynReg reg, unsigned int value);
public:
/**
* \brief
......@@ -286,12 +296,12 @@ class CDynamixelMotorGroup
* \brief
*
*/
void get_punch(std::vector<unsigned short int> &punches);
void get_punch(std::vector<unsigned int> &punches);
/**
* \brief
*
*/
void set_punch(std::vector<unsigned short int> &punches);
void set_punch(std::vector<unsigned int> &punches);
/**
* \brief
*
......
......@@ -112,7 +112,7 @@ TDynReg mx_1_0_reg[NUM_REG]={// Info
{0xFFFF,0,false},
{0x001C,1,false},
{0x001B,1,false},
{0x001A,0,false},
{0x001A,1,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
{0xFFFF,0,false},
......@@ -181,7 +181,7 @@ TDynReg mx_106_1_0_reg[NUM_REG]={// Info
{0xFFFF,0,false},
</