diff --git a/src/dynamixel_pan_tilt.cpp b/src/dynamixel_pan_tilt.cpp index 3d182079e5457c64b02a9cfc6f4c59eb3f0762c1..f1001b6da806c7c85ed5a6439ed2d1bcc0d7673d 100644 --- a/src/dynamixel_pan_tilt.cpp +++ b/src/dynamixel_pan_tilt.cpp @@ -358,6 +358,55 @@ void CDynamixel_Pan_Tilt::load_config(std::string &filename) throw CDynamixel_Pan_TiltException(_HERE_,"The dynamixel pan&tilt is not properly configured. Configuration not loaded"); } +void CDynamixel_Pan_Tilt::read_config(std::string &filename, Dynamixel_pan_tilt_xml_limits &xml_limits) +{ + struct stat buffer; + + if(stat(filename.c_str(),&buffer)==0) + { + // try to open the specified file + try{ + std::auto_ptr<dynamixel_pan_tilt_config_t> cfg(dynamixel_pan_tilt_config(filename.c_str(), xml_schema::flags::dont_validate)); + // configure the parameters of the controller + + dynamixel_pan_tilt_config_t::min_angle_const_iterator min_angle (cfg->min_angle().begin()); + dynamixel_pan_tilt_config_t::max_angle_const_iterator max_angle (cfg->max_angle().begin()); + xml_limits.min_angle.pan = *min_angle; + xml_limits.max_angle.pan = *max_angle; + + min_angle = cfg->min_angle().end(); + max_angle = cfg->max_angle().end(); + min_angle--; + max_angle--; + xml_limits.min_angle.tilt = *min_angle; + xml_limits.max_angle.tilt = *max_angle; + + if (cfg->max_torque().size() > 0) + { + dynamixel_pan_tilt_config_t::max_torque_const_iterator max_torque (cfg->max_torque().begin()); + xml_limits.max_torque.pan = *max_torque; + max_torque = cfg->max_torque().end(); + max_torque--; + xml_limits.max_torque.tilt = *max_torque; + } + else + { + xml_limits.max_torque.pan = 50; + xml_limits.max_torque.tilt = 50; + } + } + catch (const xml_schema::exception& e) + { + std::ostringstream os; + os << "loading config exception: " << e; + /* handle exceptions */ + throw CDynamixelMotorException(_HERE_,os.str()); + } + } + else + throw CDynamixel_Pan_TiltException(_HERE_,"The configuration file does not exist."); +} + void CDynamixel_Pan_Tilt::save_config(std::string &filename) { /*xml_schema::namespace_infomap map; diff --git a/src/dynamixel_pan_tilt.h b/src/dynamixel_pan_tilt.h index 9b4c3a7e47267e4af69a011c8bef567b2b1b4ec0..52871dd02ec3de99ac912b2be0f37db244265d5d 100644 --- a/src/dynamixel_pan_tilt.h +++ b/src/dynamixel_pan_tilt.h @@ -188,6 +188,25 @@ typedef struct Torque_moving_state tilt; }Torque_pan_tilt_moving_state; +/** + * \struct Dynamixel_pan_tilt_xml_limits. + * + * \brief A struct to save the configuration limits readed of a xml file. + * + * It has three members of type Dynamixel_pan_tilt_data: + * - max_torque + * - max_angle + * - min_angle + */ + +typedef struct +{ + Dynamixel_pan_tilt_data max_torque; + Dynamixel_pan_tilt_data max_angle; + Dynamixel_pan_tilt_data min_angle; +}Dynamixel_pan_tilt_xml_limits; + + /** * \class CDynamyxel_Pan_Tilt * @@ -443,6 +462,14 @@ class CDynamixel_Pan_Tilt * \param filename The file where it's going to be read the pan configuration. */ void load_config(std::string &filename); + + /** + * \brief Static function to read the configuration of the pan-tilt reading the information from a specific file. + * + * \param filename The file where it's going to be read the pan configuration. + * \param xml_limits The struct where is going to be saved the information. + */ + static void read_config(std::string &filename, Dynamixel_pan_tilt_xml_limits &xml_limits); /** * \brief Function to save the current configuration of the pan-tilt on a specific file. diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 70de73b2d646e9e6a2f86e38ae9092e05e045936..fd57ded6a4448c51cd884233fcccbdaee0dcbe96 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -2,4 +2,7 @@ ADD_EXECUTABLE(test_dynamixel_pan_tilt test_dynamixel_pan_tilt.cpp) # link necessary libraries TARGET_LINK_LIBRARIES(test_dynamixel_pan_tilt dynamixel_pan_tilt) - +# create an example application +ADD_EXECUTABLE(fucking_test fucking_test.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(fucking_test dynamixel_pan_tilt) diff --git a/src/examples/test_dynamixel_pan_tilt.cpp b/src/examples/test_dynamixel_pan_tilt.cpp index 30ed6dc0a52098dfa6ad52a224733678269288da..de44d89f85b2e70d2ed2d63b834a923b03a04728 100644 --- a/src/examples/test_dynamixel_pan_tilt.cpp +++ b/src/examples/test_dynamixel_pan_tilt.cpp @@ -209,6 +209,13 @@ int main(int argc, char *argv[]) sleep(1); //pan_tilt->save_config(config_saving_file); + + Dynamixel_pan_tilt_xml_limits xml_limits; + CDynamixel_Pan_Tilt::read_config(config_file, xml_limits); + pan_tilt->print_pan_tilt_data(xml_limits.max_angle); + pan_tilt->print_pan_tilt_data(xml_limits.min_angle); + pan_tilt->print_pan_tilt_data(xml_limits.max_torque); + //*/ pan_tilt->get_compliance_control(compliance); diff --git a/src/xml/dyn_pan_tilt_config_AX12plus.xml b/src/xml/dyn_pan_tilt_config_AX12plus.xml index abc50829685794b8c85dd786319e17d47188efaa..ba3b5970d69752da2cc3e536944f570cf5517fc9 100644 --- a/src/xml/dyn_pan_tilt_config_AX12plus.xml +++ b/src/xml/dyn_pan_tilt_config_AX12plus.xml @@ -7,7 +7,7 @@ <temp_limit>85</temp_limit> <max_voltage>19</max_voltage> <min_voltage>6</min_voltage> - <max_torque>20</max_torque> + <max_torque>60</max_torque> <cw_comp_margin>2</cw_comp_margin> <ccw_comp_margin>2</ccw_comp_margin> <cw_comp_slope>64</cw_comp_slope> @@ -16,8 +16,8 @@ <kp>0</kp> <ki>0</ki> <kd>0</kd> - <min_angle>-95</min_angle><!--PAN --> - <max_angle>95</max_angle><!--PAN --> - <min_angle>-95</min_angle><!--TILT --> - <max_angle>60</max_angle><!--TILT --> + <min_angle>-85</min_angle><!--PAN --> + <max_angle>85</max_angle><!--PAN --> + <min_angle>-85</min_angle><!--TILT --> + <max_angle>85</max_angle><!--TILT --> </dynamixel_pan_tilt_config>