diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 974c13a92de957fafd3e7821b4b9ce74fa39c4ae..05a9da30c8eea3a2d9756db96a85d529ae290fea 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1,7 +1,6 @@
#include "dynamixel_motor_exceptions.h"
#include "dynamixel_registers.h"
#include "dynamixelexceptions.h"
-#include "dynamixelserver.h"
#include "eventexceptions.h"
#include "dynamixel_motor.h"
#include "ftdiserver.h"
@@ -16,7 +15,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,dyn_version_t version)
+CDynamixelMotor::CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_server,unsigned char dev_id,dyn_version_t version)
{
this->info.model="";
this->info.firmware_ver=(unsigned char)-1;
@@ -26,7 +25,6 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
this->info.max_angle=-1;
this->info.center_angle=-1;
this->info.max_speed=-1;
- this->info.bus_id="";
this->info.baudrate=(unsigned int)-1;
this->info.id=(unsigned char)-1;
this->compliance.cw_compliance_margin=0x00;
@@ -44,65 +42,10 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
this->config.max_torque=0.0;
this->config.punch=0x20;
this->registers=NULL;
- this->dyn_server=CDynamixelServer::instance();
+ this->dyn_server=dyn_server;
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,version);
- this->info.id=dev_id;
- this->get_model();
- 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_torque=this->get_max_torque();
- this->get_compliance_control(this->compliance);
- this->get_pid_control(this->pid);
- this->alarms=this->get_turn_off_alarms();
- }catch(CException &e){
- /* handle exceptions */
- if(this->dynamixel_dev!=NULL)
- delete this->dynamixel_dev;
- this->dynamixel_dev=NULL;
- throw;
- }
-}
-
-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;
- this->info.gear_ratio=(unsigned int)-1;
- this->info.encoder_resolution=(unsigned int)-1;
- this->info.pid_control=false;
- this->info.max_angle=-1;
- this->info.center_angle=-1;
- this->info.max_speed=-1;
- this->info.bus_id="";
- this->info.baudrate=(unsigned int)-1;
- this->info.id=(unsigned char)-1;
- this->compliance.cw_compliance_margin=0x00;
- this->compliance.ccw_compliance_margin=0x00;
- this->compliance.cw_compliance_slope=0x00;
- this->compliance.ccw_compliance_slope=0x00;
- this->pid.p=0x00;
- this->pid.i=0x00;
- this->pid.d=0x00;
- this->config.max_angle=0.0;
- this->config.min_angle=0.0;
- this->config.max_temperature=0.0;
- 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->info.baudrate=this->dyn_server->get_baudrate();
this->dynamixel_dev=this->dyn_server->get_device(dev_id,version);
this->info.id=dev_id;
this->get_model();
@@ -505,7 +448,6 @@ void CDynamixelMotor::get_servo_info(TDynamixel_info &info)
info.max_angle=this->info.max_angle;
info.center_angle=this->info.center_angle;
info.max_speed=this->info.max_speed;
- info.bus_id=this->info.bus_id;
info.baudrate=this->info.baudrate;
info.id=this->info.id;
}
@@ -1489,7 +1431,6 @@ CDynamixelMotor::~CDynamixelMotor()
this->info.max_angle=-1;
this->info.center_angle=-1;
this->info.max_speed=-1;
- this->info.bus_id="";
this->info.baudrate=(unsigned int)-1;
this->info.id=(unsigned char)-1;
this->compliance.cw_compliance_margin=0x00;
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index ca81c0a2bf8e2677c8cbb99d15bd3fc670234841..d466a2ac165d968ab42ead5964a66d6e94a85874 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -33,7 +33,6 @@ typedef struct
double max_angle;
double center_angle;
double max_speed;
- std::string bus_id;
unsigned int baudrate;
unsigned char id;
}TDynamixel_info;
@@ -155,12 +154,7 @@ class CDynamixelMotor
* \brief
*
*/
- 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,dyn_version_t version=dyn_version1);
+ CDynamixelMotor(std::string& cont_id,CDynamixelServer *dyn_server,unsigned char dev_id,dyn_version_t version=dyn_version1);
/**
* \brief
*
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index e5096b00133ea5745d55720cc584756af7ab97d9..0e86391e1aa4d376d4ad011973c3c50d51d97888 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -17,7 +17,7 @@
#include "xml/dynamixel_motor_group_cfg_file.hxx"
#endif
-CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id)
+CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,CDynamixelServer *dyn_server)
{
if(group_id.size()==0)
{
@@ -27,13 +27,13 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id)
else
{
this->group_id=group_id;
- this->dyn_server=CDynamixelServer::instance();
+ this->dyn_server=dyn_server;
this->clear();
this->mode=angle_ctrl;
}
}
-CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version)
+CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,CDynamixelServer *dyn_server,std::vector<unsigned char> &ids,dyn_version_t version)
{
unsigned int i=0;
@@ -45,7 +45,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
else
{
this->group_id=group_id;
- this->dyn_server=CDynamixelServer::instance();
+ this->dyn_server=dyn_server;
this->clear();
this->mode=torque_ctrl;
if(ids.size()==0)
@@ -54,40 +54,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
throw CDynamixelMotorGroupException(_HERE_,"Impossible to create an empty group");
}
try{
- this->dyn_server->config_bus(bus_id,baudrate);
- for(i=0;i<ids.size();i++)
- this->init_motor(ids[i],version);
- this->set_control_mode(angle_ctrl);
- }catch(CException &e){
- /* handle exceptions */
- this->clear();
- throw;
- }
- }
-}
-
-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;
-
- if(group_id.size()==0)
- {
- /* handle exceptions */
- throw CDynamixelMotorGroupException(_HERE_,"Invalid group identifier - empty string");
- }
- else
- {
- this->group_id=group_id;
- this->dyn_server=CDynamixelServer::instance();
- this->clear();
- this->mode=torque_ctrl;
- if(ids.size()==0)
- {
- /* handle exceptions */
- throw CDynamixelMotorGroupException(_HERE_,"Impossible to create an empty group");
- }
- try{
- this->dyn_server->config_bus(bus_id,baudrate);
for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version);
this->set_control_mode(angle_ctrl);
@@ -656,7 +622,6 @@ void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version)
CDynamixel *dynamixel_dev=NULL;
info.baudrate=this->dyn_server->get_baudrate();
- info.bus_id=this->dyn_server->get_bus_serial();
dynamixel_dev=this->dyn_server->get_device(id,version);
this->dynamixel_dev.push_back(dynamixel_dev);
info.id=id;
@@ -746,7 +711,6 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
this->clear();
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());
@@ -810,8 +774,6 @@ TDynamixelGroup_config CDynamixelMotorGroup::read_config(std::string &filename)
// 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.id.resize(cfg->dyn_motor_config().size());
config.dyn_config.resize(cfg->dyn_motor_config().size());
config.pid_control.resize(cfg->dyn_motor_config().size());
@@ -855,7 +817,6 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
// 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));
- 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());
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index d7e35b3de09bd3959ade7f1080c2af2550c860eb..a79f87a36f3055413e3d3b72a0f3ebc791616a67 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -7,8 +7,6 @@
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;
@@ -185,17 +183,12 @@ class CDynamixelMotorGroup
* \brief
*
*/
- CDynamixelMotorGroup(std::string &group_id);
+ CDynamixelMotorGroup(std::string &group_id,CDynamixelServer *dyn_server);
/**
* \brief
*
*/
- 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,dyn_version_t version=dyn_version1);
+ CDynamixelMotorGroup(std::string &group_id,CDynamixelServer *dyn_server,std::vector<unsigned char> &ids,dyn_version_t version=dyn_version1);
/* configuration functions */
/**
* \brief
diff --git a/src/examples/motor_testbench.cpp b/src/examples/motor_testbench.cpp
index 44bacebf21ed170548ca8035420fbbbfda29b6e8..357925af6ec1693448b275ddb302a0240386dcec 100755
--- a/src/examples/motor_testbench.cpp
+++ b/src/examples/motor_testbench.cpp
@@ -1,16 +1,16 @@
#include "dynamixelexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.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";
+std::string config_file="../src/xml/dyn_servo_config.xml";
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CDynamixelMotor *cont;
std::string serial="A400gavq";
TDynamixel_info info;
@@ -42,7 +42,8 @@ int main(int argc, char *argv[])
}
if(dyn_server->get_num_buses()>0)
{
- cont = new CDynamixelMotor(cont_name, serial, 1000000, 23,dyn_version2);
+ dyn_server->config_bus(serial,1000000);
+ cont = new CDynamixelMotor(cont_name, dyn_server, 23,dyn_version2);
if(argc==3)
cont->load_config(config_file);
else
diff --git a/src/examples/test_dynamixel_motion_seq.cpp b/src/examples/test_dynamixel_motion_seq.cpp
index 1a0d4d48efe9b2c0a1a46181ccc41b818a518ded..3a96c4bf063fc08402acb3e7053630653aa505b8 100755
--- a/src/examples/test_dynamixel_motion_seq.cpp
+++ b/src/examples/test_dynamixel_motion_seq.cpp
@@ -1,5 +1,5 @@
#include "dynamixelexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.h"
#include "eventserver.h"
#include "exceptions.h"
#include "dynamixel_motor_group.h"
@@ -12,14 +12,15 @@ std::string sequence_filename="../src/xml/sequence1.xml";
std::string group_name="GROUP1";
-std::string config_file="../src/xml/dyn_group_config.xml";
+std::string group_config_file="../src/xml/dyn_group_config.xml";
+std::string dyn_config_file="../src/xml/dyn_config.xml";
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CEventServer *event_server=CEventServer::instance();
std::vector<double> angles,speeds;
- CDynamixelMotorGroup group(group_name);
+ CDynamixelMotorGroup group(group_name,dyn_server);
CTrajectory<CDynamixelMotorGroup> dyn_traj("dyn_traj",&group,&CDynamixelMotorGroup::stop,&CDynamixelMotorGroup::get_current_angle,
&CDynamixelMotorGroup::move_absolute_angle,&CDynamixelMotorGroup::move_relative_angle,
&CDynamixelMotorGroup::get_num_motors);
@@ -28,7 +29,8 @@ int main(int argc, char *argv[])
if(dyn_server->get_num_buses()>0)
{
#ifdef _HAVE_XSD
- group.load_config(config_file);
+ dyn_server->config(dyn_config_file);
+ group.load_config(group_config_file);
dyn_traj.load_trajectory(sequence_filename);
dyn_traj.start_trajectory();
while(!event_server->event_is_set(dyn_traj.get_trajectory_complete_event_id()))
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index dd79d3dc9d183bff51bb236fe2696ff92c2e3da2..f096687ac1f02802a63274bd071cd9e50d2c85ae 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -1,15 +1,15 @@
#include "dynamixelexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.h"
#include "exceptions.h"
#include "dynamixel_motor.h"
#include <iostream>
std::string cont_name="RX-28";
-std::string config_file="../src/xml/dyn_config.xml";
+std::string config_file="../src/xml/dyn_servo_config.xml";
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CDynamixelMotor *cont;
std::string serial="A400gavq";
TDynamixel_info info;
@@ -22,7 +22,8 @@ int main(int argc, char *argv[])
{
if(dyn_server->get_num_buses()>0)
{
- cont = new CDynamixelMotor(cont_name, serial, 57600, 1);
+ dyn_server->config_bus(serial,1000000);
+ cont = new CDynamixelMotor(cont_name, dyn_server, 1);
cont->get_servo_info(info);
std::cout << "[INFO]: Servo model: " << info.model << std::endl;
@@ -49,7 +50,6 @@ int main(int argc, char *argv[])
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);
@@ -69,7 +69,6 @@ int main(int argc, char *argv[])
cont->turn_led_on();
sleep(1);
cont->turn_led_off();
- return 1;
// cont->move_absolute_angle(-90,300,0);
// sleep(2);
// cont->move_absolute_angle(90,100,0);
@@ -93,7 +92,6 @@ int main(int argc, char *argv[])
cont->move_torque(100);
sleep(5);
cont->move_torque(0);
- return 1;
for (size_t aa = 0; aa < 100; ++aa)
{
try
diff --git a/src/examples/test_dynamixel_motor_group.cpp b/src/examples/test_dynamixel_motor_group.cpp
index 9c44294384ebd908e2efe468b22ff7182404e44b..b2d0e7635825dfa9746c4ac580874d624bf000ae 100644
--- a/src/examples/test_dynamixel_motor_group.cpp
+++ b/src/examples/test_dynamixel_motor_group.cpp
@@ -1,5 +1,5 @@
#include "dynamixelexceptions.h"
-#include "dynamixelserver.h"
+#include "dynamixelserver_ftdi.h"
#include "eventserver.h"
#include "exceptions.h"
#include "dynamixel_motor_group.h"
@@ -9,20 +9,22 @@
std::string group_name="GROUP1";
-std::string config_file="../src/xml/dyn_group_config.xml";
+std::string group_config_file="../src/xml/dyn_group_config.xml";
+std::string dyn_config_file="../src/xml/dyn_config.xml";
int main(int argc, char *argv[])
{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
std::vector<double> angles,speeds,torques;
- CDynamixelMotorGroup group(group_name);
+ CDynamixelMotorGroup group(group_name,dyn_server);
unsigned int i=0;
try{
if(dyn_server->get_num_buses()>0)
{
#ifdef _HAVE_XSD
- group.load_config(config_file);
+ dyn_server->config(dyn_config_file);
+ group.load_config(group_config_file);
angles.resize(2);
angles[0]=90;
angles[1]=-10;
diff --git a/src/xml/dyn_group_config.xml b/src/xml/dyn_group_config.xml
index 85c2803ab3d42e3bef563688018004e02b245066..a284fad5896904dd98491f34633d5f9f8c7bcbc3 100755
--- a/src/xml/dyn_group_config.xml
+++ b/src/xml/dyn_group_config.xml
@@ -3,14 +3,12 @@
<dyn_motor_group_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="dyn_motor_group_cfg_file.xsd">
- <bus_id>A400gavq</bus_id>
- <baudrate>1000000</baudrate>
<dyn_motor_config>
<id>2</id>
- <config_file>dyn_config.xml</config_file>
+ <config_file>dyn_servo_config.xml</config_file>
</dyn_motor_config>
<dyn_motor_config>
<id>3</id>
- <config_file>dyn_config.xml</config_file>
+ <config_file>dyn_servo_config.xml</config_file>
</dyn_motor_config>
</dyn_motor_group_config>
diff --git a/src/xml/dyn_config.xml b/src/xml/dyn_servo_config.xml
similarity index 100%
rename from src/xml/dyn_config.xml
rename to src/xml/dyn_servo_config.xml
diff --git a/src/xml/dynamixel_motor_group_cfg_file.xsd b/src/xml/dynamixel_motor_group_cfg_file.xsd
index c2664d681dca68d02659ecc4e96b0c9bcd694f03..926ec83dd9160918fa33a46e8a59bb229ef0fd42 100755
--- a/src/xml/dynamixel_motor_group_cfg_file.xsd
+++ b/src/xml/dynamixel_motor_group_cfg_file.xsd
@@ -21,10 +21,6 @@ copyright : not copyrighted - public domain
<xsd:complexType name="dyn_motor_group_config_t">
<xsd:sequence>
- <xsd:element name="bus_id" type="xsd:string">
- </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">