diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e9ea8eda7a50ff091b85af4183f500060875467a
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,84 @@
+# Pre-requisites about cmake itself
+CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
+
+if(COMMAND cmake_policy)
+ cmake_policy(SET CMP0005 NEW)
+ cmake_policy(SET CMP0003 NEW)
+
+endif(COMMAND cmake_policy)
+
+# The project name and the type of project
+PROJECT(library_name)
+
+SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
+SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+SET(CMAKE_INSTALL_PREFIX /usr/local)
+
+IF (NOT CMAKE_BUILD_TYPE)
+ SET(CMAKE_BUILD_TYPE "DEBUG")
+ENDIF (NOT CMAKE_BUILD_TYPE)
+
+SET(CMAKE_C_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
+SET(CMAKE_C_FLAGS_RELEASE "-O3 -D_REENTRANT")
+
+ADD_SUBDIRECTORY(src)
+
+FIND_PACKAGE(Doxygen)
+
+FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
+IF (IRI_DOC_DIR)
+ ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/iri_doc/doxygen.conf)
+ELSE (IRI_DOC_DIR)
+ ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
+ENDIF (IRI_DOC_DIR)
+
+ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
+
+IF (UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "distribution clean"
+ COMMAND make ARGS clean
+ COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
+
+ TARGET distclean
+ )
+ELSE(UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "distclean only implemented in unix"
+ TARGET distclean
+ )
+ENDIF(UNIX)
+
+ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
+
+IF (UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "uninstall package"
+ COMMAND xargs ARGS rm < install_manifest.txt
+
+ TARGET uninstall
+ )
+ELSE(UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "uninstall only implemented in unix"
+ TARGET uninstall
+ )
+ENDIF(UNIX)
+
+IF (UNIX)
+ SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${DISTRIB}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
+ SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
+ SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Part of IRI-laboratory libraries. More information at http://wikiri.upc.es/index.php/Robotics_Lab")
+ SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
+ SET(CPACK_GENERATOR "DEB")
+ SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "labrobotica@iri.upc.edu")
+ SET(CPACK_SET_DESTDIR "ON") # Necessary because of the absolute install paths
+
+ INCLUDE(CPack)
+ELSE(UNIX)
+ ADD_CUSTOM_COMMAND(
+ COMMENT "packaging only implemented in unix"
+ TARGET uninstall
+ )
+ENDIF(UNIX)
+
diff --git a/Findsegway_head.cmake b/Findsegway_head.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..e25190c14e961c31e44d0a6f111ac74536b7b08f
--- /dev/null
+++ b/Findsegway_head.cmake
@@ -0,0 +1,21 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(segway_head_INCLUDE_DIR segway_head.h /usr/include/iridrivers /usr/local/include/iridrivers)
+
+FIND_LIBRARY(segway_head_LIBRARY
+ NAMES segway_head
+ PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
+
+IF (segway_head_INCLUDE_DIR AND segway_head_LIBRARY)
+ SET(segway_head_FOUND TRUE)
+ENDIF (segway_head_INCLUDE_DIR AND segway_head_LIBRARY)
+
+IF (segway_head_FOUND)
+ IF (NOT segway_head_FIND_QUIETLY)
+ MESSAGE(STATUS "Found segway_head: ${segway_head_LIBRARY}")
+ ENDIF (NOT segway_head_FIND_QUIETLY)
+ELSE (segway_head_FOUND)
+ IF (segway_head_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find segway_head")
+ ENDIF (segway_head_FIND_REQUIRED)
+ENDIF (segway_head_FOUND)
+
diff --git a/ReadMe.txt b/ReadMe.txt
new file mode 100644
index 0000000000000000000000000000000000000000..acb4bedbca276a6f877d8f3121d41d04dcecf9b7
--- /dev/null
+++ b/ReadMe.txt
@@ -0,0 +1,19 @@
+Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+Author shernand (shernand@iri.upc.edu)
+All rights reserved.
+
+This file is part of segway head driver library
+segway head driver library is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>
+
+
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
new file mode 100644
index 0000000000000000000000000000000000000000..347af2ef86bf4786d49b13d48c12146b0d001b01
--- /dev/null
+++ b/doc/doxygen.conf
@@ -0,0 +1,251 @@
+# Doxyfile 1.5.5
+
+@INCLUDE_PATH = ../doc/
+@INCLUDE = doxygen_project_name.conf
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+DOXYFILE_ENCODING = UTF-8
+PROJECT_NUMBER =
+OUTPUT_DIRECTORY = ../doc
+CREATE_SUBDIRS = NO
+OUTPUT_LANGUAGE = English
+BRIEF_MEMBER_DESC = YES
+REPEAT_BRIEF = NO
+ABBREVIATE_BRIEF =
+ALWAYS_DETAILED_SEC = NO
+INLINE_INHERITED_MEMB = NO
+FULL_PATH_NAMES = YES
+STRIP_FROM_PATH =
+STRIP_FROM_INC_PATH =
+SHORT_NAMES = NO
+JAVADOC_AUTOBRIEF = NO
+QT_AUTOBRIEF = NO
+MULTILINE_CPP_IS_BRIEF = NO
+DETAILS_AT_TOP = NO
+INHERIT_DOCS = YES
+SEPARATE_MEMBER_PAGES = NO
+TAB_SIZE = 8
+ALIASES =
+OPTIMIZE_OUTPUT_FOR_C = YES
+OPTIMIZE_OUTPUT_JAVA = NO
+OPTIMIZE_FOR_FORTRAN = NO
+OPTIMIZE_OUTPUT_VHDL = NO
+BUILTIN_STL_SUPPORT = NO
+CPP_CLI_SUPPORT = NO
+SIP_SUPPORT = NO
+DISTRIBUTE_GROUP_DOC = NO
+SUBGROUPING = YES
+TYPEDEF_HIDES_STRUCT = NO
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL = NO
+EXTRACT_PRIVATE = NO
+EXTRACT_STATIC = NO
+EXTRACT_LOCAL_CLASSES = YES
+EXTRACT_LOCAL_METHODS = NO
+EXTRACT_ANON_NSPACES = NO
+HIDE_UNDOC_MEMBERS = NO
+HIDE_UNDOC_CLASSES = NO
+HIDE_FRIEND_COMPOUNDS = NO
+HIDE_IN_BODY_DOCS = NO
+INTERNAL_DOCS = NO
+CASE_SENSE_NAMES = YES
+HIDE_SCOPE_NAMES = NO
+SHOW_INCLUDE_FILES = YES
+INLINE_INFO = YES
+SORT_MEMBER_DOCS = NO
+SORT_BRIEF_DOCS = NO
+SORT_GROUP_NAMES = NO
+SORT_BY_SCOPE_NAME = NO
+GENERATE_TODOLIST = YES
+GENERATE_TESTLIST = YES
+GENERATE_BUGLIST = YES
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS =
+MAX_INITIALIZER_LINES = 30
+SHOW_USED_FILES = YES
+SHOW_DIRECTORIES = YES
+FILE_VERSION_FILTER =
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET = YES
+WARNINGS = YES
+WARN_IF_UNDOCUMENTED = YES
+WARN_IF_DOC_ERROR = YES
+WARN_NO_PARAMDOC = NO
+WARN_FORMAT = "$file:$line: $text"
+WARN_LOGFILE =
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+INPUT = ../src \
+ ../doc/main.dox
+INPUT_ENCODING = UTF-8
+FILE_PATTERNS = *.c \
+ *.h \
+ *.cpp
+RECURSIVE = YES
+EXCLUDE =
+EXCLUDE_SYMLINKS = NO
+EXCLUDE_PATTERNS = *.tab.c \
+ *.tab.h \
+ lex* \
+ *glr.h \
+ *llr.h \
+ *glr.c \
+ *llr.c \
+ *general.h
+EXCLUDE_SYMBOLS =
+EXAMPLE_PATH = ../src/examples
+EXAMPLE_PATTERNS =
+EXAMPLE_RECURSIVE = NO
+IMAGE_PATH = ../doc/images
+INPUT_FILTER =
+FILTER_PATTERNS =
+FILTER_SOURCE_FILES = NO
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER = YES
+INLINE_SOURCES = NO
+STRIP_CODE_COMMENTS = YES
+REFERENCED_BY_RELATION = YES
+REFERENCES_RELATION = YES
+REFERENCES_LINK_SOURCE = YES
+USE_HTAGS = NO
+VERBATIM_HEADERS = YES
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX = YES
+COLS_IN_ALPHA_INDEX = 5
+IGNORE_PREFIX =
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML = YES
+HTML_OUTPUT = html
+HTML_FILE_EXTENSION = .html
+HTML_HEADER =
+HTML_FOOTER =
+HTML_STYLESHEET =
+HTML_ALIGN_MEMBERS = YES
+GENERATE_HTMLHELP = NO
+GENERATE_DOCSET = NO
+DOCSET_FEEDNAME = "Doxygen generated docs"
+DOCSET_BUNDLE_ID = org.doxygen.Project
+HTML_DYNAMIC_SECTIONS = NO
+CHM_FILE =
+HHC_LOCATION =
+GENERATE_CHI = NO
+BINARY_TOC = NO
+TOC_EXPAND = NO
+DISABLE_INDEX = NO
+ENUM_VALUES_PER_LINE = 4
+GENERATE_TREEVIEW = NO
+TREEVIEW_WIDTH = 250
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX = NO
+LATEX_OUTPUT = latex
+LATEX_CMD_NAME = latex
+MAKEINDEX_CMD_NAME = makeindex
+COMPACT_LATEX = NO
+PAPER_TYPE = a4
+EXTRA_PACKAGES =
+LATEX_HEADER =
+PDF_HYPERLINKS = YES
+USE_PDFLATEX = NO
+LATEX_BATCHMODE = NO
+LATEX_HIDE_INDICES = NO
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF = NO
+RTF_OUTPUT = rtf
+COMPACT_RTF = NO
+RTF_HYPERLINKS = NO
+RTF_STYLESHEET_FILE =
+RTF_EXTENSIONS_FILE =
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN = NO
+MAN_OUTPUT = man
+MAN_EXTENSION = .3
+MAN_LINKS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML = NO
+XML_OUTPUT = xml
+XML_SCHEMA =
+XML_DTD =
+XML_PROGRAMLISTING = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD = NO
+PERLMOD_LATEX = NO
+PERLMOD_PRETTY = YES
+PERLMOD_MAKEVAR_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING = YES
+MACRO_EXPANSION = NO
+EXPAND_ONLY_PREDEF = NO
+SEARCH_INCLUDES = YES
+INCLUDE_PATH =
+INCLUDE_FILE_PATTERNS =
+PREDEFINED = _USE_MPI=1
+EXPAND_AS_DEFINED =
+SKIP_FUNCTION_MACROS = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+TAGFILES =
+GENERATE_TAGFILE =
+ALLEXTERNALS = NO
+EXTERNAL_GROUPS = YES
+PERL_PATH = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS = YES
+MSCGEN_PATH =
+HIDE_UNDOC_RELATIONS = YES
+HAVE_DOT = YES
+CLASS_GRAPH = YES
+COLLABORATION_GRAPH = YES
+GROUP_GRAPHS = YES
+UML_LOOK = NO
+TEMPLATE_RELATIONS = NO
+INCLUDE_GRAPH = NO
+INCLUDED_BY_GRAPH = NO
+CALL_GRAPH = YES
+CALLER_GRAPH = YES
+GRAPHICAL_HIERARCHY = YES
+DIRECTORY_GRAPH = NO
+DOT_IMAGE_FORMAT = png
+DOT_PATH =
+DOTFILE_DIRS =
+DOT_GRAPH_MAX_NODES = 50
+MAX_DOT_GRAPH_DEPTH = 2
+DOT_TRANSPARENT = YES
+DOT_MULTI_TARGETS = NO
+GENERATE_LEGEND = YES
+DOT_CLEANUP = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine
+#---------------------------------------------------------------------------
+SEARCHENGINE = NO
diff --git a/doc/doxygen_project_name.conf b/doc/doxygen_project_name.conf
new file mode 100644
index 0000000000000000000000000000000000000000..00d4264b7989e69c8cc246bc8c5805d8bf17eef1
--- /dev/null
+++ b/doc/doxygen_project_name.conf
@@ -0,0 +1 @@
+PROJECT_NAME = "segway head driver"
diff --git a/doc/main.dox b/doc/main.dox
new file mode 100644
index 0000000000000000000000000000000000000000..12cd23d061a796f66233678bdf2e54f5d811fbc9
--- /dev/null
+++ b/doc/main.dox
@@ -0,0 +1,107 @@
+/*! \mainpage segway head driver
+
+ \section Introduction
+
+ \subsection Pre-Requisites
+
+ This package requires of the following libraries and packages
+ - <A href="http://www.cmake.org">cmake</A>, a cross-platform build system.
+ - <A href="http://www.doxygen.org">doxygen</a> and
+ <A href="http://www.graphviz.org">graphviz</a> to generate the documentation.
+ - stdc++,
+ .
+
+ Under linux all of these utilities are available in ready-to-use packages.
+
+ Under MacOS most of the packages are available via <a href="http://www.finkproject.org/">fink</a>. <br>
+
+ \subsection Compilation
+
+ Just download this package, uncompress it, and execute
+ - cd build
+ - cmake ..
+ .
+ to generate the makefile and then
+ - make
+ .
+ to obtain the shared library (in this case called <em>iriutils.so</em>) and
+ also all the example programs.
+
+ The <em>cmake</em> only need to be executed once (make will automatically call
+ <em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
+
+ To generate this documentation type
+ - make doc
+ .
+
+ The files in the <em>build</em> directory are genetated by <em>cmake</em>
+ and <em>make</em> and can be safely removed.
+ After doing so you will need to call cmake manually again.
+
+ \subsection Configuration
+
+ The default build mode is DEBUG. That is, objects and executables
+ include debug information.
+
+ The RELEASE build mode optimizes for speed. To build in this mode
+ execute
+ - cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+ .
+ The release mode will be kept until next time cmake is executed.
+
+ \subsection Installation
+
+ In order to be able to use the library, it it necessary to copy it into the system.
+ To do that, execute
+
+ - make install
+ .
+
+ as root and the shared libraries will be copied to <em>/usr/local/lib/iriutils</em> directory
+ and the header files will be copied to <em>/usr/local/include/iriutils</em> dierctory. At
+ this point, the library may be used by any user.
+
+ To remove the library from the system, exceute
+ - make uninstall
+ .
+ as root, and all the associated files will be removed from the system.
+
+ \section Customization
+
+ To build a new application using these library, first it is necessary to locate if the library
+ has been installed or not using the following command
+
+ - FIND_PACKAGE(segway_head REQUIRED)
+
+ In the case that the package is present, it is necessary to add the header files directory to
+ the include directory path by using
+
+ - INCLUDE_DIRECTORIES(${segway_head_INCLUDE_DIR})
+
+ Finally, it is also nevessary to link with the desired libraries by using the following command
+
+ - TARGET_LINK_LIBRARIES(<executable name> ${segway_head_LIBRARY})
+ .
+
+ \section License
+
+ This package is licensed under a
+ <a href="http://www.gnu.org/licenses/lgpl.html">
+ LGPL 3.0 License</a>.
+
+ \section Disclaimer
+
+ This is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+ */
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..594e479683e2abd219b5450e210ad6c0cae57911
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,30 @@
+# edit the following line to add all the source code files of the library
+SET(sources segway_head.cpp)
+# edit the following line to add all the header files of the library
+SET(headers segway_head.h)
+
+# edit the following line to find the necessary packages
+FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
+FIND_PACKAGE(dynamixel REQUIRED)
+FIND_PACKAGE(dynamixel_motor_cont REQUIRED)
+
+# edit the following line to add the necessary include directories
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR} ${dynamixel_motor_cont_INCLUDE_DIR} .)
+
+ADD_LIBRARY(segway_head SHARED ${sources})
+
+#edit the following line to add the necessary system libraries (if any)
+TARGET_LINK_LIBRARIES(segway_head ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${dynamixel_motor_cont_LIBRARY})
+
+INSTALL(TARGETS segway_head
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib/iridrivers
+ ARCHIVE DESTINATION lib/iridrivers
+)
+
+INSTALL(FILES ${headers} DESTINATION include/iridrivers)
+
+INSTALL(FILES ../Findsegway_head.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+
+ADD_SUBDIRECTORY(examples)
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..33b128590a7424d4afb8559b9ec5b2deb9fddc28
--- /dev/null
+++ b/src/examples/CMakeLists.txt
@@ -0,0 +1,6 @@
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(segway_head_test segway_head_test.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(segway_head_test segway_head ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${dynamixel_motor_cont_LIBRARY})
+
diff --git a/src/examples/segway_head_test.cpp b/src/examples/segway_head_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..453cae280a9928e2643902e2744b80f0495c6374
--- /dev/null
+++ b/src/examples/segway_head_test.cpp
@@ -0,0 +1,48 @@
+#include "segway_head.h"
+#include <iostream>
+
+std::string head_name="tibi_head";
+
+int main(int argc, char *argv[])
+{
+ CSegwayHead head(head_name);
+ THeadServos servo_ids;
+ int i;
+
+ servo_ids.bus_id=0;
+ servo_ids.pan=1;
+ servo_ids.tilt=2;
+ servo_ids.side=3;
+ servo_ids.lights=5;
+
+ head.config(&servo_ids);
+ head.move_pan(150,100);
+ head.move_tilt(150,100);
+ head.move_side(150,100);
+ sleep(2);
+
+ for(i=0;i<1000;i++)
+ {
+ head.move_pan(70,100);
+ sleep(1);
+ head.move_pan(230,100);
+ sleep(1);
+ head.move_pan(150,100);
+ sleep(1);
+ head.move_side(110,100);
+ sleep(1);
+ head.move_side(190,100);
+ sleep(1);
+ head.move_side(150,100);
+ sleep(1);
+ head.move_tilt(100,100);
+ sleep(1);
+ head.move_tilt(150,100);
+ sleep(1);
+ std::cout << "iteration: " << i << std::endl;
+ }
+ head.stand_by();
+ sleep(2);
+ head.happy();
+ sleep(2);
+}
diff --git a/src/segway_head.cpp b/src/segway_head.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..23f7ec23b33f2e8a329faba579f29170b6f3dadf
--- /dev/null
+++ b/src/segway_head.cpp
@@ -0,0 +1,532 @@
+#include "segway_head.h"
+#include "eventexceptions.h"
+#include "exceptions.h"
+#include <iostream>
+
+CSegwayHead::CSegwayHead(std::string &device_id)
+{
+ this->servo_ids.bus_id=-1;
+ this->servo_ids.pan=-1;
+ this->servo_ids.tilt=-1;
+ this->servo_ids.side=-1;
+ this->servo_ids.lights=-1;
+ this->pan=NULL;
+ this->pan_motion_event_id="";
+ this->tilt=NULL;
+ this->tilt_motion_event_id="";
+ this->side=NULL;
+ this->side_motion_event_id="";
+ this->lights=NULL;
+ this->dyn_server=CDynamixelServer::instance();
+ if(this->dyn_server->get_num_buses()==0)
+ {
+ /* handle exceptions */
+ }
+ else
+ {
+ this->dyn_server->set_bus_id(0);
+ this->dyn_server->set_baudrate(1000000);
+ }
+}
+
+void CSegwayHead::set_pan_servo_id(unsigned char id)
+{
+ std::string pan_motor="pan_motor";
+
+ if(id>0xFD)
+ {
+ /* handle exceptions */
+ }
+ else
+ {
+ if(id==this->servo_ids.tilt || id==this->servo_ids.side || id==this->servo_ids.lights)
+ {
+ /* handle errors */
+ }
+ else
+ {
+ try{
+ if(this->pan!=NULL)
+ {
+ delete this->pan;
+ this->pan=NULL;
+ }
+ this->pan=new CDynamixelMotor(pan_motor,this->servo_ids.bus_id,id);
+ // configure the position feedback
+ this->pan_motion_event_id=this->pan->config_position_feedback(fb_polling,100.0);
+ this->servo_ids.pan=id;
+ }catch(CException &e){
+ /* handle exceptions */
+ throw;
+ }
+ }
+ }
+}
+
+void CSegwayHead::set_tilt_servo_id(unsigned char id)
+{
+ std::string tilt_motor="tilt_motor";
+
+ if(id>0xFD)
+ {
+ /* handle exceptions */
+ }
+ else
+ {
+ if(id==this->servo_ids.pan || id==this->servo_ids.side || id==this->servo_ids.lights)
+ {
+ /* handle errors */
+ }
+ else
+ {
+ try{
+ if(this->tilt!=NULL)
+ {
+ delete this->tilt;
+ this->tilt=NULL;
+ }
+ this->tilt=new CDynamixelMotor(tilt_motor,this->servo_ids.bus_id,id);
+ // configure the position feedback
+ this->tilt_motion_event_id=this->tilt->config_position_feedback(fb_polling,100.0);
+ this->servo_ids.tilt=id;
+ }catch(CException &e){
+ /* handle exceptions */
+ throw;
+ }
+ }
+ }
+}
+
+void CSegwayHead::set_side_servo_id(unsigned char id)
+{
+ std::string side_motor="side_motor";
+
+ if(id>0xFD)
+ {
+ /* handle exceptions */
+ }
+ else
+ {
+ if(id==this->servo_ids.pan || id==this->servo_ids.tilt || id==this->servo_ids.lights)
+ {
+ /* handle errors */
+ }
+ else
+ {
+ try{
+ if(this->side!=NULL)
+ {
+ delete this->side;
+ this->side!=NULL;
+ }
+ this->side=new CDynamixelMotor(side_motor,this->servo_ids.bus_id,id);
+ // configure the position feedback
+ this->side_motion_event_id=this->side->config_position_feedback(fb_polling,100.0);
+ this->servo_ids.side=id;
+ }catch(CException &e){
+ /* handle exceptions */
+ throw;
+ }
+ }
+ }
+}
+
+void CSegwayHead::set_light_id(unsigned char id)
+{
+ if(id>0xFD)
+ {
+ /* handle exceptions */
+ }
+ else
+ {
+ if(id==this->servo_ids.pan || id==this->servo_ids.tilt || id==this->servo_ids.side)
+ {
+ /* handle errors */
+ }
+ else
+ {
+ try{
+ if(this->lights!=NULL)
+ {
+ delete this->lights;
+ this->lights=NULL;
+ }
+ this->lights=this->dyn_server->get_device(id);
+ this->servo_ids.lights=id;
+ }catch(CException &e){
+ /* handle exceptions */
+ throw;
+ }
+ }
+ }
+}
+
+void CSegwayHead::turn_on_all(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(0x1a,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::turn_off_all(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(0x1a,0x00);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::config(THeadServos *config)
+{
+ this->servo_ids.bus_id=config->bus_id;
+ this->dyn_server->set_bus_id(config->bus_id);
+ try{
+ this->set_pan_servo_id(config->pan);
+ this->set_tilt_servo_id(config->tilt);
+ this->set_side_servo_id(config->side);
+ this->set_light_id(config->lights);
+ this->stand_by();
+ }catch(CException &e){
+ throw;
+ }
+}
+
+unsigned char CSegwayHead::get_pan_servo_id(void)
+{
+ return this->servo_ids.pan;
+}
+
+unsigned char CSegwayHead::get_tilt_servo_id(void)
+{
+ return this->servo_ids.tilt;
+}
+
+unsigned char CSegwayHead::get_side_servo_id(void)
+{
+ return this->servo_ids.side;
+}
+
+unsigned char CSegwayHead::get_lights_id(void)
+{
+ return this->servo_ids.lights;
+}
+
+void CSegwayHead::move_pan(float degrees,float speed)
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::vector<float> position(1),velocity(1),accel(1);
+ std::list<std::string> events;
+
+ if(this->servo_ids.pan!=-1)
+ {
+ try{
+ events.push_back(this->pan_motion_event_id);
+ event_server->wait_all(events);
+ position[0]=degrees;
+ velocity[0]=speed;
+ accel[0]=speed*speed/(0.1*degrees);
+ this->pan->load(position,velocity,accel);
+ this->pan->move();
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::move_tilt(float degrees,float speed)
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::vector<float> position(1),velocity(1),accel(1);
+ std::list<std::string> events;
+
+ if(this->servo_ids.tilt!=-1)
+ {
+ try{
+ events.push_back(this->tilt_motion_event_id);
+ event_server->wait_all(events);
+ position[0]=degrees;
+ velocity[0]=speed;
+ accel[0]=speed*speed/(0.1*degrees);
+ this->tilt->load(position,velocity,accel);
+ this->tilt->move();
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::move_side(float degrees,float speed)
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::vector<float> position(1),velocity(1),accel(1);
+ std::list<std::string> events;
+
+ if(this->servo_ids.side!=-1)
+ {
+ try{
+ events.push_back(this->side_motion_event_id);
+ event_server->wait_all(events);
+ position[0]=degrees;
+ velocity[0]=speed;
+ accel[0]=speed*speed/(0.1*degrees);
+ this->side->load(position,velocity,accel);
+ this->side->move();
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::move_head(float pan, float tilt, float side,float speed)
+{
+ CEventServer *event_server=CEventServer::instance();
+ std::vector<float> position(1),velocity(1),accel(1);
+ std::list<std::string> events;
+
+ if(this->servo_ids.pan!=-1 && this->servo_ids.tilt!=-1 && this->servo_ids.side!=-1)
+ {
+ events.push_back(this->pan_motion_event_id);
+ events.push_back(this->tilt_motion_event_id);
+ events.push_back(this->side_motion_event_id);
+ event_server->wait_all(events);
+ velocity[0]=speed;
+ accel[0]=speed*speed/(0.1*pan);
+ position[0]=pan;
+ this->pan->load(position,velocity,accel);
+ accel[0]=speed*speed/(0.1*tilt);
+ position[0]=tilt;
+ this->tilt->load(position,velocity,accel);
+ accel[0]=speed*speed/(0.1*side);
+ position[0]=side;
+ this->side->load(position,velocity,accel);
+ this->pan->move();
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+
+}
+
+float CSegwayHead::get_pan_position(void)
+{
+ if(this->servo_ids.pan!=-1)
+ {
+ return this->pan->get_position()[0];
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+float CSegwayHead::get_tilt_position(void)
+{
+ if(this->servo_ids.tilt!=-1)
+ {
+ return this->tilt->get_position()[0];
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+float CSegwayHead::get_side_position(void)
+{
+ if(this->servo_ids.side!=-1)
+ {
+ return this->side->get_position()[0];
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::get_head_position(float *pan, float *tilt, float *side)
+{
+ *pan=this->get_pan_position();
+ *tilt=this->get_tilt_position();
+ *side=this->get_side_position();
+}
+
+void CSegwayHead::set_brightness(float brightness)
+{
+ unsigned char brightness_value;
+
+ if(this->servo_ids.lights!=-1)
+ {
+ if(brightness<0.0 || brightness>100.0)
+ {
+ /* handle exceptions */
+ }
+ else
+ {
+ brightness_value=(unsigned char)(brightness*255.0/100.0);
+ this->lights->write_byte_register(brightness_reg,brightness_value);
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+float CSegwayHead::get_brightness(void)
+{
+ unsigned char brightness_value;
+
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->read_byte_register(brightness_reg,&brightness_value);
+ return ((float)brightness_value)*100.0/255.0;
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::turn_on_segment(segment_id id)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register((unsigned char)id,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::turn_off_segment(segment_id id)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register((unsigned char)id,0x00);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+bool CSegwayHead::is_segment_on(segment_id id)
+{
+ unsigned char state;
+
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->read_byte_register((unsigned char)id,&state);
+ if(state==0x01)
+ return true;
+ else
+ return false;
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::stand_by(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(standby_reg,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::happy(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(happy_reg,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::sat(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(sat_reg,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::angry(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(angry_reg,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::confused(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(confused_reg,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+void CSegwayHead::shy(void)
+{
+ if(this->servo_ids.lights!=-1)
+ {
+ this->lights->write_byte_register(shy_reg,0x01);
+ }
+ else
+ {
+ /* handle exceptions */
+ }
+}
+
+CSegwayHead::~CSegwayHead()
+{
+ this->servo_ids.bus_id=-1;
+ this->servo_ids.pan=-1;
+ this->servo_ids.tilt=-1;
+ this->servo_ids.side=-1;
+}
diff --git a/src/segway_head.h b/src/segway_head.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d4366a9ba1f71f8d08f62d8d2d8848909f33f04
--- /dev/null
+++ b/src/segway_head.h
@@ -0,0 +1,100 @@
+#ifndef _SEGWAY_HEAD_H
+#define _SEGWAY_HEAD_H
+
+#include "dynamixel.h"
+#include "dynamixel_motor.h"
+
+typedef struct
+{
+ unsigned char bus_id;
+ unsigned char pan;
+ unsigned char tilt;
+ unsigned char side;
+ unsigned char lights;
+}THeadServos;
+
+typedef enum {left_eyebrow_left=0x01,
+ left_eyebrow_center=0x02,
+ left_eyebrow_right=0x03,
+ left_cheek_in=0x04,
+ left_cheek_out=0x05,
+ lip_upper_left=0x06,
+ lip_bottom_left=0x07,
+ lip_upper_center=0x08,
+ lip_center=0x09,
+ right_eyebrow_center=0x0A,
+ right_eyebrow_left=0x0B,
+ right_eyebrow_right=0x0C,
+ right_cheek_out=0x0D,
+ right_cheek_in=0x0E,
+ lip_bottom_center=0x0F,
+ lip_bottom_right=0x10,
+ lip_upper_right=0x11,
+ additional1=0x12,
+ additional2=0x13,
+ additional3=0x14,
+ additional4=0x15,
+ additional5=0x16,
+ additional6=0x17,
+ additional7=0x18,
+ additional8=0x19} segment_id;
+
+const unsigned char brightness_reg=0x00;
+const unsigned char standby_reg=0x20;
+const unsigned char happy_reg=0x21;
+const unsigned char sat_reg=0x22;
+const unsigned char angry_reg=0x23;
+const unsigned char confused_reg=0x24;
+const unsigned char shy_reg=0x25;
+
+class CSegwayHead
+{
+ private:
+ THeadServos servo_ids;
+ CDynamixelServer *dyn_server;
+ CDynamixelMotor *pan;
+ std::string pan_motion_event_id;
+ CDynamixelMotor *tilt;
+ std::string tilt_motion_event_id;
+ CDynamixelMotor *side;
+ std::string side_motion_event_id;
+ CDynamixel *lights;
+ protected:
+ void set_pan_servo_id(unsigned char id);
+ void set_tilt_servo_id(unsigned char id);
+ void set_side_servo_id(unsigned char id);
+ void set_light_id(unsigned char id);
+ void turn_on_all(void);
+ void turn_off_all(void);
+ public:
+ CSegwayHead(std::string &device_id);
+ void config(THeadServos *config);
+ unsigned char get_pan_servo_id(void);
+ unsigned char get_tilt_servo_id(void);
+ unsigned char get_side_servo_id(void);
+ unsigned char get_lights_id(void);
+ // neck control
+ void move_pan(float degrees,float speed=0.0);
+ void move_tilt(float degrees,float speed=0.0);
+ void move_side(float degrees,float speed=0.0);
+ void move_head(float pan, float tilt, float side,float speed=0.0);
+ float get_pan_position(void);
+ float get_tilt_position(void);
+ float get_side_position(void);
+ void get_head_position(float *pan, float *tilt, float *side);
+ // lights control
+ void set_brightness(float brightness);
+ float get_brightness(void);
+ void turn_on_segment(segment_id id);
+ void turn_off_segment(segment_id id);
+ bool is_segment_on(segment_id id);
+ void stand_by(void);
+ void happy(void);
+ void sat(void);
+ void angry(void);
+ void confused(void);
+ void shy(void);
+ ~CSegwayHead();
+};
+
+#endif