Skip to content
Snippets Groups Projects
Commit a0303f56 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added support for the XM430 servo model.

Checked the validity of the registers variable before trying to access it.
parent 71895cb7
No related branches found
No related tags found
No related merge requests found
......@@ -197,6 +197,8 @@ void CDynamixelMotor::reset_motor(void)
unsigned int current_position;
unsigned int maximum_pwm;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
try{
this->read_register(this->registers[current_pos],current_position);
}catch(CDynamixelAlarmException &e){
......@@ -238,6 +240,7 @@ void CDynamixelMotor::get_model(void)
{
try{
this->dynamixel_dev->read_word_register(0x0000,&model);
this->registers=NULL;
switch(model)
{
case 0x000C: this->info.model="AX-12A";
......@@ -373,7 +376,7 @@ void CDynamixelMotor::get_model(void)
this->info.ext_memory_map=true;
break;
case 0x460: this->info.model="XM540-W270";
this->info.max_angle=350.0;
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=1023 * 0.229;
this->info.max_current=0.0;
......@@ -384,6 +387,18 @@ void CDynamixelMotor::get_model(void)
this->info.multi_turn=true;
this->info.ext_memory_map=true;
break;
case 0x3FC: this->info.model="XM430-W350";
this->info.max_angle=360.0;
this->info.center_angle=180.0;
this->info.max_speed=2760;
this->info.max_current=0.0;
this->info.encoder_resolution=4095;
this->info.gear_ratio=353.5;
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;
}
......@@ -393,8 +408,11 @@ void CDynamixelMotor::get_model(void)
this->reset_motor();
throw;
}
this->read_register(this->registers[firmware_version],value);
this->info.firmware_ver=value;
if(this->registers!=NULL)
{
this->read_register(this->registers[firmware_version],value);
this->info.firmware_ver=value;
}
}
}
......@@ -405,6 +423,8 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
if(this->mode!=mode)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
if(this->enabled)
this->disable();
if(this->info.ext_memory_map)
......@@ -459,6 +479,8 @@ void CDynamixelMotor::set_drive_mode(bool time_based,bool reverse)
}
else
this->reverse_mode=false;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->write_register(this->registers[drive_mode],value);
}
......@@ -704,6 +726,8 @@ void CDynamixelMotor::set_position_range(double min, double max)
}
else
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->config.max_angle=max;
this->config.min_angle=min;
this->write_register(this->registers[max_angle_limit],max_pos);
......@@ -719,6 +743,8 @@ void CDynamixelMotor::get_position_range(double *min, double *max)
{
unsigned int angle_limit;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[max_angle_limit],angle_limit);
(*max)=this->to_angles(angle_limit);
this->read_register(this->registers[min_angle_limit],angle_limit);
......@@ -729,6 +755,8 @@ int CDynamixelMotor::get_temperature_limit(void)
{
unsigned int limit;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[temp_limit],limit);
return limit;
......@@ -745,6 +773,8 @@ void CDynamixelMotor::set_temperature_limit(int limit)
}
else
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->config.max_temperature=limit;
value=limit;
this->write_register(this->registers[temp_limit],value);
......@@ -755,6 +785,8 @@ void CDynamixelMotor::get_voltage_limits(double *min, double *max)
{
unsigned int value;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[min_voltage_limit],value);
*min=((double)value/10.0);
this->read_register(this->registers[max_voltage_limit],value);
......@@ -779,6 +811,8 @@ void CDynamixelMotor::set_voltage_limits(double min, double max)
}
else
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->config.min_voltage=min;
this->config.max_voltage=max;
value=min*10;
......@@ -793,6 +827,8 @@ unsigned char CDynamixelMotor::get_led_alarms(void)
{
unsigned int led_alarms;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[alarm_led],led_alarms);
return led_alarms;
......@@ -809,6 +845,8 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms)
}
else
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
value=alarms;
this->write_register(this->registers[alarm_led],value);
}
......@@ -818,6 +856,8 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void)
{
unsigned int shutdown_alarms;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[alarm_shtdwn],shutdown_alarms);
return shutdown_alarms;
......@@ -834,6 +874,8 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
}
else
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->alarms=alarms;
value=alarms;
this->write_register(this->registers[alarm_shtdwn],value);
......@@ -845,6 +887,8 @@ double CDynamixelMotor::get_max_pwm(void)
unsigned int load;
double torque;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[max_pwm],load);
torque=(load&0x3FF)*100.0/1023.0;
if(load>0x3FF)
......@@ -858,6 +902,8 @@ double CDynamixelMotor::get_pwm_limit(void)
unsigned int load;
double torque;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[pwm_limit],load);
if(this->info.ext_memory_map)
torque=((signed short int)load)*100.0/885.0;
......@@ -881,6 +927,8 @@ void CDynamixelMotor::set_max_pwm(double torque_ratio)
}
else
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
load=torque_ratio*1023.0/100.0;
this->config.max_pwm=torque_ratio;
this->write_register(this->registers[max_pwm],load);
......@@ -897,6 +945,8 @@ void CDynamixelMotor::set_pwm_limit(double torque_ratio)
}
else
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
if(this->info.ext_memory_map)
load=torque_ratio*885.0/100.0;
else
......@@ -910,6 +960,8 @@ double CDynamixelMotor::get_current_limit(void)
double current;
unsigned int data;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[current_limit],data);
current=((signed short int)data)*2.69/1000.0;
......@@ -920,6 +972,8 @@ void CDynamixelMotor::set_current_limit(double current)
{
unsigned int data;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
data=(signed short int)(current*1000.0/2.69);
this->write_register(this->registers[current_limit],data);
}
......@@ -929,6 +983,8 @@ double CDynamixelMotor::get_speed_limit(void)
unsigned int data;
double speed;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[velocity_limit],data);
speed=this->to_speeds(data);
......@@ -939,6 +995,8 @@ void CDynamixelMotor::set_speed_limit(double speed)
{
unsigned int data;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
data=this->from_speeds(speed);
this->write_register(this->registers[velocity_limit],data);
}
......@@ -950,6 +1008,8 @@ void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
if(!this->info.pid_control)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[cw_comp_margin],value);
config.cw_compliance_margin=value;
this->read_register(this->registers[ccw_comp_margin],value);
......@@ -977,6 +1037,8 @@ void CDynamixelMotor::set_compliance_control(TDynamixel_compliance &config)
/* handle exceptions */
throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
}
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
if(config.cw_compliance_slope>=1 && config.cw_compliance_slope<=5) config.cw_compliance_slope=4;
else if(config.cw_compliance_slope>=6 && config.cw_compliance_slope<=11) config.cw_compliance_slope=8;
else if(config.cw_compliance_slope>=12 && config.cw_compliance_slope<=23) config.cw_compliance_slope=16;
......@@ -1022,6 +1084,8 @@ void CDynamixelMotor::get_pid_control(TDynamixel_pid &config)
if(this->info.pid_control)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[pos_pid_p],value);
config.p=value;
this->read_register(this->registers[pos_pid_i],value);
......@@ -1037,6 +1101,8 @@ void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
if(this->info.pid_control)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->pid.p=config.p;
this->pid.i=config.i;
this->pid.d=config.d;
......@@ -1071,11 +1137,15 @@ void CDynamixelMotor::set_feedfwd_control(double acc_gain,double vel_gain)
void CDynamixelMotor::turn_led_on(void)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->write_register(this->registers[led],1);
}
void CDynamixelMotor::turn_led_off(void)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->write_register(this->registers[led],0);
}
......@@ -1091,12 +1161,16 @@ void CDynamixelMotor::lock(void)
void CDynamixelMotor::enable(void)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->write_register(this->registers[torque_en],1);
this->enabled=true;
}
void CDynamixelMotor::disable(void)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->write_register(this->registers[torque_en],0);
this->enabled=false;
}
......@@ -1105,6 +1179,8 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed,double curre
{
unsigned int data;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
if(current==std::numeric_limits<double>::infinity())
{
this->set_control_mode(position_ctrl);
......@@ -1146,6 +1222,8 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed,double curre
unsigned int data,pos;
double abs_angle;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
if(current==std::numeric_limits<double>::infinity())
{
this->set_control_mode(position_ctrl);
......@@ -1190,6 +1268,8 @@ void CDynamixelMotor::move_speed(double speed)
{
unsigned int data;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->set_control_mode(velocity_ctrl);
if(speed>this->info.max_speed)
speed=this->info.max_speed;
......@@ -1203,6 +1283,8 @@ void CDynamixelMotor::move_pwm(double pwm_ratio)
{
unsigned int torque=0;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->set_control_mode(pwm_ctrl);
if(pwm_ratio>100.0)
pwm_ratio=100.0;
......@@ -1226,6 +1308,8 @@ void CDynamixelMotor::move_current(double current)
{
unsigned int data;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->set_control_mode(current_ctrl);
if(current>this->info.max_current)
current=this->info.max_current;
......@@ -1241,6 +1325,8 @@ void CDynamixelMotor::stop(void)
if(this->mode==position_ctrl || this->mode==ext_position_ctrl || this->mode==current_pos_ctrl)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[current_pos],current_position);
if(this->to_angles(current_position)>this->config.max_angle)
current_position=from_angles(this->config.max_angle);
......@@ -1261,6 +1347,8 @@ double CDynamixelMotor::get_current_angle(void)
unsigned int data;
double current_position;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[current_pos],data);
current_position = this->to_angles(data);
......@@ -1272,6 +1360,8 @@ double CDynamixelMotor::get_current_speed(void)
unsigned int data;
double c_speed;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[current_speed],data);
c_speed = this->to_speeds(data&0x03FF);
if(data&0x0400)
......@@ -1285,6 +1375,8 @@ double CDynamixelMotor::get_current_temperature(void)
unsigned int data;
double c_temp;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[current_temp],data);
c_temp = (double)data;
......@@ -1296,6 +1388,8 @@ double CDynamixelMotor::get_current_voltage(void)
unsigned int data;
double c_voltage;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[current_voltage],data);
c_voltage = (double)data/10.0;
......@@ -1307,6 +1401,8 @@ double CDynamixelMotor::get_current_pwm(void)
unsigned int data;
double c_effort;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
if(this->info.ext_memory_map)
{
this->read_register(this->registers[current_pwm],data);
......@@ -1328,6 +1424,8 @@ double CDynamixelMotor::get_current_current(void)
double current;
unsigned int data;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[current_load],data);
current=((signed short int)data)*2.69/1000.0;
......@@ -1338,6 +1436,8 @@ unsigned int CDynamixelMotor::get_punch(void)
{
unsigned int value;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->read_register(this->registers[punch],value);
return value;
......@@ -1347,6 +1447,8 @@ void CDynamixelMotor::set_punch(unsigned int punch_value)
{
if(punch_value<0x0020 || punch_value>0x03FF)
throw CDynamixelMotorException(_HERE_,"Invalid Punch value");
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
this->config.punch=punch_value;
this->write_register(this->registers[punch],punch_value);
}
......@@ -1355,6 +1457,8 @@ control_mode CDynamixelMotor::get_control_mode(void)
{
unsigned int value;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
if(this->info.ext_memory_map)
{
this->read_register(this->registers[op_mode],value);
......@@ -1384,6 +1488,8 @@ void CDynamixelMotor::get_drive_mode(bool &time_based,bool &reverse)
{
unsigned int value;
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
try{
this->read_register(this->registers[drive_mode],value);
if(value&0x04)
......@@ -1416,6 +1522,8 @@ void CDynamixelMotor::get_drive_mode(bool &time_based,bool &reverse)
unsigned short int CDynamixelMotor::get_register_address(reg_id id)
{
if(this->registers==NULL)
throw CDynamixelMotorException(_HERE_,"Unknown dynamixel model.");
return this->registers[id].address;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment