diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c626213960026842e233717dde865f1d5ce084ad..1e6bb535f58970ecc144663df0615254c97f4455 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,9 +1,9 @@ ADD_SUBDIRECTORY(xml) # edit the following line to add all the source code files of the library -SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp dynamixel_motor_exceptions.cpp) +SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp dynamixel_registers.cpp dynamixel_motor_exceptions.cpp) # edit the following line to add all the header files of the library -SET(headers dynamixel_motor.h dynamixel_motor_group.h dynamixel_motor_exceptions.h) +SET(headers dynamixel_motor.h dynamixel_motor_group.h dynamixel_registers.h dynamixel_motor_exceptions.h) FIND_PACKAGE(iriutils REQUIRED) diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp index 591e7ecdc158005012d1f74b5d5f388180f4ee21..015703c47489ac6809c56dffc305ce7d9d5d4058 100644 --- a/src/dynamixel_motor.cpp +++ b/src/dynamixel_motor.cpp @@ -1,4 +1,5 @@ #include "dynamixel_motor_exceptions.h" +#include "dynamixel_registers.h" #include "dynamixelexceptions.h" #include "dynamixelserver.h" #include "eventexceptions.h" @@ -15,7 +16,7 @@ #include "xml/dynamixel_motor_cfg_file.hxx" #endif -CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id) +CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id,dyn_version_t version) { this->info.model=""; this->info.firmware_ver=(unsigned char)-1; @@ -41,13 +42,15 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b this->config.max_voltage=0.0; this->config.min_voltage=0.0; this->config.max_torque=0.0; + this->config.punch=0x20; + this->registers=NULL; this->dyn_server=CDynamixelServer::instance(); this->dynamixel_dev=NULL; try{ this->dyn_server->config_bus(bus_id,baudrate); this->info.baudrate=baudrate; this->info.bus_id=this->dyn_server->get_bus_serial(); - this->dynamixel_dev=this->dyn_server->get_device(dev_id); + this->dynamixel_dev=this->dyn_server->get_device(dev_id,version); this->info.id=dev_id; this->get_model(); this->get_position_range(&this->config.min_angle,&this->config.max_angle); @@ -70,7 +73,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b } } -CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id) +CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id,dyn_version_t version) { this->info.model=""; this->info.firmware_ver=(unsigned char)-1; @@ -96,13 +99,15 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int ba this->config.max_voltage=0.0; this->config.min_voltage=0.0; this->config.max_torque=0.0; + this->config.punch=0x20; + this->registers=NULL; this->dyn_server=CDynamixelServer::instance(); this->dynamixel_dev=NULL; try{ this->dyn_server->config_bus(bus_id,baudrate); this->info.baudrate=baudrate; this->info.bus_id=bus_id; - this->dynamixel_dev=this->dyn_server->get_device(dev_id); + this->dynamixel_dev=this->dyn_server->get_device(dev_id,version); this->info.id=dev_id; this->get_model(); this->get_position_range(&this->config.min_angle,&this->config.max_angle); @@ -169,27 +174,27 @@ void CDynamixelMotor::reset_motor(void) unsigned short int maximum_torque; try{ - this->dynamixel_dev->read_word_register(current_pos,¤t_position); + this->dynamixel_dev->read_word_register(this->registers[current_pos],¤t_position); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev->write_word_register(goal_pos,current_position); + this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev->read_word_register(max_torque,&maximum_torque); + this->dynamixel_dev->read_word_register(this->registers[max_torque],&maximum_torque); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev->write_word_register(torque_limit,maximum_torque); + this->dynamixel_dev->write_word_register(this->registers[torque_limit],maximum_torque); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev->write_byte_register(led,0x00); + this->dynamixel_dev->write_byte_register(this->registers[led],0x00); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } @@ -207,8 +212,8 @@ void CDynamixelMotor::get_model(void) else { try{ - this->dynamixel_dev->read_byte_register(firmware_version,&this->info.firmware_ver); - this->dynamixel_dev->read_word_register(model_number,&model); + this->dynamixel_dev->read_byte_register(std_firmware_version,&this->info.firmware_ver); + this->dynamixel_dev->read_word_register(std_model_number,&model); switch(model) { case 0x000C: this->info.model="AX-12A"; @@ -217,6 +222,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=354; this->info.encoder_resolution=1023; this->info.gear_ratio=254; + this->registers=std_compl_reg; this->info.pid_control=false; break; case 0x012C: this->info.model="AX-12W"; @@ -225,6 +231,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=2830; this->info.encoder_resolution=1023; this->info.gear_ratio=32; + this->registers=std_compl_reg; this->info.pid_control=false; break; case 0x0012: this->info.model="AX-18A"; @@ -233,6 +240,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=582; this->info.encoder_resolution=1023; this->info.gear_ratio=254; + this->registers=std_compl_reg; this->info.pid_control=false; break; case 0x001D: this->info.model="MX-28"; @@ -241,6 +249,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=330; this->info.encoder_resolution=4095; this->info.gear_ratio=193; + this->registers=std_pid_reg; this->info.pid_control=true; break; case 0x0018: this->info.model="RX-24F"; @@ -249,6 +258,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=756; this->info.encoder_resolution=1023; this->info.gear_ratio=193; + this->registers=std_compl_reg; this->info.pid_control=false; break; case 0x001C: this->info.model="RX-28"; @@ -257,6 +267,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=402; this->info.encoder_resolution=1023; this->info.gear_ratio=193; + this->registers=std_compl_reg; this->info.pid_control=false; break; case 0x0136: this->info.model="MX-64"; @@ -265,6 +276,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=378; this->info.encoder_resolution=4095; this->info.gear_ratio=200; + this->registers=std_pid_reg; this->info.pid_control=true; break; case 0x0040: this->info.model="Rx-64"; @@ -273,6 +285,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=294; this->info.encoder_resolution=1023; this->info.gear_ratio=200; + this->registers=std_compl_reg; this->info.pid_control=false; break; case 0x0140: this->info.model="MX-106"; @@ -281,6 +294,7 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=270; this->info.encoder_resolution=4095; this->info.gear_ratio=225; + this->registers=std_pid_reg; this->info.pid_control=true; break; case 0x006B: this->info.model="Ex-106+"; @@ -289,8 +303,18 @@ void CDynamixelMotor::get_model(void) this->info.max_speed=414; this->info.encoder_resolution=4095; this->info.gear_ratio=184; + this->registers=std_compl_reg; this->info.pid_control=false; break; + case 0x015E: this->info.model="XL_320"; + this->info.max_angle=300.0; + this->info.center_angle=150; + this->info.max_speed=684; + this->info.encoder_resolution=1023; + this->info.gear_ratio=238; + this->registers=xl_reg; + this->info.pid_control=true; + break; default: this->info.model="unknown"; break; } @@ -313,6 +337,43 @@ void CDynamixelMotor::load_config(TDynamixel_config &config) } #ifdef _HAVE_XSD +void CDynamixelMotor::read_config(std::string &filename,TDynamixel_config &config, TDynamixel_compliance &comp, TDynamixel_pid &pid) +{ + struct stat buffer; + + if(stat(filename.c_str(),&buffer)==0) + { + // try to open the specified file + try{ + std::auto_ptr<dynamixel_motor_config_t> cfg(dynamixel_motor_config(filename.c_str(), xml_schema::flags::dont_validate)); + // configure the parameters of the controller + config.max_angle=cfg->max_angle(); + config.min_angle=cfg->min_angle(); + config.max_temperature=cfg->temp_limit(); + config.max_voltage=cfg->max_voltage(); + config.min_voltage=cfg->min_voltage(); + config.max_torque=cfg->max_torque(); + config.punch=cfg->punch(); + /* pid control */ + pid.p=cfg->kp(); + pid.i=cfg->ki(); + pid.d=cfg->kd(); + /* compliance control */ + comp.cw_compliance_margin=cfg->cw_comp_margin(); + comp.ccw_compliance_margin=cfg->ccw_comp_margin(); + comp.cw_compliance_slope=cfg->cw_comp_slope(); + comp.ccw_compliance_slope=cfg->ccw_comp_slope(); + }catch (const xml_schema::exception& e){ + std::ostringstream os; + os << e; + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,os.str()); + } + } + else + throw CDynamixelMotorException(_HERE_,"The configuration file does not exist"); +} + void CDynamixelMotor::load_config(std::string &filename) { TDynamixel_compliance compliance; @@ -342,9 +403,9 @@ void CDynamixelMotor::load_config(std::string &filename) compliance.ccw_compliance_margin=cfg->ccw_comp_margin(); compliance.cw_compliance_slope=cfg->cw_comp_slope(); compliance.ccw_compliance_slope=cfg->ccw_comp_slope(); - compliance.punch=cfg->punch(); this->set_compliance_control(compliance); } + this->set_punch(cfg->punch()); this->set_max_torque(cfg->max_torque()); this->set_limit_torque(cfg->max_torque()); }catch (const xml_schema::exception& e){ @@ -384,7 +445,7 @@ void CDynamixelMotor::save_config(std::string &filename) compliance.ccw_compliance_margin, compliance.cw_compliance_slope, compliance.ccw_compliance_slope, - compliance.punch, + this->get_punch(), pid.p, pid.i, pid.d); @@ -438,8 +499,10 @@ void CDynamixelMotor::set_position_range(double min, double max) try{ this->config.max_angle=max; this->config.min_angle=min; - this->dynamixel_dev->write_word_register(ccw_angle_limit,max_pos); - this->dynamixel_dev->write_word_register(cw_angle_limit,min_pos); + this->dynamixel_dev->write_word_register(this->registers[ccw_angle_limit],max_pos); + usleep(10000); + this->dynamixel_dev->write_word_register(this->registers[cw_angle_limit],min_pos); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -462,9 +525,9 @@ void CDynamixelMotor::get_position_range(double *min, double *max) else { try{ - this->dynamixel_dev->read_word_register(ccw_angle_limit,&angle_limit); + this->dynamixel_dev->read_word_register(this->registers[ccw_angle_limit],&angle_limit); (*max)=this->to_angles(angle_limit); - this->dynamixel_dev->read_word_register(cw_angle_limit,&angle_limit); + this->dynamixel_dev->read_word_register(this->registers[cw_angle_limit],&angle_limit); (*min)=this->to_angles(angle_limit); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -487,7 +550,7 @@ int CDynamixelMotor::get_temperature_limit(void) else { try{ - this->dynamixel_dev->read_byte_register(temp_limit,&limit); + this->dynamixel_dev->read_byte_register(this->registers[temp_limit],&limit); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -517,7 +580,8 @@ void CDynamixelMotor::set_temperature_limit(int limit) { try{ this->config.max_temperature=limit; - this->dynamixel_dev->write_byte_register(temp_limit,(unsigned char)limit); + this->dynamixel_dev->write_byte_register(this->registers[temp_limit],(unsigned char)limit); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -540,8 +604,8 @@ void CDynamixelMotor::get_voltage_limits(double *min, double *max) else { try{ - this->dynamixel_dev->read_byte_register(low_voltage_limit,&min_voltage); - this->dynamixel_dev->read_byte_register(high_voltage_limit,&max_voltage); + this->dynamixel_dev->read_byte_register(this->registers[low_voltage_limit],&min_voltage); + this->dynamixel_dev->read_byte_register(this->registers[high_voltage_limit],&max_voltage); *min=((double)min_voltage/10.0); *max=((double)max_voltage/10.0); }catch(CDynamixelAlarmException &e){ @@ -583,8 +647,10 @@ void CDynamixelMotor::set_voltage_limits(double min, double max) this->config.max_voltage=max; min_voltage=min*10; max_voltage=max*10; - this->dynamixel_dev->write_byte_register(low_voltage_limit,min_voltage); - this->dynamixel_dev->write_byte_register(high_voltage_limit,max_voltage); + this->dynamixel_dev->write_byte_register(this->registers[low_voltage_limit],min_voltage); + usleep(10000); + this->dynamixel_dev->write_byte_register(this->registers[high_voltage_limit],max_voltage); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -608,7 +674,7 @@ unsigned char CDynamixelMotor::get_led_alarms(void) else { try{ - this->dynamixel_dev->read_byte_register(alarm_led,&led_alarms); + this->dynamixel_dev->read_byte_register(this->registers[alarm_led],&led_alarms); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -637,7 +703,7 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms) else { try{ - this->dynamixel_dev->write_byte_register(alarm_led,(unsigned char)alarms); + this->dynamixel_dev->write_byte_register(this->registers[alarm_led],(unsigned char)alarms); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -660,7 +726,7 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void) else { try{ - this->dynamixel_dev->read_byte_register(alarm_shtdwn, &shutdown_alarms); + this->dynamixel_dev->read_byte_register(this->registers[alarm_shtdwn], &shutdown_alarms); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -690,7 +756,8 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms) { try{ this->alarms=alarms; - this->dynamixel_dev->write_byte_register(alarm_shtdwn,(unsigned char)alarms); + this->dynamixel_dev->write_byte_register(this->registers[alarm_shtdwn],(unsigned char)alarms); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -714,7 +781,7 @@ double CDynamixelMotor::get_max_torque(void) else { try{ - this->dynamixel_dev->read_word_register(max_torque,&load); + this->dynamixel_dev->read_word_register(this->registers[max_torque],&load); torque=(load&0x3FF)*100.0/1023; if(load>0x3FF) torque=-1*torque; @@ -742,7 +809,7 @@ double CDynamixelMotor::get_limit_torque(void) else { try{ - this->dynamixel_dev->read_word_register(torque_limit,&load); + this->dynamixel_dev->read_word_register(this->registers[torque_limit],&load); torque=(load&0x3FF)*100.0/1023; if(load>0x3FF) torque=-1*torque; @@ -777,7 +844,8 @@ void CDynamixelMotor::set_max_torque(double torque_ratio) load=torque_ratio*1023/100.0; try{ this->config.max_torque=torque_ratio; - this->dynamixel_dev->write_word_register(max_torque,load); + this->dynamixel_dev->write_word_register(this->registers[max_torque],load); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -807,7 +875,7 @@ void CDynamixelMotor::set_limit_torque(double torque_ratio) { load=torque_ratio*1023/100.0; try{ - this->dynamixel_dev->write_word_register(torque_limit,load); + this->dynamixel_dev->write_word_register(this->registers[torque_limit],load); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -830,11 +898,10 @@ void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config) if(!this->info.pid_control) { try{ - this->dynamixel_dev->read_byte_register(cw_comp_margin,&config.cw_compliance_margin); - this->dynamixel_dev->read_byte_register(ccw_comp_margin,&config.ccw_compliance_margin); - this->dynamixel_dev->read_byte_register(cw_comp_slope,&config.cw_compliance_slope); - this->dynamixel_dev->read_byte_register(ccw_comp_slope,&config.ccw_compliance_slope); - this->dynamixel_dev->read_byte_register(punch,&config.punch); + this->dynamixel_dev->read_byte_register(this->registers[cw_comp_margin],&config.cw_compliance_margin); + this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_margin],&config.ccw_compliance_margin); + this->dynamixel_dev->read_byte_register(this->registers[cw_comp_slope],&config.cw_compliance_slope); + this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_slope],&config.ccw_compliance_slope); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -890,21 +957,15 @@ void CDynamixelMotor::set_compliance_control(TDynamixel_compliance &config) /* handle exceptions */ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope."); } - if(config.punch<0 || config.punch>1023) - { - /* handle exceptions */ - throw CDynamixelMotorException(_HERE_,"Invalid punch value."); - } try{ this->compliance.cw_compliance_margin=config.cw_compliance_margin; this->compliance.ccw_compliance_margin=config.ccw_compliance_margin; this->compliance.cw_compliance_slope=config.cw_compliance_slope; this->compliance.ccw_compliance_slope=config.ccw_compliance_slope; - this->dynamixel_dev->write_byte_register(cw_comp_margin,config.cw_compliance_margin); - this->dynamixel_dev->write_byte_register(ccw_comp_margin,config.ccw_compliance_margin); - this->dynamixel_dev->write_byte_register(cw_comp_slope,config.cw_compliance_slope); - this->dynamixel_dev->write_byte_register(ccw_comp_slope,config.ccw_compliance_slope); - this->dynamixel_dev->write_word_register(punch,config.punch); + this->dynamixel_dev->write_byte_register(this->registers[cw_comp_margin],config.cw_compliance_margin); + this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_margin],config.ccw_compliance_margin); + this->dynamixel_dev->write_byte_register(this->registers[cw_comp_slope],config.cw_compliance_slope); + this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_slope],config.ccw_compliance_slope); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -927,9 +988,9 @@ void CDynamixelMotor::get_pid_control(TDynamixel_pid &config) if(this->info.pid_control) { try{ - this->dynamixel_dev->read_byte_register(pid_p,&config.p); - this->dynamixel_dev->read_byte_register(pid_i,&config.i); - this->dynamixel_dev->read_byte_register(pid_d,&config.d); + this->dynamixel_dev->read_byte_register(this->registers[pid_p],&config.p); + this->dynamixel_dev->read_byte_register(this->registers[pid_i],&config.i); + this->dynamixel_dev->read_byte_register(this->registers[pid_d],&config.d); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -954,9 +1015,9 @@ void CDynamixelMotor::set_pid_control(TDynamixel_pid &config) this->pid.p=config.p; this->pid.i=config.i; this->pid.d=config.d; - this->dynamixel_dev->write_byte_register(pid_p,config.p); - this->dynamixel_dev->write_byte_register(pid_i,config.i); - this->dynamixel_dev->write_byte_register(pid_d,config.d); + this->dynamixel_dev->write_byte_register(this->registers[pid_p],config.p); + this->dynamixel_dev->write_byte_register(this->registers[pid_i],config.i); + this->dynamixel_dev->write_byte_register(this->registers[pid_d],config.d); } } } @@ -971,7 +1032,7 @@ void CDynamixelMotor::turn_led_on(void) else { try{ - this->dynamixel_dev->write_byte_register(led,1); + this->dynamixel_dev->write_byte_register(this->registers[led],1); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -991,7 +1052,7 @@ void CDynamixelMotor::turn_led_off(void) else { try{ - this->dynamixel_dev->write_byte_register(led,0); + this->dynamixel_dev->write_byte_register(this->registers[led],0); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -1021,7 +1082,7 @@ void CDynamixelMotor::enable(void) else { try{ - this->dynamixel_dev->write_byte_register(torque_en,1); + this->dynamixel_dev->write_byte_register(this->registers[torque_en],1); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -1041,7 +1102,7 @@ void CDynamixelMotor::disable(void) else { try{ - this->dynamixel_dev->write_byte_register(torque_en,0); + this->dynamixel_dev->write_byte_register(this->registers[torque_en],0); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -1070,7 +1131,7 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed) } cmd[0]=this->from_angles(angle); cmd[1]=this->from_speeds(speed); - this->dynamixel_dev->write_registers(goal_pos,(unsigned char *)cmd,4); + this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -1097,10 +1158,10 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed) this->set_position_range(-this->info.center_angle,this->info.max_angle-this->info.center_angle); this->mode=angle_ctrl; } - this->dynamixel_dev->read_word_register(current_pos,&pos); + this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos); cmd[0]=this->from_angles(angle+this->to_angles(pos)); cmd[1]=this->from_speeds(speed); - this->dynamixel_dev->write_registers(goal_pos,(unsigned char *)cmd,4); + this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -1130,7 +1191,7 @@ void CDynamixelMotor::move_torque(double torque_ratio) if(torque_ratio<0.0) torque+=0x0400; torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF; - this->dynamixel_dev->write_word_register(goal_speed,torque); + this->dynamixel_dev->write_word_register(this->registers[goal_speed],torque); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -1154,11 +1215,11 @@ void CDynamixelMotor::stop(void) try{ if(this->mode==angle_ctrl) { - this->dynamixel_dev->read_word_register(current_pos,¤t_position); - this->dynamixel_dev->write_word_register(goal_pos,current_position); + this->dynamixel_dev->read_word_register(this->registers[current_pos],¤t_position); + this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position); } else - this->dynamixel_dev->write_word_register(goal_speed,0); + this->dynamixel_dev->write_word_register(this->registers[goal_speed],0); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms) @@ -1181,7 +1242,7 @@ double CDynamixelMotor::get_current_angle(void) else { try{ - this->dynamixel_dev->read_word_register(current_pos,&data); + this->dynamixel_dev->read_word_register(this->registers[current_pos],&data); current_position = this->to_angles(data); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1207,7 +1268,7 @@ double CDynamixelMotor::get_current_speed(void) else { try{ - this->dynamixel_dev->read_word_register(current_speed,&data); + this->dynamixel_dev->read_word_register(this->registers[current_speed],&data); c_speed = this->to_speeds(data); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1233,7 +1294,7 @@ double CDynamixelMotor::get_current_temperature(void) else { try{ - this->dynamixel_dev->read_byte_register(current_temp,&data); + this->dynamixel_dev->read_byte_register(this->registers[current_temp],&data); c_temp = (double)data; }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1259,7 +1320,7 @@ double CDynamixelMotor::get_current_voltage(void) else { try{ - this->dynamixel_dev->read_byte_register(current_voltage,&data); + this->dynamixel_dev->read_byte_register(this->registers[current_voltage],&data); c_voltage = (double)data/10; }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1285,7 +1346,7 @@ double CDynamixelMotor::get_current_effort(void) else { try{ - this->dynamixel_dev->read_byte_register(current_load,&data); + this->dynamixel_dev->read_byte_register(this->registers[current_load],&data); c_effort = (double)((data&0x3FF)*100.0/1023); if (this->get_current_speed() < 0) { @@ -1302,6 +1363,53 @@ double CDynamixelMotor::get_current_effort(void) return c_effort; } +unsigned short int CDynamixelMotor::get_punch(void) +{ + unsigned short int value; + + if(this->dynamixel_dev==NULL) + { + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured."); + } + else + { + try{ + this->dynamixel_dev->read_word_register(this->registers[punch],&value); + }catch(CDynamixelAlarmException &e){ + /* handle dynamixel exception */ + if(e.get_alarm()&this->alarms) + this->reset_motor(); + throw; + } + } + + return value; +} + +void CDynamixelMotor::set_punch(unsigned short int punch_value) +{ + if(this->dynamixel_dev==NULL) + { + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured."); + } + else + { + try{ + if(punch<0x0020 || punch>0x03FF) + throw CDynamixelMotorException(_HERE_,"Invalid Punch value"); + this->config.punch=punch; + this->dynamixel_dev->write_word_register(this->registers[punch],punch); + }catch(CDynamixelAlarmException &e){ + /* handle dynamixel exception */ + if(e.get_alarm()&this->alarms) + this->reset_motor(); + throw; + } + } +} + CDynamixelMotor::~CDynamixelMotor() { /* stop the motor */ diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h index 3d315201ce00e4af5b476f5c409d1824ba11d721..ba52bca56d8a3439c974aa4ece4a048ec52d6006 100644 --- a/src/dynamixel_motor.h +++ b/src/dynamixel_motor.h @@ -12,49 +12,6 @@ #include <vector> #include <memory> -typedef enum { - // [Access] [Description] - // EEPROM Area - model_number=0x00, // (R) Model Number - firmware_version=0x02, // (R) Version of the Firmware - id=0x03, // (RW) ID of Dynamixel - baudrate=0x04, // (RW) Baud Rate of Dynamixel - return_delay_time=0x05, // (RW) Return Delay Time - cw_angle_limit=0x06, // (RW) Clockwise Angle Limit - ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit - temp_limit=0x0B, // (RW) Internal Temperature Limit - low_voltage_limit=0x0C, // (RW) Lowest Voltage Limit - high_voltage_limit=0x0D, // (RW) Highest Voltage Limit - max_torque=0x0E, // (RW) Maximum Torque - return_level=0x10, // (RW) Status Return Level - alarm_led=0x11, // (RW) LED for Alarm - alarm_shtdwn=0x12, // (RW) Shutdown for Alarm - down_cal=0x14, // (RW) - up_cal=0x16, // (RW) - // RAM Area - torque_en=0x18, // (RW) Torque On/Off - led=0x19, // (RW) LED On/Off - pid_p=0x1A, // (RW) - pid_i=0x1B, // (RW) - pid_d=0x1C, // (RW) - cw_comp_margin=0x1A, // (RW) CW Compliance Margin - ccw_comp_margin=0x1B, // (RW) CCW Compliance Margin - cw_comp_slope=0x1C, // (RW) CW Compliance Slope - ccw_comp_slope=0x1D, // (RW) CCW Compliance Slope - goal_pos=0x1E, // (RW) Goal Position - goal_speed=0x20, // (RW) Moving Speed - torque_limit=0x22, // (RW) Torque Limit - current_pos=0x24, // (R) Current Position - current_speed=0x26, // (R) Current Speed - current_load=0x28, // (R) Current Load - current_voltage=0x2A, // (R) Current Voltage - current_temp=0x2B, // (RW) Current Temperature - reg_inst=0x2C, // (RW) Means if Instruction is registered - moving=0x2E, // (R) Means if there is any movement - lock=0x2F, // (RW) Locking EEPROM - punch=0x30 // (RW) Punch -} reg_id; - typedef enum { input_voltage_error=0x01, @@ -87,7 +44,6 @@ typedef struct unsigned char ccw_compliance_margin; unsigned char cw_compliance_slope; unsigned char ccw_compliance_slope; - unsigned char punch; }TDynamixel_compliance; typedef struct @@ -105,6 +61,7 @@ typedef struct double max_voltage; double min_voltage; double max_torque; + unsigned short int punch; }TDynamixel_config; typedef enum{angle_ctrl=0,torque_ctrl=1} control_mode; @@ -152,7 +109,11 @@ class CDynamixelMotor * */ control_mode mode; - + /** + * \brief + * + */ + unsigned short int *registers; protected: /** * \brief @@ -190,12 +151,12 @@ class CDynamixelMotor * \brief * */ - CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id); + CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id,dyn_version_t version=dyn_version1); /** * \brief * */ - CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id); + CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id,dyn_version_t version=dyn_version1); /** * \brief * @@ -208,6 +169,11 @@ class CDynamixelMotor */ void load_config(TDynamixel_config &config); #ifdef _HAVE_XSD + /** + * \brief + * + */ + static void read_config(std::string &filename,TDynamixel_config &config, TDynamixel_compliance &comp, TDynamixel_pid &pid); /** * \brief * @@ -385,6 +351,16 @@ class CDynamixelMotor * */ double get_current_temperature(void); + /** + * \brief + * + */ + unsigned short int get_punch(void); + /** + * \brief + * + */ + void set_punch(unsigned short int punch_value); /** * \brief * diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp index 8cd9adf0d12a289f4495323cf8bd2ea82ce09fae..c0904f58b228c0d80a75c2b2dba5d0c593909d99 100644 --- a/src/dynamixel_motor_group.cpp +++ b/src/dynamixel_motor_group.cpp @@ -1,4 +1,5 @@ #include "dynamixel_motor_exceptions.h" +#include "dynamixel_registers.h" #include "dynamixelexceptions.h" #include "dynamixelserver.h" #include "eventexceptions.h" @@ -39,7 +40,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id) } } -CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids) +CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version) { unsigned int i=0; @@ -66,7 +67,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id try{ this->dyn_server->config_bus(bus_id,baudrate); for(i=0;i<ids.size();i++) - this->init_motor(ids[i]); + this->init_motor(ids[i],version); if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle) { this->mode=torque_ctrl; @@ -89,7 +90,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id } } -CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids) +CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version) { unsigned int i=0; @@ -111,7 +112,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu try{ this->dyn_server->config_bus(bus_id,baudrate); for(i=0;i<ids.size();i++) - this->init_motor(ids[i]); + this->init_motor(ids[i],version); if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle) { this->mode=torque_ctrl; @@ -178,27 +179,27 @@ void CDynamixelMotorGroup::reset_motor(unsigned int index) unsigned short int maximum_torque; try{ - this->dynamixel_dev[index]->read_word_register(current_pos,¤t_position); + this->dynamixel_dev[index]->read_word_register(this->registers[index][current_pos],¤t_position); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev[index]->write_word_register(goal_pos,current_position); + this->dynamixel_dev[index]->write_word_register(this->registers[index][goal_pos],current_position); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev[index]->read_word_register(max_torque,&maximum_torque); + this->dynamixel_dev[index]->read_word_register(this->registers[index][max_torque],&maximum_torque); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev[index]->write_word_register(torque_limit,maximum_torque); + this->dynamixel_dev[index]->write_word_register(this->registers[index][torque_limit],maximum_torque); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } try{ - this->dynamixel_dev[index]->write_byte_register(led,0x00); + this->dynamixel_dev[index]->write_byte_register(this->registers[index][led],0x00); }catch(CDynamixelAlarmException &e){ /* do nothing - expected exception */ } @@ -216,8 +217,8 @@ void CDynamixelMotorGroup::get_model(unsigned int index) else { try{ - this->dynamixel_dev[index]->read_byte_register(firmware_version,&this->info[index].firmware_ver); - this->dynamixel_dev[index]->read_word_register(model_number,&model); + this->dynamixel_dev[index]->read_byte_register(std_firmware_version,&this->info[index].firmware_ver); + this->dynamixel_dev[index]->read_word_register(std_model_number,&model); switch(model) { case 0x000C: this->info[index].model="AX-12A"; @@ -226,6 +227,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=354.0; this->info[index].encoder_resolution=1023; this->info[index].gear_ratio=254; + this->registers[index]=std_compl_reg; this->info[index].pid_control=false; break; case 0x012C: this->info[index].model="AX-12W"; @@ -234,6 +236,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=2830; this->info[index].encoder_resolution=1023; this->info[index].gear_ratio=32; + this->registers[index]=std_compl_reg; this->info[index].pid_control=false; break; case 0x0012: this->info[index].model="AX-18A"; @@ -242,6 +245,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=582; this->info[index].encoder_resolution=1023; this->info[index].gear_ratio=254; + this->registers[index]=std_compl_reg; this->info[index].pid_control=false; break; case 0x001D: this->info[index].model="MX-28"; @@ -250,6 +254,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=330; this->info[index].encoder_resolution=4095; this->info[index].gear_ratio=193; + this->registers[index]=std_pid_reg; this->info[index].pid_control=true; break; case 0x0018: this->info[index].model="RX-24F"; @@ -258,6 +263,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=756; this->info[index].encoder_resolution=1023; this->info[index].gear_ratio=193; + this->registers[index]=std_compl_reg; this->info[index].pid_control=false; break; case 0x001C: this->info[index].model="RX-28"; @@ -266,6 +272,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=402; this->info[index].encoder_resolution=1023; this->info[index].gear_ratio=193; + this->registers[index]=std_compl_reg; this->info[index].pid_control=false; break; case 0x0136: this->info[index].model="MX-64"; @@ -274,6 +281,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=378; this->info[index].encoder_resolution=4095; this->info[index].gear_ratio=200; + this->registers[index]=std_pid_reg; this->info[index].pid_control=true; break; case 0x0040: this->info[index].model="Rx-64"; @@ -282,6 +290,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=294; this->info[index].encoder_resolution=1023; this->info[index].gear_ratio=200; + this->registers[index]=std_compl_reg; this->info[index].pid_control=false; break; case 0x0140: this->info[index].model="MX-106"; @@ -290,6 +299,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=270; this->info[index].encoder_resolution=4095; this->info[index].gear_ratio=225; + this->registers[index]=std_pid_reg; this->info[index].pid_control=true; break; case 0x006B: this->info[index].model="Ex-106+"; @@ -298,7 +308,17 @@ void CDynamixelMotorGroup::get_model(unsigned int index) this->info[index].max_speed=414; this->info[index].encoder_resolution=4095; this->info[index].gear_ratio=184; + this->registers[index]=std_compl_reg; this->info[index].pid_control=false; + break; + case 0x015E: this->info[index].model="XL_320"; + this->info[index].max_angle=300.0; + this->info[index].center_angle=150; + this->info[index].max_speed=684; + this->info[index].encoder_resolution=1023; + this->info[index].gear_ratio=238; + this->registers[index]=xl_reg; + this->info[index].pid_control=true; break; default: this->info[index].model="unknown"; break; @@ -335,8 +355,10 @@ void CDynamixelMotorGroup::set_position_range(unsigned int index,double min, dou try{ this->config[index].max_angle=max; this->config[index].min_angle=min; - this->dynamixel_dev[index]->write_word_register(ccw_angle_limit,max_pos); - this->dynamixel_dev[index]->write_word_register(cw_angle_limit,min_pos); + this->dynamixel_dev[index]->write_word_register(this->registers[index][ccw_angle_limit],max_pos); + usleep(10000); + this->dynamixel_dev[index]->write_word_register(this->registers[index][cw_angle_limit],min_pos); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -359,9 +381,9 @@ void CDynamixelMotorGroup::get_position_range(unsigned int index,double *min, do else { try{ - this->dynamixel_dev[index]->read_word_register(ccw_angle_limit,&angle_limit); + this->dynamixel_dev[index]->read_word_register(this->registers[index][ccw_angle_limit],&angle_limit); (*max)=this->to_angles(index,angle_limit); - this->dynamixel_dev[index]->read_word_register(cw_angle_limit,&angle_limit); + this->dynamixel_dev[index]->read_word_register(this->registers[index][cw_angle_limit],&angle_limit); (*min)=this->to_angles(index,angle_limit); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -390,7 +412,8 @@ void CDynamixelMotorGroup::set_temperature_limit(unsigned int index,int limit) { try{ this->config[index].max_temperature=limit; - this->dynamixel_dev[index]->write_byte_register(temp_limit,(unsigned char)limit); + this->dynamixel_dev[index]->write_byte_register(this->registers[index][temp_limit],(unsigned char)limit); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -413,7 +436,7 @@ int CDynamixelMotorGroup::get_temperature_limit(unsigned int index) else { try{ - this->dynamixel_dev[index]->read_byte_register(temp_limit,&limit); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][temp_limit],&limit); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -455,8 +478,10 @@ void CDynamixelMotorGroup::set_voltage_limits(unsigned int index,double min, dou this->config[index].max_voltage=max; min_voltage=min*10; max_voltage=max*10; - this->dynamixel_dev[index]->write_byte_register(low_voltage_limit,min_voltage); - this->dynamixel_dev[index]->write_byte_register(high_voltage_limit,max_voltage); + this->dynamixel_dev[index]->write_byte_register(this->registers[index][low_voltage_limit],min_voltage); + usleep(10000); + this->dynamixel_dev[index]->write_byte_register(this->registers[index][high_voltage_limit],max_voltage); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -480,8 +505,8 @@ void CDynamixelMotorGroup::get_voltage_limits(unsigned int index,double *min, do else { try{ - this->dynamixel_dev[index]->read_byte_register(low_voltage_limit,&min_voltage); - this->dynamixel_dev[index]->read_byte_register(high_voltage_limit,&max_voltage); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][low_voltage_limit],&min_voltage); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][high_voltage_limit],&max_voltage); *min=((double)min_voltage/10.0); *max=((double)max_voltage/10.0); }catch(CDynamixelAlarmException &e){ @@ -505,7 +530,7 @@ unsigned char CDynamixelMotorGroup::get_turn_off_alarms(unsigned int index) else { try{ - this->dynamixel_dev[index]->read_byte_register(alarm_shtdwn,&shutdown_alarms); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][alarm_shtdwn],&shutdown_alarms); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -537,7 +562,8 @@ void CDynamixelMotorGroup::set_max_torque(unsigned int index,double torque_ratio load=torque_ratio*1023/100.0; try{ this->config[index].max_torque=torque_ratio; - this->dynamixel_dev[index]->write_word_register(max_torque,load); + this->dynamixel_dev[index]->write_word_register(this->registers[index][max_torque],load); + usleep(10000); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -567,7 +593,7 @@ void CDynamixelMotorGroup::set_limit_torque(unsigned int index,double torque_rat { load=torque_ratio*1023/100.0; try{ - this->dynamixel_dev[index]->write_word_register(torque_limit,load); + this->dynamixel_dev[index]->write_word_register(this->registers[index][torque_limit],load); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -591,7 +617,7 @@ double CDynamixelMotorGroup::get_max_torque(unsigned int index) else { try{ - this->dynamixel_dev[index]->read_word_register(max_torque,&load); + this->dynamixel_dev[index]->read_word_register(this->registers[index][max_torque],&load); torque=(load&0x3FF)*100.0/1023; if(load>0x3FF) torque=-1*torque; @@ -618,11 +644,10 @@ void CDynamixelMotorGroup::get_compliance_control(unsigned int index,TDynamixel_ if(!this->info[index].pid_control) { try{ - this->dynamixel_dev[index]->read_byte_register(cw_comp_margin,&config.cw_compliance_margin); - this->dynamixel_dev[index]->read_byte_register(ccw_comp_margin,&config.ccw_compliance_margin); - this->dynamixel_dev[index]->read_byte_register(cw_comp_slope,&config.cw_compliance_slope); - this->dynamixel_dev[index]->read_byte_register(ccw_comp_slope,&config.ccw_compliance_slope); - this->dynamixel_dev[index]->read_byte_register(punch,&config.punch); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_margin],&config.cw_compliance_margin); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_margin],&config.ccw_compliance_margin); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_slope],&config.cw_compliance_slope); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_slope],&config.ccw_compliance_slope); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -645,9 +670,9 @@ void CDynamixelMotorGroup::get_pid_control(unsigned int index,TDynamixel_pid &co if(this->info[index].pid_control) { try{ - this->dynamixel_dev[index]->read_byte_register(pid_p,&config.p); - this->dynamixel_dev[index]->read_byte_register(pid_i,&config.i); - this->dynamixel_dev[index]->read_byte_register(pid_d,&config.d); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_p],&config.p); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_i],&config.i); + this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_d],&config.d); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[index]) @@ -744,7 +769,7 @@ void *CDynamixelMotorGroup::sequence_thread(void *param) pthread_exit(NULL); } -void CDynamixelMotorGroup::init_motor(unsigned int id) +void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version) { TDynamixel_info info; TDynamixel_config config; @@ -754,10 +779,11 @@ void CDynamixelMotorGroup::init_motor(unsigned int id) info.baudrate=this->dyn_server->get_baudrate(); info.bus_id=this->dyn_server->get_bus_serial(); - dynamixel_dev=this->dyn_server->get_device(id); + dynamixel_dev=this->dyn_server->get_device(id,version); this->dynamixel_dev.push_back(dynamixel_dev); info.id=id; this->info.push_back(info); + this->registers.push_back(NULL); this->get_model(this->servo_id.size()); this->get_position_range(this->servo_id.size(),&config.min_angle,&config.max_angle); config.max_temperature=this->get_temperature_limit(this->servo_id.size()); @@ -817,6 +843,7 @@ void CDynamixelMotorGroup::clear(void) this->dynamixel_dev.clear(); this->compliance.clear(); this->pid.clear(); + this->registers.clear(); this->config.clear(); this->info.clear(); this->alarms.clear(); @@ -876,6 +903,7 @@ int CDynamixelMotorGroup::load_motion(unsigned int step_id) void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config) { std::vector<TDynamixel_compliance> compliance; + std::vector<unsigned short int> punch; std::vector<TDynamixel_pid> pid; unsigned int i=0; @@ -883,10 +911,11 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config) try{ this->dyn_server->config_bus(config.bus_id,config.baudrate); pid.resize(config.id.size()); + punch.resize(config.id.size()); compliance.resize(config.id.size()); for(i=0;i<config.id.size();i++) { - this->init_motor(config.id[i]); + this->init_motor(config.id[i],config.dyn_version); // load the configuration file this->set_position_range(i,config.dyn_config[i].min_angle,config.dyn_config[i].max_angle); this->set_temperature_limit(i,config.dyn_config[i].max_temperature); @@ -903,8 +932,8 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config) compliance[i].ccw_compliance_margin=config.compliance_control[i].ccw_compliance_margin; compliance[i].cw_compliance_slope=config.compliance_control[i].cw_compliance_slope; compliance[i].ccw_compliance_slope=config.compliance_control[i].ccw_compliance_slope; - compliance[i].punch=config.compliance_control[i].punch; } + punch[i]=config.dyn_config[i].punch; this->set_max_torque(i,config.dyn_config[i].max_torque); this->set_limit_torque(i,config.dyn_config[i].max_torque); } @@ -920,6 +949,7 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config) for(i=1;i<this->servo_id.size();i++) this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle); } + this->set_punch(punch); this->set_pid_control(pid); this->set_compliance_control(compliance); }catch(CException &e){ @@ -929,10 +959,51 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config) } #ifdef _HAVE_XSD +TDynamixelGroup_config CDynamixelMotorGroup::read_config(std::string &filename) +{ + dyn_motor_group_config_t::dyn_motor_config_iterator iterator; + TDynamixelGroup_config config; + std::string full_path,path; + struct stat buffer; + size_t found; + unsigned int i=0; + + if(stat(filename.c_str(),&buffer)==0) + { + // try to open the specified file + try{ + std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate)); + config.bus_id=cfg->bus_id(); + config.baudrate=cfg->baudrate(); + config.dyn_config.resize(cfg->dyn_motor_config().size()); + config.pid_control.resize(cfg->dyn_motor_config().size()); + config.compliance_control.resize(cfg->dyn_motor_config().size()); + found=filename.find_last_of("/"); + path=filename.substr(0,found+1); + for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++) + { + config.bus_id[i]=iterator->id(); + full_path=path+iterator->config_file(); + CDynamixelMotor::read_config(full_path,config.dyn_config[i],config.compliance_control[i],config.pid_control[i]); + } + }catch (const xml_schema::exception& e){ + std::ostringstream os; + os << e; + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,os.str()); + } + } + else + throw CDynamixelMotorException(_HERE_,"The configuration file does not exist"); + + return config; +} + void CDynamixelMotorGroup::load_config(std::string &filename) { dyn_motor_group_config_t::dyn_motor_config_iterator iterator; std::vector<TDynamixel_compliance> compliance; + std::vector<unsigned short int> punch; std::vector<TDynamixel_pid> pid; std::string full_path,path; struct stat buffer; @@ -948,12 +1019,16 @@ void CDynamixelMotorGroup::load_config(std::string &filename) std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate)); this->dyn_server->config_bus(cfg->bus_id(),cfg->baudrate()); pid.resize(cfg->dyn_motor_config().size()); + punch.resize(cfg->dyn_motor_config().size()); compliance.resize(cfg->dyn_motor_config().size()); found=filename.find_last_of("/"); path=filename.substr(0,found+1); for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++) { - this->init_motor(iterator->id()); + if(cfg->dynamixel_version().present()) + this->init_motor(iterator->id(),(dyn_version_t)cfg->dynamixel_version().get()); + else + this->init_motor(iterator->id()); // load the configuration file full_path=path+iterator->config_file(); std::auto_ptr<dynamixel_motor_config_t> motor(dynamixel_motor_config(full_path.c_str(),xml_schema::flags::dont_validate)); @@ -972,8 +1047,8 @@ void CDynamixelMotorGroup::load_config(std::string &filename) compliance[i].ccw_compliance_margin=motor->ccw_comp_margin(); compliance[i].cw_compliance_slope=motor->cw_comp_slope(); compliance[i].ccw_compliance_slope=motor->ccw_comp_slope(); - compliance[i].punch=motor->punch(); } + punch[i]=motor->punch(); this->set_max_torque(i,motor->max_torque()); this->set_limit_torque(i,motor->max_torque()); } @@ -989,6 +1064,7 @@ void CDynamixelMotorGroup::load_config(std::string &filename) for(i=1;i<this->servo_id.size();i++) this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle); } + this->set_punch(punch); this->set_pid_control(pid); this->set_compliance_control(compliance); }catch (const xml_schema::exception& e){ @@ -1161,21 +1237,15 @@ void CDynamixelMotorGroup::set_compliance_control(std::vector<TDynamixel_complia /* handle exceptions */ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope."); } - if(config[i].punch<0 || config[i].punch>1023) - { - /* handle exceptions */ - throw CDynamixelMotorException(_HERE_,"Invalid punch value."); - } try{ this->compliance[i].cw_compliance_margin=config[i].cw_compliance_margin; this->compliance[i].ccw_compliance_margin=config[i].ccw_compliance_margin; this->compliance[i].cw_compliance_slope=config[i].cw_compliance_slope; this->compliance[i].ccw_compliance_slope=config[i].ccw_compliance_slope; - this->dynamixel_dev[i]->write_byte_register(cw_comp_margin,config[i].cw_compliance_margin); - this->dynamixel_dev[i]->write_byte_register(ccw_comp_margin,config[i].ccw_compliance_margin); - this->dynamixel_dev[i]->write_byte_register(cw_comp_slope,config[i].cw_compliance_slope); - this->dynamixel_dev[i]->write_byte_register(ccw_comp_slope,config[i].ccw_compliance_slope); - this->dynamixel_dev[i]->write_word_register(punch,config[i].punch); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][cw_comp_margin],config[i].cw_compliance_margin); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_margin],config[i].ccw_compliance_margin); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][cw_comp_slope],config[i].cw_compliance_slope); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_slope],config[i].ccw_compliance_slope); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[i]) @@ -1205,9 +1275,9 @@ void CDynamixelMotorGroup::set_pid_control(std::vector<TDynamixel_pid> &config) this->pid[i].p=config[i].p; this->pid[i].i=config[i].i; this->pid[i].d=config[i].d; - this->dynamixel_dev[i]->write_byte_register(pid_p,config[i].p); - this->dynamixel_dev[i]->write_byte_register(pid_i,config[i].i); - this->dynamixel_dev[i]->write_byte_register(pid_d,config[i].d); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_p],config[i].p); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_i],config[i].i); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_d],config[i].d); } } } @@ -1227,7 +1297,7 @@ void CDynamixelMotorGroup::enable(void) else { try{ - this->dynamixel_dev[i]->write_byte_register(torque_en,1); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en],1); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[i]) @@ -1252,7 +1322,7 @@ void CDynamixelMotorGroup::disable(void) else { try{ - this->dynamixel_dev[i]->write_byte_register(torque_en,0); + this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en],0); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[i]) @@ -1287,7 +1357,7 @@ void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std:: data[i][2]=((int)(cmd[1])%256); data[i][3]=(int)(cmd[1]/256); } - this->dyn_server->write_sync(this->servo_id, goal_pos, data); + this->dyn_server->write_sync(this->servo_id, this->registers[0][goal_pos], data); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[i]) @@ -1319,7 +1389,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std:: } else { - this->dynamixel_dev[i]->read_word_register(current_pos,&pos); + this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&pos); data[i].resize(4); cmd[0]=this->from_angles(i,angles[i]+this->to_angles(i,pos)); cmd[1]=this->from_speeds(i,speeds[i]); @@ -1329,7 +1399,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std:: data[i][3]=(int)(cmd[1]/256); } } - this->dyn_server->write_sync(this->servo_id,goal_pos,data); + this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_pos],data); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[i]) @@ -1370,7 +1440,7 @@ void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios) data[i][1]=(int)(torque/256); } } - this->dyn_server->write_sync(this->servo_id,goal_speed,data); + this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_speed],data); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ if(e.get_alarm()&this->alarms[i]) @@ -1396,11 +1466,11 @@ void CDynamixelMotorGroup::stop(void) { if(this->mode==angle_ctrl) { - this->dynamixel_dev[i]->read_word_register(current_pos,¤t_position); - this->dynamixel_dev[i]->write_word_register(goal_pos,current_position); + this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],¤t_position); + this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_pos],current_position); } else - this->dynamixel_dev[i]->write_word_register(goal_speed,0); + this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_speed],0); } } }catch(CDynamixelAlarmException &e){ @@ -1427,7 +1497,7 @@ void CDynamixelMotorGroup::get_current_angle(std::vector<double> &angles) else { try{ - this->dynamixel_dev[i]->read_word_register(current_pos,&data); + this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&data); angles[i] = this->to_angles(i, data); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1455,7 +1525,7 @@ void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds) else { try{ - this->dynamixel_dev[i]->read_word_register(current_speed,&data); + this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&data); speeds[i] = this->to_speeds(i, data); }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1484,8 +1554,8 @@ void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts) else { try{ - this->dynamixel_dev[i]->read_word_register(current_load,&c_effort); - this->dynamixel_dev[i]->read_word_register(current_speed,&c_speed); + this->dynamixel_dev[i]->read_word_register(this->registers[i][current_load],&c_effort); + this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&c_speed); efforts[i] = (double)((c_effort&0x3FF)*100.0/1023); if (this->to_speeds(i, c_speed) < 0) { @@ -1517,7 +1587,7 @@ void CDynamixelMotorGroup::get_current_voltage(std::vector<double> &voltages) else { try{ - this->dynamixel_dev[i]->read_byte_register(current_voltage,&data); + this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_voltage],&data); voltages[i] = (double)data/10; }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1545,7 +1615,7 @@ void CDynamixelMotorGroup::get_current_temperature(std::vector<double> &temperat else { try{ - this->dynamixel_dev[i]->read_byte_register(current_temp,&data); + this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_temp],&data); temperatures[i] = (double)data; }catch(CDynamixelAlarmException &e){ /* handle dynamixel exception */ @@ -1562,6 +1632,58 @@ unsigned int CDynamixelMotorGroup::get_num_motors(void) return this->servo_id.size(); } +void CDynamixelMotorGroup::get_punch(std::vector<unsigned short int> &punches) +{ + punches.resize(this->servo_id.size()); + + for(unsigned int i=0; i<this->servo_id.size(); i++) + { + if(this->dynamixel_dev[i]==NULL) + { + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured."); + } + else + { + try{ + this->dynamixel_dev[i]->read_word_register(this->registers[i][punch],&punches[i]); + }catch(CDynamixelAlarmException &e){ + /* handle dynamixel exception */ + if(e.get_alarm()&this->alarms[i]) + this->reset_motor(i); + throw; + } + } + } +} + +void CDynamixelMotorGroup::set_punch(std::vector<unsigned short int> &punches) +{ + punches.resize(this->servo_id.size()); + + for(unsigned int i=0; i<this->servo_id.size(); i++) + { + if(this->dynamixel_dev[i]==NULL) + { + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured."); + } + else + { + try{ + if(punches[i]<0x0020 || punches[i]>0x03FF) + throw CDynamixelMotorException(_HERE_,"Invalid punch value"); + this->dynamixel_dev[i]->write_word_register(this->registers[i][punch],punches[i]); + }catch(CDynamixelAlarmException &e){ + /* handle dynamixel exception */ + if(e.get_alarm()&this->alarms[i]) + this->reset_motor(i); + throw; + } + } + } +} + TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void) { return this->sequence_state; diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h index 633f62082aad0c1f1fa04531e65d0ba8371fed45..0d9e5ad97b18dd9eb36472118eaa6f03cf23574b 100644 --- a/src/dynamixel_motor_group.h +++ b/src/dynamixel_motor_group.h @@ -12,6 +12,7 @@ typedef struct std::string bus_id; unsigned int baudrate; std::vector<unsigned char> id; + dyn_version_t dyn_version; std::vector<TDynamixel_config> dyn_config; std::vector<TDynamixel_pid> pid_control; std::vector<TDynamixel_compliance> compliance_control; @@ -125,6 +126,11 @@ class CDynamixelMotorGroup * */ std::vector<unsigned char> servo_id; + /** + * \brief + * + */ + std::vector<unsigned short int *>registers; protected: /** * \brief @@ -251,7 +257,7 @@ class CDynamixelMotorGroup * \brief * */ - void init_motor(unsigned int id); + void init_motor(unsigned int id,dyn_version_t version=dyn_version1); /** * \brief * @@ -288,12 +294,12 @@ class CDynamixelMotorGroup * \brief * */ - CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids); + CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version=dyn_version1); /** * \brief * */ - CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids); + CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version=dyn_version1); /* configuration functions */ /** * \brief @@ -301,6 +307,11 @@ class CDynamixelMotorGroup */ void load_config(TDynamixelGroup_config &config); #ifdef _HAVE_XSD + /** + * \brief + * + */ + static TDynamixelGroup_config read_config(std::string &filename); /** * \brief * @@ -382,6 +393,16 @@ class CDynamixelMotorGroup * */ unsigned int get_num_motors(void); + /** + * \brief + * + */ + void get_punch(std::vector<unsigned short int> &punches); + /** + * \brief + * + */ + void set_punch(std::vector<unsigned short int> &punches); /* motion sequence public functions */ /** * \brief diff --git a/src/dynamixel_registers.cpp b/src/dynamixel_registers.cpp new file mode 100644 index 0000000000000000000000000000000000000000..58904ec06956226413f8d9611a6dd4df606f1570 --- /dev/null +++ b/src/dynamixel_registers.cpp @@ -0,0 +1,125 @@ +#include "dynamixel_registers.h" + +unsigned short int std_compl_reg[40]={std_model_number, + std_firmware_version, + std_id, + std_baudrate, + std_return_delay_time, + std_cw_angle_limit, + std_ccw_angle_limit, + (unsigned short int)-1, + std_temp_limit, + std_low_voltage_limit, + std_high_voltage_limit, + std_max_torque, + std_return_level, + std_alarm_led, + std_alarm_shtdwn, + std_down_cal, + std_up_cal, + std_torque_en, + std_led, + (unsigned short int)-1, + (unsigned short int)-1, + (unsigned short int)-1, + std_cw_comp_margin, + std_ccw_comp_margin, + std_cw_comp_slope, + std_ccw_comp_slope, + std_goal_pos, + std_goal_speed, + std_torque_limit, + (unsigned short int)-1, + std_current_pos, + std_current_speed, + std_current_load, + std_current_voltage, + std_current_temp, + std_reg_inst, + std_moving, + std_lock, + (unsigned short int)-1, + std_punch}; + +unsigned short int std_pid_reg[40]={std_model_number, + std_firmware_version, + std_id, + std_baudrate, + std_return_delay_time, + std_cw_angle_limit, + std_ccw_angle_limit, + (unsigned short int)-1, + std_temp_limit, + std_low_voltage_limit, + std_high_voltage_limit, + std_max_torque, + std_return_level, + std_alarm_led, + std_alarm_shtdwn, + std_down_cal, + std_up_cal, + std_torque_en, + std_led, + std_pid_p, + std_pid_i, + std_pid_d, + (unsigned short int)-1, + (unsigned short int)-1, + (unsigned short int)-1, + (unsigned short int)-1, + std_goal_pos, + std_goal_speed, + std_torque_limit, + (unsigned short int)-1, + std_current_pos, + std_current_speed, + std_current_load, + std_current_voltage, + std_current_temp, + std_reg_inst, + std_moving, + std_lock, + (unsigned short int)-1, + std_punch}; + +unsigned short int xl_reg[40]={xl_model_number, + xl_firmware_version, + xl_id, + xl_baudrate, + xl_return_delay_time, + xl_cw_angle_limit, + xl_ccw_angle_limit, + xl_control_mode, + xl_temp_limit, + xl_low_voltage_limit, + xl_high_voltage_limit, + xl_max_torque, + xl_return_level, + (unsigned short int)-1, + xl_alarm_shtdwn, + xl_down_cal, + xl_up_cal, + xl_torque_en, + xl_led, + xl_pid_p, + xl_pid_i, + xl_pid_d, + (unsigned short int)-1, + (unsigned short int)-1, + (unsigned short int)-1, + (unsigned short int)-1, + xl_goal_pos, + xl_goal_speed, + (unsigned short int)-1, + xl_goal_torque, + xl_current_pos, + xl_current_speed, + xl_current_load, + xl_current_voltage, + xl_current_temp, + xl_reg_inst, + xl_moving, + (unsigned short int)-1, + xl_hardware_error, + xl_punch}; + diff --git a/src/dynamixel_registers.h b/src/dynamixel_registers.h new file mode 100644 index 0000000000000000000000000000000000000000..2bfbe13d4dfd6b2fcbab947ace5e4b8ecb88e0aa --- /dev/null +++ b/src/dynamixel_registers.h @@ -0,0 +1,138 @@ +#ifndef _DYNAMIXEL_REGISTERS_H +#define _DYNAMIXEL_REGISTERS_H + +extern unsigned short int std_compl_reg[40]; +extern unsigned short int std_pid_reg[40]; +extern unsigned short int xl_reg[40]; + +typedef enum { + // [Access] [Description] + // EEPROM Area + model_number=0, // (R) Model Number + firmware_version, // (R) Version of the Firmware + id, // (RW) ID of Dynamixel + baudrate, // (RW) Baud Rate of Dynamixel + return_delay_time, // (RW) Return Delay Time + cw_angle_limit, // (RW) Clockwise Angle Limit + ccw_angle_limit, // (RW) Counterclockwise Angle Limit + dyn_control_mode, // (RW) Joint or wheel mode + temp_limit, // (RW) Internal Temperature Limit + low_voltage_limit, // (RW) Lowest Voltage Limit + high_voltage_limit, // (RW) Highest Voltage Limit + max_torque, // (RW) Maximum Torque + return_level, // (RW) Status Return Level + alarm_led, // (RW) LED for Alarm + alarm_shtdwn, // (RW) Shutdown for Alarm + down_cal, // (RW) + up_cal, // (RW) + // RAM Area + torque_en, // (RW) Torque On/Off + led, // (RW) LED On/Off + pid_p, // (RW) + pid_i, // (RW) + pid_d, // (RW) + cw_comp_margin, // (RW) CW Compliance Margin + ccw_comp_margin, // (RW) CCW Compliance Margin + cw_comp_slope, // (RW) CW Compliance Slope + ccw_comp_slope, // (RW) CCW Compliance Slope + goal_pos, // (RW) Goal Position + goal_speed, // (RW) Moving Speed + torque_limit, // (RW) Torque Limit + goal_torque, // (RW) Torque Limit + current_pos, // (R) Current Position + current_speed, // (R) Current Speed + current_load, // (R) Current Load + current_voltage, // (R) Current Voltage + current_temp, // (RW) Current Temperature + reg_inst, // (RW) Means if Instruction is registered + moving, // (R) Means if there is any movement + lock, // (RW) Locking EEPROM + hardware_error, // (R) Means if there is any movement + punch // (RW) Punch +} reg_id; + +typedef enum { + // [Access] [Description] + // EEPROM Area + std_model_number=0x00, // (R) Model Number + std_firmware_version=0x02, // (R) Version of the Firmware + std_id=0x03, // (RW) ID of Dynamixel + std_baudrate=0x04, // (RW) Baud Rate of Dynamixel + std_return_delay_time=0x05, // (RW) Return Delay Time + std_cw_angle_limit=0x06, // (RW) Clockwise Angle Limit + std_ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit + std_temp_limit=0x0B, // (RW) Internal Temperature Limit + std_low_voltage_limit=0x0C, // (RW) Lowest Voltage Limit + std_high_voltage_limit=0x0D, // (RW) Highest Voltage Limit + std_max_torque=0x0E, // (RW) Maximum Torque + std_return_level=0x10, // (RW) Status Return Level + std_alarm_led=0x11, // (RW) LED for Alarm + std_alarm_shtdwn=0x12, // (RW) Shutdown for Alarm + std_down_cal=0x14, // (RW) + std_up_cal=0x16, // (RW) + // RAM Area + std_torque_en=0x18, // (RW) Torque On/Off + std_led=0x19, // (RW) LED On/Off + std_pid_p=0x1A, // (RW) + std_pid_i=0x1B, // (RW) + std_pid_d=0x1C, // (RW) + std_cw_comp_margin=0x1A, // (RW) CW Compliance Margin + std_ccw_comp_margin=0x1B, // (RW) CCW Compliance Margin + std_cw_comp_slope=0x1C, // (RW) CW Compliance Slope + std_ccw_comp_slope=0x1D, // (RW) CCW Compliance Slope + std_goal_pos=0x1E, // (RW) Goal Position + std_goal_speed=0x20, // (RW) Moving Speed + std_torque_limit=0x22, // (RW) Torque Limit + std_current_pos=0x24, // (R) Current Position + std_current_speed=0x26, // (R) Current Speed + std_current_load=0x28, // (R) Current Load + std_current_voltage=0x2A, // (R) Current Voltage + std_current_temp=0x2B, // (RW) Current Temperature + std_reg_inst=0x2C, // (RW) Means if Instruction is registered + std_moving=0x2E, // (R) Means if there is any movement + std_lock=0x2F, // (RW) Locking EEPROM + std_punch=0x30 // (RW) Punch +} std_reg_id; + +typedef enum { + // [Access] [Description] + // EEPROM Area + xl_model_number=0x00, // (R) Model Number + xl_firmware_version=0x02, // (R) Version of the Firmware + xl_id=0x03, // (RW) ID of Dynamixel + xl_baudrate=0x04, // (RW) Baud Rate of Dynamixel + xl_return_delay_time=0x05, // (RW) Return Delay Time + xl_cw_angle_limit=0x06, // (RW) Clockwise Angle Limit + xl_ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit + xl_control_mode=0x0B, // (RW) Joint or wheel mode + xl_temp_limit=0x0C, // (RW) Internal Temperature Limit + xl_low_voltage_limit=0x0D, // (RW) Lowest Voltage Limit + xl_high_voltage_limit=0x0E, // (RW) Highest Voltage Limit + xl_max_torque=0x0F, // (RW) Maximum Torque + xl_return_level=0x11, // (RW) Status Return Level + xl_alarm_shtdwn=0x12, // (RW) Shutdown for Alarm + xl_down_cal=0x14, // (RW) + xl_up_cal=0x16, // (RW) + // RAM Area + xl_torque_en=0x18, // (RW) Torque On/Off + xl_led=0x19, // (RW) LED On/Off + xl_pid_d=0x1B, // (RW) + xl_pid_i=0x1C, // (RW) + xl_pid_p=0x1D, // (RW) + xl_goal_pos=0x1E, // (RW) Goal Position + xl_goal_speed=0x20, // (RW) Moving Speed + xl_goal_torque=0x23, // (RW) Torque Limit + xl_current_pos=0x25, // (R) Current Position + xl_current_speed=0x27, // (R) Current Speed + xl_current_load=0x29, // (R) Current Load + xl_current_voltage=0x2D, // (R) Current Voltage + xl_current_temp=0x2E, // (RW) Current Temperature + xl_reg_inst=0x2F, // (RW) Means if Instruction is registered + xl_moving=0x31, // (R) Means if there is any movement + xl_hardware_error=0x32, // (R) Means if there is any movement + xl_punch=0x33 // (RW) Punch +} xl_reg_id; + + +#endif + diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index d9cd6210087bde801f80bb1799652b5129eb4830..fcfb0ccaa1376723d9a757983d2ab89c62b2b549 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -15,3 +15,9 @@ ADD_EXECUTABLE(test_dynamixel_motion_seq test_dynamixel_motion_seq.cpp) # edit the following line to add the necessary libraries TARGET_LINK_LIBRARIES(test_dynamixel_motion_seq dynamixel_motor_cont ${comm_LIBRARY}) + +# edit the following line to add the source code for the example and the name of the executable +ADD_EXECUTABLE(motor_testbench motor_testbench.cpp) + +# edit the following line to add the necessary libraries +TARGET_LINK_LIBRARIES(motor_testbench dynamixel_motor_cont ${comm_LIBRARY}) diff --git a/src/examples/motor_testbench.cpp b/src/examples/motor_testbench.cpp new file mode 100755 index 0000000000000000000000000000000000000000..7f238c3ab6b4aea2520369eb7e35c3585eecf081 --- /dev/null +++ b/src/examples/motor_testbench.cpp @@ -0,0 +1,111 @@ +#include "dynamixelexceptions.h" +#include "dynamixelserver.h" +#include "exceptions.h" +#include "dynamixel_motor.h" +#include <iostream> +#include "ctime.h" + +std::string cont_name="XL_320"; +std::string config_file="../src/xml/dyn_config.xml"; + +int main(int argc, char *argv[]) +{ + CDynamixelServer *dyn_server=CDynamixelServer::instance(); + CDynamixelMotor *cont; + std::string serial="A400gavq"; + TDynamixel_info info; + double angle; + double amplitude,freq; + CTime time; + TDynamixel_pid pid; + int i; + + try + { + if(argc!=3 && argc!=6) + { + std::cout << "Invalid number of arguments." << std::endl; + std::cout << " motor_testbench <amplitude> <speed>" << std::endl; + } + else if(argc==3) + { + amplitude=atoi(argv[1]); + freq=atoi(argv[2]); + } + else + { + amplitude=atoi(argv[1]); + freq=atoi(argv[2]); + pid.p=atoi(argv[3]); + pid.i=atoi(argv[4]); + pid.d=atoi(argv[5]); + } + if(dyn_server->get_num_buses()>0) + { + cont = new CDynamixelMotor(cont_name, serial, 1000000, 23,dyn_version2); + if(argc==3) + cont->load_config(config_file); + else + cont->set_pid_control(pid); + +/* cont->get_servo_info(info); + std::cout << "[INFO]: Servo model: " << info.model << std::endl; + std::cout << "[INFO]: Firmware version: " << (int)info.firmware_ver << std::endl; + std::cout << "[INFO]: Gear ratio: " << info.gear_ratio << std::endl; + std::cout << "[INFO]: Encoder resolution: " << info.encoder_resolution << " counts" << std::endl; + if(info.pid_control) + { + std::cout << "[INFO]: PID control capable" << std::endl; + pid.p=10; + pid.i=20; + pid.d=30; + cont->set_pid_control(pid); + cont->get_pid_control(pid); + std::cout << "[INFO]: PID control: (" << (int)pid.p << "," << (int)pid.i << "," << (int)pid.d << ")" << std::endl; + } + else + { + std::cout << "[INFO]: Compliance control capable" << std::endl; + cont->get_compliance_control(compliance); + std::cout << "[INFO]: Compliance margin: (" << std::dec << (int)compliance.cw_compliance_margin << "," << (int)compliance.ccw_compliance_margin << ")" << std::endl; + std::cout << "[INFO]: Compliance slope: (" << std::dec << (int)compliance.cw_compliance_slope << "," << (int)compliance.ccw_compliance_slope << ")" << std::endl; + } + std::cout << "[INFO]: Maximum angle: " << info.max_angle << " degrees" << std::endl; + std::cout << "[INFO]: Center angle: " << info.center_angle << " degrees" << std::endl; + std::cout << "[INFO]: Maximum speed: " << info.max_speed << " degress/s" << std::endl; + std::cout << "[INFO]: Bus identifier: " << info.bus_id << std::endl; + std::cout << "[INFO]: Bus baudrate: " << info.baudrate << " bps" << std::endl; + std::cout << "[INFO]: Servo identifier: " << (int)info.id << std::endl; + cont->get_position_range(&min_angle,&max_angle); + std::cout << "[INFO]: Angle range: (" << min_angle << "," << max_angle << ")" << std::endl; + sd_alarms = cont->get_turn_off_alarms(); + std::cout << "[INFO]: Shutdown alarm flags: " << std::hex << (int)sd_alarms << std::endl; + std::cout << "[INFO]: - Input Voltage Error:\t" << bool(sd_alarms&0x01) << std::endl; + std::cout << "[INFO]: - Angle Limit Error:\t" << bool(sd_alarms&0x02) << std::endl; + std::cout << "[INFO]: - OverHeating Error:\t" << bool(sd_alarms&0x04) << std::endl; + std::cout << "[INFO]: - Range Error:\t\t" << bool(sd_alarms&0x08) << std::endl; + std::cout << "[INFO]: - CheckSum Error:\t" << bool(sd_alarms&0x10) << std::endl; + std::cout << "[INFO]: - Overload Error:\t" << bool(sd_alarms&0x20) << std::endl; + std::cout << "[INFO]: - Instruction Error:\t" << bool(sd_alarms&0x40) << std::endl; + led_alarms=cont->get_led_alarms(); + std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl; + std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl; + cont->turn_led_on(); + sleep(1); + cont->turn_led_off();*/ + for(i=0;i<freq;i++) + { + angle=amplitude*sin(i*2*3.14159/freq); + cont->move_absolute_angle(angle,0); + time.set(); + std::cout << time.getTimeInMicroseconds(); + std::cout << "," << angle; + std::cout << "," << cont->get_current_angle() << std::endl; + usleep(7800); + } + delete cont; + } + }catch(CException &e){ + std::cout << "[Genereal exception]: " << e.what() << std::endl; } + return 0; +} diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp index aaab65f988ce37b46d86714f794d9a9b3a2c3c90..4396ee5b92805c536f991808171dc7f5747c35fc 100755 --- a/src/examples/test_dynamixel_motor.cpp +++ b/src/examples/test_dynamixel_motor.cpp @@ -65,7 +65,7 @@ int main(int argc, char *argv[]) std::cout << "[INFO]: - Instruction Error:\t" << bool(sd_alarms&0x40) << std::endl; led_alarms=cont->get_led_alarms(); std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl; - std::cout << "[INFO]: Punch: " << std::dec << (int)compliance.punch << std::endl; + std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl; cont->turn_led_on(); sleep(1); cont->turn_led_off(); diff --git a/src/xml/dynamixel_motor_group_cfg_file.xsd b/src/xml/dynamixel_motor_group_cfg_file.xsd index 6b416ce9f6eafb7d149af2d534c4ab60652c5c0e..c2664d681dca68d02659ecc4e96b0c9bcd694f03 100755 --- a/src/xml/dynamixel_motor_group_cfg_file.xsd +++ b/src/xml/dynamixel_motor_group_cfg_file.xsd @@ -25,6 +25,8 @@ copyright : not copyrighted - public domain </xsd:element> <xsd:element name="baudrate" type="xsd:int"> </xsd:element> + <xsd:element name="dynamixel_version" type="xsd:int" minOccurs="0"> + </xsd:element> <xsd:element name="dyn_motor_config" type="dyn_motor_config_t" maxOccurs="unbounded"> </xsd:element> </xsd:sequence>