diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..212386fe9c39a5fa8804e8ba83d7b6af144d7f80
--- /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(comm)
+
+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")
+SET(CMAKE_C_FLAGS_RELEASE "-O3")
+
+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_rmp_200.cmake b/Findsegway_rmp_200.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..8c47911d9d22969376aeb4a573e6b2e264289802
--- /dev/null
+++ b/Findsegway_rmp_200.cmake
@@ -0,0 +1,20 @@
+FIND_PATH(segway_rmp_200_INCLUDE_DIR segway_rmp200.h segway_rmp200_exceptions.h /usr/include/iridrivers /usr/local/include/iridrivers)
+
+FIND_LIBRARY(segway_rmp_200_LIBRARY
+ NAMES segway_rmp_200
+ PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
+
+IF (segway_rmp_200_INCLUDE_DIR AND segway_rmp_200_LIBRARY)
+ SET(segway_rmp_200_FOUND TRUE)
+ENDIF (segway_rmp_200_INCLUDE_DIR AND segway_rmp_200_LIBRARY)
+
+IF (segway_rmp_200_FOUND)
+ IF (NOT segway_rmp_200_FIND_QUIETLY)
+ MESSAGE(STATUS "Found Segway RMP200 driver: ${segway_rmp_200_LIBRARY}")
+ ENDIF (NOT segway_rmp_200_FIND_QUIETLY)
+ELSE (segway_rmp_200_FOUND)
+ IF (segway_rmp_200_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find segway RMP200 driver")
+ ENDIF (segway_rmp_200_FIND_REQUIRED)
+ENDIF (segway_rmp_200_FOUND)
+
diff --git a/ReadMe.txt b/ReadMe.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6f90a5cfa1bcb547b7003fb3303bfaafee9a0731
--- /dev/null
+++ b/ReadMe.txt
@@ -0,0 +1,31 @@
+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 RMP 200 driver library
+segway RMP 200 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..37b18d4c01c606682a80fd74c0ab435cf79d9387
--- /dev/null
+++ b/doc/doxygen_project_name.conf
@@ -0,0 +1 @@
+PROJECT_NAME = "segway RMP 200 driver"
diff --git a/doc/images/threads_diagram.pdf b/doc/images/threads_diagram.pdf
new file mode 100755
index 0000000000000000000000000000000000000000..0a67e87eb4f2573fab3f533bcfe8f16bdbf2b5df
Binary files /dev/null and b/doc/images/threads_diagram.pdf differ
diff --git a/doc/main.dox b/doc/main.dox
new file mode 100644
index 0000000000000000000000000000000000000000..4e59c249fe279e468cc3b89be82d516514f7a228
--- /dev/null
+++ b/doc/main.dox
@@ -0,0 +1,172 @@
+/*! \mainpage segway RMP200 driver
+
+ \section Introduction
+
+ This driver is in charge to operate motion of a SegwayRMP200 and as well as monitoring
+
+ The main class is SegwayRMP200,
+
+ - The class has an FTDI driver object to ensure USB communication with the segway
+ - It also launches two threads for reading data and sending commands to the segway
+ - Uses a mutex variable, to ensure that only one thread is accessing ftdi object at a time.
+ .
+
+ Segway parameters such as position and velocities paramaters are internal variables of the class,
+ and updated every 10 ms approx, by the read thread.
+
+ The read thread waits for a reception event coming from FTDI device. Once received it reads data stored into
+ receiving buffer of FTDI device. A reference of reception event of FTDI device is retrieved at constructor so
+ as to wait upon it.
+
+
+ Comanding the segway on the ohter hand is done at 50 Hz approx by the command thread, i.e one new command each
+ 20 ms.Actually, segway must receive a command at least every 0.4 seconds (2.5 Hz) in order to keep it moving.
+ Otherwise it will automatically set motors to zero.
+
+
+ \section Installation
+
+ \subsection Pre-Requisites
+
+ This driver 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.
+ - <a href="http://www.libusb.org">libusb</a> for libftd2xx.
+ - <a href="http://www.ftdichip.com">libftd2xx</a> for FTDI chip
+ - <a href="http://wikiri.upc.es/index.php/Utilities_library">libiriutils</a> for threads and events from IRI repository.
+ - <a href="http://wikiri.upc.es/index.php/Communications_library">communications library</a> for the FTDI driver.
+ .
+
+ Under linux all of these utilities are available in ready-to-use packages.
+
+ \subsection Compilation
+
+
+ \par Install iriutils
+
+ For information on how to download, compile and install this library, please
+ visit the following web page: http://wikiri.upc.es/index.php/Utilities_library
+
+ (FTDI,...)
+
+ \par Install this library
+ To build (placed in ./bin) the library just type in FTDI directory:
+
+ \code
+ cmake .
+ make
+ \endcode
+
+ To create the documentation (placed in ./doc) type:
+ \code
+ make doc
+ \endcode
+
+ To install the library into system folders
+ \code
+ sudo make install
+ \endcode
+
+ Just download this package, uncompress it, and execute cmake in build folder,
+ this step will generate the makefile and put all cmake files this folder.
+
+ \code
+ cd build
+ cmake ..
+ \endcode
+
+ To obtain the executables execute in build folder:
+
+ \code
+ make
+ \endcode
+
+ To install the library SegwayRMP200 into system folders, in build folder:
+ \code
+ sudo make install
+ \endcode
+
+ To generate the documentation, in build folder:
+ \code
+ make doc
+ \endcode
+
+ <em>Cmake</em> only needs to be executed once (make will automatically call
+ <em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
+
+ 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 Contents
+
+ This project is composed by the following source files
+ - SegwayRMP200Driver.cpp (Segway driver)
+ - SegwayRMP200Driver.h
+ - SegwayRMP200_main.cpp (example program)
+ .
+
+ \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.
+
+ \section License
+
+ This package is licensed under a
+ <a href="http://creativecommons.org/licenses/by/3.0/">
+ Creative Commons Attribution 3.0 Unported License</a>.
+
+ \section Disclaimer
+
+ <i>This package is distributed in the hope that it will be useful, but without any warranty.
+ it is provided "as is" without warranty of any kind, either expressed or implied, including,
+ but not limited to, the implied warranties of merchantability and fitness for a particular
+ purpose. The entire risk as to the quality and performance of the program is with you.
+ should the program prove defective, the GMR group does not assume the cost of any necessary
+ servicing, repair or correction.
+
+ In no event unless required by applicable law the author will be liable to you for damages,
+ including any general, special, incidental or consequential damages arising out of the use or
+ inability to use the program (including but not limited to loss of data or data being rendered
+ inaccurate or losses sustained by you or third parties or a failure of the program to operate
+ with any other programs), even if the author has been advised of the possibility of such damages.</i>
+
+ \author Ismael Haddad
+ \version 0.1
+ \date september 2009
+
+*/
+
+
+
+/*! \todo - afegir variables internes : velocity command (as received) i turn command (as received). Correspond al packet de dades 0x407, veure pag. 10 del Segway RMP Interface guide
+ - mutex per vT i vR ?. fer un mutex.enter() i mutex.exit() a la funcio setComands()
+ - mirar quins son els rangs de valors corrents per velocitat translacio i rotació i implementar algun mecanisme per no sobrepasar els rangs.
+ - Amb Processador UI ences (boto verd) i motors apagats, es pot llegir missatges. Son els heartbeats, missatges sense informacio que envia el UI processor als CPU dels motors
+ - Per llegir informacio util del segway, els motors han d'estar encesos
+ - Si s'utilitza amb un segway 200 (amb balanceig), anar amb compte amb la configuració.
+ De fet, es pot configurar el mode de traccio en el que s'ha de posar el segway (tractor, balance) aixi com bloquejar el balanceig (sendConfig() o config())
+ - sembla que la carga de les batteries dels UI variin lleugerament (augmenten?) quan s'utilitzen els motors. Potser hi ha algun mecanisme de carrega de les bateries amb alternador (verificar)
+ .
+*/
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..60b7e99959d65aa60334dcb9698248546e7ea2bb
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,31 @@
+# edit the following line to add all the source code files of the library
+SET(sources segway_rmp200.cpp segway_rmp200_exceptions.cpp)
+# edit the following line to add all the header files of the library
+SET(headers segway_rmp200.h segway_rmp200_exceptions.h)
+
+# edit the following line to find the necessary packages
+FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
+
+# edit the following line to add the necessary include directories
+INCLUDE_DIRECTORIES(.)
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
+
+ADD_LIBRARY(segway_rmp_200 SHARED ${sources})
+
+#edit the following line to add the necessary system libraries (if any)
+TARGET_LINK_LIBRARIES(segway_rmp_200 ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(segway_rmp_200 ${comm_LIBRARY})
+
+INSTALL(TARGETS segway_rmp_200
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib/iridrivers
+ ARCHIVE DESTINATION lib/iridrivers
+)
+
+INSTALL(FILES ${headers} DESTINATION include/iridrivers)
+
+INSTALL(FILES ../Findsegway_rmp_200.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..6d03ddab41eb690bac833170fcad02f0b1acd7df
--- /dev/null
+++ b/src/examples/CMakeLists.txt
@@ -0,0 +1,9 @@
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_segway_rmp_200 test_segwayRMP200.cpp)
+ADD_EXECUTABLE(motionModelData motionModelData.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_segway_rmp_200 segway_rmp_200 ${comm_LIBRARY})
+TARGET_LINK_LIBRARIES(motionModelData segway_rmp_200 ${comm_LIBRARY})
+
+
diff --git a/src/examples/motionModelData.cpp b/src/examples/motionModelData.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4df786ea5d9c578eb4c6536d8c96e7708e95b247
--- /dev/null
+++ b/src/examples/motionModelData.cpp
@@ -0,0 +1,217 @@
+#include "segway_rmp200.h"
+#include "ftdiexceptions.h"
+#include "ftdiserver.h"
+#include "ftdimodule.h"
+#include <iostream>
+#include <fstream>
+#include <time.h>
+#include <sys/time.h>
+#include <math.h>
+
+
+/** \example motionModelData.cpp
+ *
+ * This example executes a series of commands to record platform data of interest to build a kinematic/dynammic model of the platform
+ *
+ * Before running this example it is important to follow the next steps:
+ *
+ * - check that E-Stop lanyard is plugged. (the red cordon). Motors won't
+ * start unless it is plugged
+ * - connect segway to USB slot of Pc host
+ * - start Segway UI processor (green button)
+ * - start Motors (yellow button)
+ *
+ * This example first creates an FTDI server to detect any possible segway
+ * platform connected to the computer. It uses the add_custom_PID() function
+ * to add the PID and VID combination used by Segway. It then opens the
+ * first device found, if any, using the serial number. At this point it is
+ * important that no other FTDI device is connected to the computer because
+ * the program does not check which kind of device it is.
+ *
+ * Once the FTDI device is opened and configured, a new CSegwayRMP200 object
+ * is created an associated to the FTDI device. Then the robotic platform is
+ * configured and commanded to move forward.
+ *
+ * While moving, every 1 second, the whole information of the platform is
+ * 50displayed on screen. Since the odometry of the robot uses inertial sensors,
+ * if the platform is not actually moving, the displayed information will
+ * have no real meaning.
+ *
+ * It is important to note that it is not possible to execute any function
+ * of the public API of the CSegwayRMP200 class until a valid FTDI object
+ * has been associated to the segway object.
+ *
+ * If the motor are not enabled (yellow button turned off), the segway only
+ * sends heartbeat commands, but it is not possible to send motion commands
+ * or receive feedback data. In this case, the program will not fail, but
+ * nothing will happen.
+ */
+
+using namespace std;
+
+string segway_name="segway";
+unsigned int iterationPeriod = 50; //msec
+
+int main(int argc, char *argv[])
+{
+ //operational variables
+ CSegwayRMP200 *segway;
+ CFTDIServer *ftdi_server=CFTDIServer::instance();
+ string serial_number;
+ unsigned int ii=0;
+ unsigned int jj=0;
+ unsigned int kk=0;
+ int opt;
+ ofstream dataFile; //data file
+ float vt, vr; //[m/s], [rad/s]
+ string fileName = "/home/andreu/Desktop/dataFile.txt";
+ timeval tvTimeStamp;
+ double timeStamp;
+ streamsize nn;
+
+ //user variables
+ float duration; //duration of all the experiment
+ float vTstepDuration; //duration of a step
+ float vRstepDuration; //duration of a step
+ float vT0; //initial translational velocity
+ float vR0; //initial rotational velocity
+ float vTstepIncrement; //velocity increment at each step
+ float vRstepIncrement; //velocity increment at each step
+
+ //user derived variables
+ unsigned int totalIterations;
+ unsigned int jjMax;
+ unsigned int kkMax;
+
+ while ((opt = getopt(argc, argv, "f:h?"))!= -1)
+ {
+ switch (opt)
+ {
+ case 'f': // file name
+ fileName = optarg;
+ break;
+ case '?': // help
+ case 'h':
+ default:
+ cout << " USAGE" << endl
+ << " " << argv[0] << " [options]" << endl
+ << " OPTIONS" << endl
+ << " -f FILE_NAME (default: " << fileName << ")" << endl;
+ return 1;
+ }
+ }
+
+ //user dialog
+ cout << endl << "Duration [s]: "; cin >> duration;
+ cout << endl << "vTstepDuration [s]: "; cin >> vTstepDuration;
+ cout << endl << "vRstepDuration [s]: "; cin >> vRstepDuration;
+ cout << endl << "vT0 [m/s]: "; cin >> vT0;
+ cout << endl << "vR0 [rad/s]: "; cin >> vR0;
+ cout << endl << "vTstepIncrement [m/s]: "; cin >> vTstepIncrement;
+ cout << endl << "vRstepIncrement [rad/s]: "; cin >> vRstepIncrement;
+ cout << endl;
+
+ //set user derived variables
+ totalIterations = (unsigned int)(duration/(iterationPeriod/1000.0));
+ jjMax = (unsigned int)(vTstepDuration/(iterationPeriod/1000.0));
+ kkMax = (unsigned int)(vRstepDuration/(iterationPeriod/1000.0));
+
+ //prints experiment config
+ cout << "*******************************************************************" << endl;
+ cout << "Duration: " << duration << endl;
+ cout << "vTstepDuration: " << vTstepDuration << endl;
+ cout << "vRstepDuration: " << vRstepDuration << endl;
+ cout << "vTstepIncrement: " << vTstepIncrement << endl;
+ cout << "vRstepIncrement: " << vRstepIncrement << endl;
+ cout << "vT0: " << vT0 << endl;
+ cout << "vR0: " << vR0 << endl;
+ cout << "totalIterations: " << totalIterations << endl;
+ cout << "jjMax: " << jjMax << endl;
+ cout << "kkMax: " << kkMax << endl;
+ cout << "*******************************************************************" << endl;
+ cout << "Do you want to continue ? (y/n)" << endl;
+ char resp;
+ cin >> resp;
+ if (resp == 'n')
+ {
+ cout << "Execution aborted" << endl;
+ return 1;
+ }
+
+ //open data file and prints a header
+ dataFile.open(fileName.c_str(), ofstream::out);
+ dataFile << "tS[s] vTcmnd[m/s] vRcmnd[rad/s] vT[m/s] vR[rad/s] pitch[rad] roll[rad] pitchRate[rad/s] rollRate[rad/s] "
+ << "leftTorque[Nm] rightTorque[Nm] uiBattery[V] motorBattery[V]" << endl;
+
+
+ //execution
+ try{
+ segway=new CSegwayRMP200(segway_name);
+ segway->connect();
+ usleep(10000);
+ segway->unlock_balance();
+ segway->reset_right_wheel_integrator();
+ segway->reset_left_wheel_integrator();
+ segway->reset_yaw_integrator();
+ segway->reset_forward_integrator();
+ segway->set_velocity_scale_factor(1.0);
+ segway->set_acceleration_scale_factor(1.0);
+ segway->set_turnrate_scale_factor(1.0);
+ //segway->set_operation_mode(balance);
+
+ vt = vT0;
+ vr = vR0;
+ for (ii=0; ii<totalIterations; ii++)
+ {
+ jj++; kk++;
+ if ( jj == jjMax )
+ {
+ jj=0;
+ vt = vt + vTstepIncrement;
+ }
+ if ( kk == kkMax )
+ {
+ kk=0;
+ vr = vr + vRstepIncrement;
+ }
+
+ //send commands to platform
+ segway->move( vt , vr/(2*M_PI) ); //rotational speed required in revolutions per second
+
+ //timeStamp
+ gettimeofday(&tvTimeStamp, NULL);
+ timeStamp = (double)(tvTimeStamp.tv_sec + tvTimeStamp.tv_usec/1e6);
+
+ //logging data
+ //cout << (*segway) << endl;
+ nn=dataFile.precision(15);
+ dataFile << timeStamp << " ";
+ dataFile.precision(6);
+ dataFile << vt << "\t" << vr << "\t"
+ << (segway->get_left_wheel_velocity()+segway->get_right_wheel_velocity())/2.0 << "\t" << segway->get_yaw_rate()*M_PI/180.0 << "\t"
+ << segway->get_pitch_angle()*M_PI/180.0 << "\t" << segway->get_roll_angle()*M_PI/180.0 << "\t"
+ << segway->get_pitch_rate()*M_PI/180.0 << "\t" << segway->get_roll_rate()*M_PI/180.0 << "\t"
+ << segway->get_left_motor_torque() << "\t" << segway->get_right_motor_torque() << "\t"
+ << segway->get_ui_battery_voltage() << "\t" << segway->get_powerbase_battery_voltage() << endl;
+
+ //sleep up to next iteration
+ usleep(iterationPeriod*1000);//sending commands @20Hz approx
+ }
+
+ dataFile.close();
+
+/* segway->reset_right_wheel_integrator();
+ segway->reset_left_wheel_integrator();
+ segway->reset_yaw_integrator();
+ segway->reset_forward_integrator();
+ sleep(5);
+ cout << (*segway) << endl;*/
+
+ segway->stop();
+ segway->close();
+ delete segway;
+
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+}
diff --git a/src/examples/test_segwayRMP200.cpp b/src/examples/test_segwayRMP200.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..7acc1bb37697133a387351c54daebd772de5f85a
--- /dev/null
+++ b/src/examples/test_segwayRMP200.cpp
@@ -0,0 +1,147 @@
+#include "segway_rmp200.h"
+#include "ftdiexceptions.h"
+#include "eventserver.h"
+#include "ftdiserver.h"
+#include "ftdimodule.h"
+#include <iostream>
+
+/** \example test_segwayRMP200.cpp
+ *
+ * This example shows the basic operation of a Segway RMP200 platform.
+ *
+ * Before running this example it is important to follow the next steps:
+ *
+ * - check that E-Stop lanyard is plugged. (the red cordon). Motors won't
+ * start unless it is plugged
+ * - connect segway to USB slot of Pc host
+ * - start Segway UI processor (green button)
+ * - start Motors (yellow button)
+ *
+ * This example first creates an FTDI server to detect any possible segway
+ * platform connected to the computer. It uses the add_custom_PID() function
+ * to add the PID and VID combination used by Segway. It then opens the
+ * first device found, if any, using the serial number. At this point it is
+ * important that no other FTDI device is connected to the computer because
+ * the program does not check which kind of device it is.
+ *
+ * Once the FTDI device is opened and configured, a new CSegwayRMP200 object
+ * is created an associated to the FTDI device. Then the robotic platform is
+ * configured and commanded to move forward.
+ *
+ * While moving, every 1 second, the whole information of the platform is
+ * displayed on screen. Since the odometry of the robot uses inertial sensors,
+ * if the platform is not actually moving, the displayed information will
+ * have no real meaning.
+ *
+ * It is important to note that it is not possible to execute any function
+ * of the public API of the CSegwayRMP200 class until a valid FTDI object
+ * has been associated to the segway object.
+ *
+ * If the motor are not enabled (yellow button turned off), the segway only
+ * sends heartbeat commands, but it is not possible to send motion commands
+ * or receive feedback data. In this case, the program will not fail, but
+ * nothing will happen.
+ */
+
+std::string segway_name="segway";
+
+int main(int argc, char *argv[])
+{
+ CSegwayRMP200 *segway = NULL;
+ std::list<std::string> events;
+ CFTDIServer *ftdi_server=CFTDIServer::instance();
+ CEventServer *event_server=CEventServer::instance();
+ std::string serial_number,cable_disconnected,power_off;
+ bool connected=false;
+ int i=0,event_id=0;
+
+ try
+ {
+ while(!connected)
+ {
+ try
+ {
+ segway = new CSegwayRMP200();
+ connected = true;
+ }
+ catch(CException &e)
+ {
+ if(segway != NULL)
+ delete segway;
+
+ segway = NULL;
+
+ std::cout << e.what() << std::endl;
+ std::cout << "The segway platfrom is still not ready (is it power on?)" << std::endl << std::endl;
+ sleep(1);
+ }
+ }
+
+ segway->unlock_balance();
+ segway->set_operation_mode(tractor);
+ segway->set_gain_schedule(light);
+ segway->reset_right_wheel_integrator();
+ segway->reset_left_wheel_integrator();
+ segway->reset_yaw_integrator();
+ segway->reset_forward_integrator();
+
+ // keep in mind that the order in of the event son the events list fix their
+ // priority. The first event on the list will be checked first, and if it is
+ // active, the other will not be checked.
+ events.push_back(segway->get_cable_disconnected_event());
+ events.push_back(segway->get_power_off_event());
+ events.push_back(segway->get_no_heartbeat_event());
+ events.push_back(segway->get_new_status_event());
+
+ segway->move(0.1,0.0);
+ for(i=0;i<1000000;i++)
+ {
+ event_id=event_server->wait_first(events);
+ if(event_id==3)
+ {
+ if((i%100)==0)
+ std::cout << (*segway) << std::endl;
+ }
+ else if(event_id==2)
+ {
+ std::cout << "No heart beat detected" << std::endl;
+ segway->stop();
+ sleep(1);
+ }
+ else if(event_id==1)
+ {
+ std::cout << "The segway platform power is off" << std::endl;
+ segway->stop();
+ sleep(1);
+ }
+ else
+ {
+ std::cout << "The USB cable has been disconnected" << std::endl;
+ segway->stop();
+ segway->close();
+ connected=false;
+ while(!connected)
+ {
+ try
+ {
+ segway->connect();
+ connected=true;
+ segway->move(0.1,0.0);
+ }
+ catch(CException &e)
+ {
+ std::cout << "The segway platfrom is still not ready" << std::endl;
+ sleep(1);
+ }
+ }
+ }
+ }
+ segway->stop();
+ segway->close();
+ delete segway;
+ }
+ catch(CException &e)
+ {
+ std::cout << e.what() << std::endl;
+ }
+}
diff --git a/src/segway_rmp200.cpp b/src/segway_rmp200.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..bef3fb563ff55a09d6589ee698a551146e4ebb53
--- /dev/null
+++ b/src/segway_rmp200.cpp
@@ -0,0 +1,953 @@
+#include "segway_rmp200.h"
+#include "segway_rmp200_exceptions.h"
+#include "eventexceptions.h"
+#include "ftdiexceptions.h"
+#include <iostream>
+#include <string>
+
+const std::string CSegwayRMP200::description = "Robotic Mobile Platform";
+const short int CSegwayRMP200::pid = 0xE729;
+
+CSegwayRMP200::CSegwayRMP200(const std::string& desc_serial)
+{
+ std::string serial_number=desc_serial;
+ std::vector<int> ftdi_devs;
+
+ this->event_server = CEventServer::instance();
+ this->thread_server = CThreadServer::instance();
+
+ this->init_attributes();
+ this->init_ftdi();
+
+ if(desc_serial.compare("") == 0)
+ {
+ ftdi_devs = this->ftdi_server->get_ids_by_description(CSegwayRMP200::description);
+
+ //check how many segway devices are connected
+ //launch exception if there are no segways or there is more than one
+ if(ftdi_devs.size() != 1)
+ {
+ if(ftdi_devs.size() == 0)
+ throw CSegwayRMP200Exception(_HERE_, "No segways available", this->id);
+ else
+ {
+ throw CSegwayRMP200Exception(_HERE_, "More than one segway available", this->id);
+ }
+ }
+ serial_number= this->ftdi_server->get_serial_number(ftdi_devs.at(0));
+ }
+ this->id = "segway_" + serial_number;
+ this->init_threads();
+ this->init_events();
+
+ this->connect(desc_serial);
+}
+
+void CSegwayRMP200::init_attributes(void)
+{
+ // initialize internal variables
+ this->left_wheel_velocity=0.0;
+ this->right_wheel_velocity=0.0;
+ this->pitch_angle=0.0;
+ this->pitch_rate=0.0;
+ this->roll_angle=0.0;
+ this->roll_rate=0.0;
+ this->yaw_rate=0.0;
+ this->servo_frames=0.0;
+ this->left_wheel_displ=0.0;
+ this->right_wheel_displ=0.0;
+ this->forward_displ=0.0;
+ this->yaw_displ=0.0;
+ this->left_torque=0.0;
+ this->right_torque=0.0;
+ this->mode=(op_mode)-1;
+ this->hardware_mode=(op_mode)-1;
+ this->gain_schedule=(gain)-1;
+ this->ui_battery=0.0;
+ this->powerbase_battery=0.0;
+ // command variables
+ this->vT=0;
+ this->vR=0;
+}
+
+void CSegwayRMP200::init_ftdi(void)
+{
+ this->comm_dev=NULL;
+ this->ftdi_server = CFTDIServer::instance();
+ this->ftdi_server->add_custom_PID(this->pid);
+}
+
+void CSegwayRMP200::init_threads(void)
+{
+
+ // create the feedback and command threads
+ this->read_thread_id = this->id;
+ this->read_thread_id += "_read_thread";
+ this->thread_server->create_thread(this->read_thread_id);
+ this->thread_server->attach_thread(this->read_thread_id,this->start_read_thread,this);
+ this->command_thread_id = this->id;
+ this->command_thread_id += "_command_thread";
+ this->thread_server->create_thread(this->command_thread_id);
+ this->thread_server->attach_thread(this->command_thread_id,this->start_command_thread,this);
+ //create the heartbeat thread
+ this->heartbeat_thread_id = this->id;
+ this->heartbeat_thread_id += "_heartbeat_thread";
+ this->thread_server->create_thread(this->heartbeat_thread_id);
+ this->thread_server->attach_thread(this->heartbeat_thread_id,this->heartbeat_thread,this);
+}
+
+void CSegwayRMP200::init_events(void)
+{
+ // create the finish events
+ this->read_finish_event = this->id;
+ this->read_finish_event += "_finish_read_thread";
+ this->event_server->create_event(this->read_finish_event);
+ this->command_finish_event = this->id;
+ this->command_finish_event += "_finish_command_thread";
+ this->event_server->create_event(this->command_finish_event);
+ // create the error events
+ this->cable_disconnected_event = this->id;
+ this->cable_disconnected_event += "_cable_disconnected";
+ this->event_server->create_event(this->cable_disconnected_event);
+ this->power_off_event = this->id;
+ this->power_off_event += "_power_off_event";
+ this->event_server->create_event(this->power_off_event);
+ this->no_heartbeat_event = this->id;
+ this->no_heartbeat_event += "_no_heartbeat";
+ this->event_server->create_event(this->no_heartbeat_event);
+ // create the heartbeat event
+ this->heartbeat_event = this->id;
+ this->heartbeat_event += "_heartbeat";
+ this->event_server->create_event(this->heartbeat_event);
+ // create the new status event
+ this->new_status_event = this->id;
+ this->new_status_event += "_new_status";
+ this->event_server->create_event(this->new_status_event);
+
+}
+
+std::string CSegwayRMP200::get_id(void)
+{
+ return this->id;
+}
+
+std::string CSegwayRMP200::get_cable_disconnected_event(void)
+{
+ return this->cable_disconnected_event;
+}
+
+std::string CSegwayRMP200::get_power_off_event(void)
+{
+ return this->power_off_event;
+}
+
+std::string CSegwayRMP200::get_no_heartbeat_event(void)
+{
+ return this->no_heartbeat_event;
+}
+
+std::string CSegwayRMP200::get_new_status_event(void)
+{
+ return this->new_status_event;
+}
+
+unsigned char CSegwayRMP200::compute_checksum(segway_packet *packet)
+{
+ int i;
+
+ unsigned short int checksum=0;
+ unsigned short int checksum_high=0;
+
+ for(i=0;i<17;i++)
+ checksum += (short)packet->data[i];
+ checksum_high = (unsigned short)(checksum >> 8);
+ checksum &= 0xFF;
+ checksum +=checksum_high;
+ checksum_high = (unsigned short)(checksum >> 8);
+ checksum &= 0xFF;
+ checksum +=checksum_high;
+ checksum = (~checksum + 1) & 0xFF;
+
+ return (unsigned char)checksum;
+}
+
+bool CSegwayRMP200::read_packet(segway_packet *packet,int *packet_len)
+{
+ int read_data=0,len=0,pos=0;
+ unsigned char *data=NULL;
+
+ if(packet==NULL)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Invalid packet structure",this->id);
+ }
+ else
+ {
+ len=this->comm_dev->get_num_data();
+ data=new unsigned char[len];
+ try{
+ read_data=this->comm_dev->read(data,len);
+ }catch(CException &e){
+ /* handle exception */
+ throw;
+ }
+ if(read_data!=len)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Unexpected error while reading the USB device",this->id);
+ }
+ while(pos<len)
+ {
+ if((*packet_len)==0)
+ {
+ if(data[pos]==0xF0)
+ {
+ packet->data[(*packet_len)]=data[pos];
+ (*packet_len)++;
+ }
+ }
+ else
+ {
+ if((*packet_len)==1)
+ {
+ if(data[pos]==0x55)
+ {
+ packet->data[(*packet_len)]=data[pos];
+ (*packet_len)++;
+ }
+ else
+ (*packet_len)=0;
+ }
+ else
+ {
+ if((*packet_len)==2)
+ {
+ if(data[pos]==0xAA || data[pos]==0xBB)
+ {
+ packet->data[(*packet_len)]=data[pos];
+ (*packet_len)++;
+ }
+ else
+ (*packet_len)=0;
+ }
+ else
+ {
+ packet->data[(*packet_len)]=data[pos];
+ (*packet_len)++;
+ if((*packet_len)==18)
+ {
+ (*packet_len)=0;
+ if(data!=NULL)
+ delete[] data;
+ return true;
+ }
+ }
+ }
+ }
+ pos++;
+ }
+ }
+
+ if(data!=NULL)
+ delete[] data;
+ return false;
+}
+
+void CSegwayRMP200::parse_packet(segway_packet *packet)
+{
+ short int command;
+ int i=0;
+
+ if(this->compute_checksum(packet)!=packet->data[17])
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Invalid packet checksum, a transmission error ocurred",this->id);
+ }
+ if(packet->data[2]==0xaa)
+ {
+ this->access_status.enter();
+ command=(packet->data[4]*256+packet->data[5])/32;
+ switch(command)
+ {
+ case 0x0400: break;
+ case 0x0401: this->pitch_angle=((float)((short int)(((int)packet->data[9]<<8)+(int)packet->data[10])))/7.8;
+ this->pitch_rate=((float)((short int)(((int)packet->data[11]<<8)+(int)packet->data[12])))/7.8;
+ this->roll_angle=((float)((short int)(((int)packet->data[13]<<8)+(int)packet->data[14])))/7.8;
+ this->roll_rate=((float)((short int)(((int)packet->data[15]<<8)+(int)packet->data[16])))/7.8;
+ break;
+ case 0x0402: this->left_wheel_velocity=((float)((short int)(((int)packet->data[9]<<8)+(int)packet->data[10])))/332.0;
+ this->right_wheel_velocity=((float)((short int)(((int)packet->data[11]<<8)+(int)packet->data[12])))/332.0;
+ this->yaw_rate=((float)((short int)(((int)packet->data[13]<<8)+(int)packet->data[14])))/7.8;
+ this->servo_frames=((float)((unsigned short int)(((int)packet->data[15]<<8)+(int)packet->data[16])))*0.01;
+ break;
+ case 0x0403: this->left_wheel_displ=((float)((int)(((int)packet->data[11]<<24)+((int)packet->data[12]<<16)+((int)packet->data[9]<<8)+(int)packet->data[10])))/33215.0;
+ this->right_wheel_displ=((float)((int)(((int)packet->data[15]<<24)+((int)packet->data[16]<<16)+((int)packet->data[13]<<8)+(int)packet->data[14])))/33215.0;
+ break;
+ case 0x0404: this->forward_displ=((float)((int)(((int)packet->data[11]<<24)+((int)packet->data[12]<<16)+((int)packet->data[9]<<8)+(int)packet->data[10])))/33215.0;;
+ this->yaw_displ=((float)((int)(((int)packet->data[15]<<24)+((int)packet->data[16]<<16)+((int)packet->data[13]<<8)+(int)packet->data[14])))/112644.0;
+ break;
+ case 0x0405: this->left_torque=((float)((short int)(((int)packet->data[9]<<8)+(int)packet->data[10])))/1094.0;
+ this->right_torque=((float)((short int)(((int)packet->data[11]<<8)+(int)packet->data[12])))/1094.0;
+ break;
+ case 0x0406: this->mode=(op_mode)(packet->data[9]*256+packet->data[10]);
+ this->gain_schedule=(gain)(((int)packet->data[11]<<8)+(int)packet->data[12]);
+ this->ui_battery=1.4+(float)(((int)packet->data[13]<<8)+(int)packet->data[14])*0.0125;
+ this->powerbase_battery=(float)(((int)packet->data[15]<<8)+(int)packet->data[16])/4.0;
+ if(!this->event_server->event_is_set(this->new_status_event))
+ this->event_server->set_event(this->new_status_event);
+ break;
+ case 0x0407: break;
+ case 0x0680: this->hardware_mode=(op_mode)(packet->data[10]&0x03);
+ if(!this->event_server->event_is_set(this->heartbeat_event))
+ this->event_server->set_event(this->heartbeat_event);
+ break;
+ case 0x0688: break;
+ default: /* handle exceptions */
+ break;
+ }
+ this->access_status.exit();
+ }
+}
+
+void *CSegwayRMP200::start_read_thread(void *param)
+{
+ CSegwayRMP200 *segway = (CSegwayRMP200 *) param;
+
+ segway->read_thread();
+}
+
+void CSegwayRMP200::read_thread(void)
+{
+ std::list<std::string> event_list;
+ int packet_len=0,event_id;
+ segway_packet packet;
+ bool end=false;
+
+ event_list.push_back(this->comm_rx_event);
+ event_list.push_back(this->read_finish_event);
+ while(!end)
+ {
+ event_id=this->event_server->wait_first(event_list);
+ if(event_id==0)
+ {
+ try{
+ if(this->read_packet(&packet,&packet_len))
+ this->parse_packet(&packet);
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ throw;
+ }
+ }
+ else
+ {
+ end=true;
+ }
+ }
+
+ pthread_exit(NULL);
+}
+
+void *CSegwayRMP200::start_command_thread(void *param)
+{
+ CSegwayRMP200 *segway = (CSegwayRMP200 *) param;
+
+ segway->command_thread();
+}
+
+void CSegwayRMP200::command_thread(void)
+{
+ segway_packet packet={0xF0,0x55,0x00,0x00,0x00,0x00,0x04,0x13,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+ std::vector<unsigned char> command;
+ int exception_count=0;
+ bool end=false;
+ short int vT,vR;
+
+ while(!end)
+ {
+ usleep(100000);
+ this->access_command.enter();
+ vT=this->vT;
+ vR=this->vR;
+ this->access_command.exit();
+ packet.data[9]=((unsigned char *)&vT)[1];
+ packet.data[10]=((unsigned char *)&vT)[0];
+ packet.data[11]=((unsigned char *)&vR)[1];
+ packet.data[12]=((unsigned char *)&vR)[0];
+ this->access_command.enter();
+ if(this->command_queue.size()>0)
+ {
+ command=this->command_queue.front();
+ this->command_queue.pop();
+ packet.data[13]=command[0];
+ packet.data[14]=command[1];
+ packet.data[15]=command[2];
+ packet.data[16]=command[3];
+ }
+ this->access_command.exit();
+ packet.data[17]=this->compute_checksum(&packet);
+ try{
+ this->comm_dev->write(packet.data,18);
+ if(this->event_server->event_is_set(this->cable_disconnected_event))
+ this->event_server->reset_event(this->cable_disconnected_event);
+ if(this->event_server->event_is_set(this->power_off_event))
+ this->event_server->reset_event(this->power_off_event);
+ }catch(CFTDIException &e){// cable disconnected
+ exception_count=0;
+ if(!this->event_server->event_is_set(this->cable_disconnected_event))
+ this->event_server->set_event(this->cable_disconnected_event);
+ }catch(CCommException &e){// power off and also cable disconnected
+ exception_count++;
+ if(exception_count==2)
+ {
+ exception_count=0;
+ if(!this->event_server->event_is_set(this->power_off_event))
+ this->event_server->set_event(this->power_off_event);
+ }
+ }catch(CException &e){// something else
+ std::cout << e.what() << std::endl;
+ throw;
+ }
+ if(this->event_server->event_is_set(this->command_finish_event))
+ {
+ this->event_server->reset_event(this->command_finish_event);
+ end=true;
+ }
+ }
+
+ pthread_exit(NULL);
+}
+
+void *CSegwayRMP200::heartbeat_thread(void *param)
+{
+ CSegwayRMP200 *segway = (CSegwayRMP200 *) param;
+ std::list<std::string> events;
+
+ events.push_back(segway->heartbeat_event);
+ while(1)
+ {
+ try{
+ segway->event_server->wait_all(events,200);
+ }catch(CEventTimeoutException &e){
+ if(!segway->event_server->event_is_set(segway->no_heartbeat_event))
+ segway->event_server->set_event(segway->no_heartbeat_event);
+ }
+ }
+
+ pthread_exit(NULL);
+}
+
+// configuration functions
+void CSegwayRMP200::set_velocity_scale_factor(float factor)
+{
+ std::vector<unsigned char> command;
+ short int value;
+
+ if(factor < 0.0 || factor > 1.0)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Invalid scale factor for velocity",this->id);
+ }
+ else
+ {
+ command.resize(4);
+ value=(short int)(factor*16);
+ command[0]=0;
+ command[1]=10;
+ command[2]=((unsigned char *)&value)[1];
+ command[3]=((unsigned char *)&value)[0];
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+ }
+}
+
+void CSegwayRMP200::set_acceleration_scale_factor(float factor)
+{
+ std::vector<unsigned char> command;
+ short int value;
+
+ if(factor < 0.0 || factor > 1.0)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Invalid scale factor for acceleration",this->id);
+ }
+ else
+ {
+ command.resize(4);
+ value=(short int)(factor*16);
+ command[0]=0;
+ command[1]=11;
+ command[2]=((unsigned char *)&value)[1];
+ command[3]=((unsigned char *)&value)[0];
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+ }
+}
+
+void CSegwayRMP200::set_turnrate_scale_factor(float factor)
+{
+ std::vector<unsigned char> command;
+ short int value;
+
+ if(factor < 0.0 || factor > 1.0)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Invalid scale factor for turn rate",this->id);
+ }
+ else
+ {
+ command.resize(4);
+ value=(short int)(factor*16);
+ command[0]=0;
+ command[1]=12;
+ command[2]=((unsigned char *)&value)[1];
+ command[3]=((unsigned char *)&value)[0];
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+ }
+}
+
+void CSegwayRMP200::set_gain_schedule(gain value)
+{
+ std::vector<unsigned char> command;
+
+ command.resize(4);
+ command[0]=0;
+ command[1]=13;
+ command[2]=((unsigned char *)&value)[1];
+ command[3]=((unsigned char *)&value)[0];
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+}
+
+void CSegwayRMP200::set_currentlimit_scale_factor(float factor)
+{
+ std::vector<unsigned char> command;
+ short int value;
+
+ if(factor < 0.0 || factor > 1.0)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Invalid scale factor for current limit",this->id);
+ }
+ else
+ {
+ command.resize(4);
+ value=(short int)(factor*256);
+ command[0]=0;
+ command[1]=14;
+ command[2]=((unsigned char *)&value)[1];
+ command[3]=((unsigned char *)&value)[0];
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+ }
+}
+
+void CSegwayRMP200::lock_balance(void)
+{
+ std::vector<unsigned char> command;
+
+ command.resize(4);
+ command[0]=0;
+ command[1]=15;
+ command[2]=0;
+ command[3]=1;
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+}
+
+void CSegwayRMP200::unlock_balance(void)
+{
+ std::vector<unsigned char> command;
+
+ command.resize(4);
+ command[0]=0;
+ command[1]=15;
+ command[2]=0;
+ command[3]=0;
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+}
+
+void CSegwayRMP200::set_operation_mode(op_mode mode)
+{
+ std::vector<unsigned char> command;
+std::cout << "set_operation_mode::hardware_mode=" << hardware_mode << " vs. mode=" << mode << std::endl;
+ if(this->hardware_mode==mode)
+ {
+ command.resize(4);
+ command[0]=0;
+ command[1]=16;
+ command[2]=((unsigned char *)&mode)[1];
+ command[3]=((unsigned char *)&mode)[0];
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"The platform current operation mode does not coincide with the desiered mode",this->id);
+ }
+}
+
+void CSegwayRMP200::reset_right_wheel_integrator(void)
+{
+ std::vector<unsigned char> command;
+
+ command.resize(4);
+ command[0]=0;
+ command[1]=50;
+ command[2]=0;
+ command[3]=1;
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+}
+
+void CSegwayRMP200::reset_left_wheel_integrator(void)
+{
+ std::vector<unsigned char> command;
+
+ command.resize(4);
+ command[0]=0;
+ command[1]=50;
+ command[2]=0;
+ command[3]=2;
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+}
+
+void CSegwayRMP200::reset_yaw_integrator(void)
+{
+ std::vector<unsigned char> command;
+
+ command.resize(4);
+ command[0]=0;
+ command[1]=50;
+ command[2]=0;
+ command[3]=4;
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+}
+
+void CSegwayRMP200::reset_forward_integrator(void)
+{
+ std::vector<unsigned char> command;
+
+ command.resize(4);
+ command[0]=0;
+ command[1]=50;
+ command[2]=0;
+ command[3]=8;
+ this->access_command.enter();
+ this->command_queue.push(command);
+ this->access_command.exit();
+}
+
+// status functions
+float CSegwayRMP200::get_pitch_angle(void)
+{
+ return this->pitch_angle;
+}
+
+float CSegwayRMP200::get_pitch_rate(void)
+{
+ return this->pitch_rate;
+}
+
+float CSegwayRMP200::get_roll_angle(void)
+{
+ return this->roll_angle;
+}
+
+float CSegwayRMP200::get_roll_rate(void)
+{
+ return this->roll_rate;
+}
+
+float CSegwayRMP200::get_left_wheel_velocity(void)
+{
+ return this->left_wheel_velocity;
+}
+
+float CSegwayRMP200::get_right_wheel_velocity(void)
+{
+ return this->right_wheel_velocity;
+}
+
+float CSegwayRMP200::get_yaw_rate(void)
+{
+ return this->yaw_rate;
+}
+
+float CSegwayRMP200::get_servo_frames(void)
+{
+ return this->servo_frames;
+}
+
+float CSegwayRMP200::get_left_wheel_displacement(void)
+{
+ return this->left_wheel_displ;
+}
+
+float CSegwayRMP200::get_right_wheel_displacement(void)
+{
+ return this->right_wheel_displ;
+}
+
+float CSegwayRMP200::get_forward_displacement(void)
+{
+ return this->forward_displ;
+}
+
+float CSegwayRMP200::get_yaw_displacement(void)
+{
+ return this->yaw_displ;
+}
+
+float CSegwayRMP200::get_left_motor_torque(void)
+{
+ return this->left_torque;
+}
+
+float CSegwayRMP200::get_right_motor_torque(void)
+{
+ return this->right_torque;
+}
+
+op_mode CSegwayRMP200::get_operation_mode(void)
+{
+ return this->mode;
+}
+
+op_mode CSegwayRMP200::get_hardware_operation_mode(void)
+{
+ return this->hardware_mode;
+}
+
+gain CSegwayRMP200::get_gain_schedule(void)
+{
+ return this->gain_schedule;
+}
+
+float CSegwayRMP200::get_ui_battery_voltage(void)
+{
+ return this->ui_battery;
+}
+
+float CSegwayRMP200::get_powerbase_battery_voltage(void)
+{
+ return this->powerbase_battery;
+}
+
+void CSegwayRMP200::connect(const std::string& desc_serial)
+{
+ std::string serial_number = desc_serial;
+ std::list<std::string> events;
+
+ // rescan the bus to update the local information
+ this->ftdi_server->scan_bus();
+
+ //if no serial number is provided
+ if(desc_serial.compare("") == 0)
+ {
+ std::vector<int> ftdi_devs;
+ ftdi_devs = this->ftdi_server->get_ids_by_description(CSegwayRMP200::description);
+
+ //check how many segway devices are connected
+ //launch exception if there are no segways or there is more than one
+ if(ftdi_devs.size() != 1)
+ {
+ if(ftdi_devs.size() == 0)
+ throw CSegwayRMP200Exception(_HERE_, "No segways available", this->id);
+ else
+ {
+ throw CSegwayRMP200Exception(_HERE_, "More than one segway available", this->id);
+ }
+ }
+ serial_number = this->ftdi_server->get_serial_number(ftdi_devs.at(0));
+ }
+
+ TFTDIconfig ftdi_config;
+
+ if(this->comm_dev!=NULL)
+ this->close();
+
+ this->comm_dev=this->ftdi_server->get_device(serial_number);
+
+ ftdi_config.baud_rate = 460800;
+ ftdi_config.word_length = -1;
+ ftdi_config.stop_bits = -1;
+ ftdi_config.parity = -1;
+ ftdi_config.read_timeout = 1000;
+ ftdi_config.write_timeout = 1000;
+ ftdi_config.latency_timer = 1;
+
+ this->comm_dev->config(&ftdi_config);
+ this->comm_rx_event=this->comm_dev->get_rx_event_id();
+ this->thread_server->start_thread(this->read_thread_id);
+ this->thread_server->start_thread(this->command_thread_id);
+
+
+ // wait until the state has been updated for the first time
+ events.push_back(this->new_status_event);
+ events.push_back(this->heartbeat_event);
+ try
+ {
+ this->event_server->wait_all(events, 2000);
+ this->event_server->wait_all(events, 2000);
+ }
+ catch(CEventTimeoutException e)
+ {
+ this->close();
+ throw CSegwayRMP200Exception(_HERE_,"Segway connection not ready",this->id);
+ }
+
+ this->thread_server->start_thread(this->heartbeat_thread_id);
+}
+
+void CSegwayRMP200::reset(void)
+{
+ segway_packet packet={0xF0,0x55,0x00,0x00,0x00,0x00,0x04,0x12,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+ if(this->comm_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"Communciation device not set",this->id);
+ }
+ else
+ {
+ packet.data[17]=this->compute_checksum(&packet);
+ this->comm_dev->write(packet.data,18);
+ }
+
+}
+
+void CSegwayRMP200::move(float vT,float vR)
+{
+std::cout << "move::hardware_mode=" << hardware_mode << " vs. mode=" << mode << std::endl;
+ if(this->mode!=this->hardware_mode)
+ {
+ /* handle exceptions */
+ throw CSegwayRMP200Exception(_HERE_,"The desired operation mode does not coincide with the one set in the platform.",this->id);
+ }
+ else
+ {
+ this->access_command.enter();
+ this->vT = (short int)(vT*3.6*147.0/1.609344);
+ this->vR = (short int)(vR*1024.0);
+ this->access_command.exit();
+ }
+}
+
+void CSegwayRMP200::stop(void)
+{
+ this->access_command.enter();
+ this->vT = 0;
+ this->vR = 0;
+ this->access_command.exit();
+}
+
+void CSegwayRMP200::close()
+{
+ //kill threads
+ if(this->comm_dev!=NULL)
+ {
+ // finish the threads
+ this->event_server->set_event(this->read_finish_event);
+ this->event_server->set_event(this->command_finish_event);
+ this->thread_server->end_thread(this->read_thread_id);
+ this->thread_server->end_thread(this->command_thread_id);
+ this->thread_server->kill_thread(this->heartbeat_thread_id);
+ /* reset the events if necessary */
+ this->event_server->reset_event(this->read_finish_event);
+ this->event_server->reset_event(this->command_finish_event);
+ if(this->event_server->event_is_set(this->no_heartbeat_event))
+ this->event_server->reset_event(this->no_heartbeat_event);
+ if(this->event_server->event_is_set(this->power_off_event))
+ this->event_server->reset_event(this->power_off_event);
+ if(this->event_server->event_is_set(this->cable_disconnected_event))
+ this->event_server->reset_event(this->cable_disconnected_event);
+ this->comm_dev->close();
+ delete this->comm_dev;
+ this->comm_dev=NULL;
+ }
+}
+
+CSegwayRMP200::~CSegwayRMP200()
+{
+ this->close();
+ /* destroy the events */
+ this->event_server->delete_event(this->read_finish_event);
+ this->read_finish_event="";
+ this->event_server->delete_event(this->command_finish_event);
+ this->command_finish_event="";
+ this->event_server->delete_event(this->cable_disconnected_event);
+ this->cable_disconnected_event="";
+ this->event_server->delete_event(this->power_off_event);
+ this->power_off_event="";
+ this->event_server->delete_event(this->no_heartbeat_event);
+ this->no_heartbeat_event="";
+ this->event_server->delete_event(this->heartbeat_event);
+ this->heartbeat_event="";
+ this->event_server->delete_event(this->new_status_event);
+ this->new_status_event="";
+ // destroy the threads
+ this->thread_server->delete_thread(this->read_thread_id);
+ this->read_thread_id="";
+ this->thread_server->delete_thread(this->command_thread_id);
+ this->command_thread_id="";
+ this->thread_server->delete_thread(this->heartbeat_thread_id);
+ this->heartbeat_thread_id="";
+}
+
+std::ostream& operator<< (std::ostream& out, CSegwayRMP200& segway)
+{
+ segway.access_status.enter();
+ out << "Pitch angle: " << segway.pitch_angle << " degrees" << std::endl;
+ out << "Pitch rate " << segway.pitch_rate << " degrees/s" << std::endl;
+ out << "Roll angle: " << segway.roll_angle << " degrees" << std::endl;
+ out << "Roll rate: " << segway.roll_rate << " degrees/s" << std::endl;
+ out << "Left wheel velocity: " << segway.left_wheel_velocity << " m/s" << std::endl;
+ out << "Right wheel velocity: " << segway.right_wheel_velocity << " m/s" << std::endl;
+ out << "Yaw rate: " << segway.yaw_rate << " degrees/s" << std::endl;
+ out << "Servo frames: " << segway.servo_frames << " frames/s" << std::endl;
+ out << "Left wheel displacement: " << segway.left_wheel_displ << " m" << std::endl;
+ out << "Right wheel displacement: " << segway.right_wheel_displ << " m" << std::endl;
+ out << "Forward displacement: " << segway.forward_displ << " m" << std::endl;
+ out << "Yaw displacement: " << segway.yaw_displ << " rev" << std::endl;
+ out << "Left motor torque: " << segway.left_torque << " Nm" << std::endl;
+ out << "Right motor torque: " << segway.right_torque << " Nm" << std::endl;
+ if(segway.mode==tractor)
+ out << "Operation mode: tractor" << std::endl;
+ else if(segway.mode==balance)
+ out << "Operation mode: balance" << std::endl;
+ else
+ out << "Operation mode: power down" << std::endl;
+ if(segway.hardware_mode==tractor)
+ out << "Hardware operation mode: tractor" << std::endl;
+ else if(segway.hardware_mode==balance)
+ out << "Hardware operation mode: balance" << std::endl;
+ else
+ out << "Hardware operation mode: power down" << std::endl;
+ if(segway.gain_schedule==light)
+ out << "Gain schedule: light" << std::endl;
+ else if(segway.gain_schedule==tall)
+ out << "Gain schedule: tall" << std::endl;
+ else
+ out << "Gain schedule: heavy" << std::endl;
+ out << "UI battery voltage: " << segway.ui_battery << " V" << std::endl;
+ out << "Powerbase battery voltage: " << segway.powerbase_battery << " V" << std::endl;
+ segway.access_status.exit();
+
+ return out;
+}
diff --git a/src/segway_rmp200.h b/src/segway_rmp200.h
new file mode 100755
index 0000000000000000000000000000000000000000..e83f0369f8a670a1e3dbc0c3e35cd363b4c1c4e5
--- /dev/null
+++ b/src/segway_rmp200.h
@@ -0,0 +1,1421 @@
+#ifndef _SEGWAYRMP200_DRIVER_H
+#define _SEGWAYRMP200_DRIVER_H
+
+#include "ftdimodule.h"
+#include "ftdiserver.h"
+#include "mutex.h"
+#include <string>
+#include <vector>
+#include <queue>
+
+/**
+ * \brief structure of a USB packet
+ *
+ * This new data type has all the information of a data packet send to or received
+ * from the USB communication device. This data type is only 18 unsigned char values
+ * arranged as a vector.
+ *
+ */
+typedef struct
+{
+ /**
+ * \brief packet data
+ *
+ * This vector has all the data of a packet that has been received or that is going
+ * to be sent. The format of this packet is as follows:
+ *
+ */
+ unsigned char data[18];
+}segway_packet;
+
+/**
+ * \brief Valid gain schedules for the segway
+ *
+ * This new data type enumerates all the possible gain schedules of a segway platform.
+ * This data type is only used on the balancing mode and ignored in the tractor mode.
+ * The possible gain schedule values are:
+ *
+ * - light: for relativelly small weights (20 kg) placed near the platform.
+ * - tall: for relativelly small weights (20 kg) distributed along the vertical axis
+ * of the robot.
+ * - heavy: for heavy weights (40 kg) placed near the platform.
+ *
+ */
+typedef enum {light,tall,heavy} gain;
+
+/**
+ * \brief Valid operation modes of the segway
+ *
+ * This new data type enumerates all the possible operation modes of a segway platform.
+ * The possible operation modes are:
+ *
+ * - tractor: in this mode the balancing feature is disabled and the platform needs an
+ * additional support point (castor wheel). In this mode it is possible to
+ * command the robot to move forward/reverse and also turn.
+ * - balance: in this mode the segway balances itself, trying to keep the given position.
+ * In this mode it is possible to move the robot without a castor wheel.
+ * - power down: the platform is shut down and no power is provided to the motors. It is
+ * possible to enter this mode from either the tractor and balance modes.
+ *
+ */
+typedef enum {tractor=1,balance=2,power_down=3} op_mode;
+
+/**
+ * \brief Segway RMP 200 driver
+ *
+ * This class implements the basic interface to communcate with an RMP200 segway platform,
+ * configure it and also send motion commands. By default the communication with the
+ * hardware platform is achieved through USB.
+ *
+ * At construction time the object tries to connect to a platform. If only one platform is
+ * connected to the computer, the default constructor can be used, but if there exist
+ * multiple segway platforms, it is necessary to pass the serial number to the constructor
+ * In order to connect to the desired one and avoid errors.
+ *
+ * When the object is properly contructed, the two main threads of the class start. These
+ * threads are:
+ *
+ * - The command thread: which is responsible of sending the mnotion command to the platform
+ * at regular intervals. Currently the update rate for the motion
+ * command is fixed to 50 times per second.
+ *
+ * When the user gives a motion command using the move() or stop()
+ * functions, the new command is not immediatelly send to the robot,
+ * but stored into internal parameters of the class. It is this thread
+ * that sends the new commands to the robot at regular intervals. The
+ * same happens with all the configuration parameters, when the
+ * corresponding function is called, the command is internally stored
+ * and send in the next command packet to the platform.
+ *
+ * At the moment it is not possible to change the update rate of the
+ * motion commands.
+ *
+ * - The feedback thread: this thread is responsible of reding all the data sent by the
+ * robot platform and store it in the internal parameters of the
+ * class as fast as they are sent.
+ *
+ * When the user tries to read the value of one of the parameters
+ * of the platform, the returned value is the one stored inside
+ * the class, which are updated by this thread.
+ *
+ * At the moment only data messages from 2 to 7 are handled, as well
+ * as the heart beat. The other messages are received but ignored.
+ *
+ * The public interface of this class alows to completelly configure all the parameters of
+ * the segway platform as well as monitor each of the available feedback parameters. Also
+ * there exist an operator << to show all feedback information already formated.
+ *
+ * This class also has support for unexpected hardware events such as diconnection of the
+ * USB cable, turning the platform power off or no reception of the heartbeat message for
+ * a long period. Primary, this actions are reported by dedicated events that are created
+ * internally at construction time. These events are:
+ *
+ * - cable_disconnected_event: when activated, it notifies that the USB cable has been
+ * disconnected from the platform. When this happens, the normal
+ * operation of the class is interrupted, but it does not crash.
+ * After this event is received, it will be necessary to call the
+ * close() function and the connect() function again to restore
+ * normal operation.
+ *
+ * When the connection is restored, this event is automatically
+ * cleared.
+ *
+ * - power_off_event : when activated, it notifies that the platform is not powerd on. This
+ * normally happens at start up when the green button on the platform is
+ * not lid or when an error ocurrs. When this event is active, the normal
+ * operation of the class is interrupted, but it is not necessary to
+ * close and reconnect the object. When the problem is solved, the class
+ * automatically returns to the normal operation mode.
+ *
+ * When the connection is restored, this event is automatically cleared.
+ *
+ * - no_heartbeat_event : when activated, it notifies that the platform is turned on, but no
+ * data is received from it. The class has an internal thread that
+ * works as a watchdog: when a heartbeat message is received, the
+ * watchdog counter is reset, but if this message is not received for
+ * a long time, the watchdog times out and generates an event.
+ *
+ * When this event is active, the normal operation of the class is
+ * interrupted, but it is not necessary to close and reconnect the
+ * object. When the problem is solved, the class automatically returns
+ * to the normal operation. When the connection is restored, this event
+ * is automatically cleared.
+ *
+ * Finally, in addition to the previously defined events, the class also provide an event to
+ * notify when a new complete status data is available. This event can be used to avoid
+ * reading the same data several times.
+ */
+class CSegwayRMP200
+{
+ public:
+ /**
+ * \brief segway PID
+ *
+ * This constant holds the PID number used by the segway platforms. It is defined
+ * as static so it can be used even if no instance of this class exists.
+ */
+ static const short int pid;
+ /**
+ * \brief Segway platform description
+ *
+ * This constant holds the description string of the segway platforms. It is
+ * defined as static so it can be used even if no instance of this class
+ * exists.
+ *
+ * This description can be used in the FTDI server to identify the segway platforms
+ * from any other FTDI device connected to the computer.
+ */
+ static const std::string description;
+ private:
+ /**
+ * \brief unique identifier of the segway platform
+ *
+ * This string has the unique identifier of the segway platform. This string is
+ * initialized at construction time and can not be modified afterwards. The
+ * get_segway_id() function can be used to get this string.
+ *
+ * This string is also used to create unique identifier for all the threads and
+ * events used inside the class. This identifier is build from the serial number
+ * of the segway platform to which it is connected.
+ *
+ */
+ std::string id;
+ /**
+ * \brief mutex for the status data
+ *
+ * This mutex is used to control the access to the status variables (shared memory)
+ * of the class. These variables are periodically updated by the feedback thread
+ * and they can be read by the user at any time using the get functions.
+ *
+ * This mutex is used by the feedback thread and all the get status function to
+ * avoid data corruption while simultaneously reading and writing to the status
+ * variable of the segway platform. There exist a different mutex for the command
+ * data.
+ *
+ */
+ CMutex access_status;
+ /**
+ * \brief mutex for the command data
+ *
+ * This mutex is used to control the access to the command variables (shared memory)
+ * of the class. These variables are changed by the user at any time using the mov()
+ * or stop() functions, and the new command is actually send to the segway platform
+ * periodically by the command thread.
+ *
+ * This mutex is used by the command thread and all the motion function to avoid data
+ * corruption while simultaneously reading and writing to the command variable of the
+ * segway platform. There exist a different mutex for the status data.
+ */
+ CMutex access_command;
+ /**
+ * \brief a reference to the FTDI USB device
+ *
+ * This attribute points to the communication device used to send and receive data
+ * to and from the segway platform. The communication device is cretaed inside the
+ * object at initialization time using the CFTDIServer and the description or serial
+ * number of the desired platform via the connect() function.
+ *
+ * It is not possible to send or receive data to or from the platform until the
+ * driver is not connected to the hardware platform. Any attempt to do so will
+ * result in an exception being thrown.
+ *
+ */
+ CFTDI *comm_dev;
+ /**
+ * \brief reference to the unique ftdi_server
+ *
+ * This reference to the unique ftdi server is initialized when an object of
+ * this class is first created. It is used to create and handle all the
+ * FTDI device drivers used to access the segway platform. The object pointed
+ * by this reference is shared by all objects in any application.
+ *
+ */
+ CFTDIServer *ftdi_server;
+ /**
+ * \brief Reference to the unique event handler
+ *
+ * This reference to the unique event handler is initialized when an object of
+ * this class is first created. It is used to create and handle all the
+ * segway platform events. The object pointed by this reference is shared by all
+ * objects in any application.
+ */
+ CEventServer *event_server;
+ /**
+ * \brief Reference to the unique thread handler
+ *
+ * This reference to the unique thread handler is initialized when an object of
+ * this class is first created. It is used to create and handle all the
+ * segway platform threads. The object pointed by this reference is shared by all
+ * objects in any application.
+ */
+ CThreadServer *thread_server;
+ /**
+ * \brief identifier of the feedback thread
+ *
+ * This string has the identifier of the thread used to read status information from
+ * the segway platform. This string is initialized at contruction time by appending
+ * the unique identifier of the segway object and the string "_read_thread". This
+ * thread is only used internally to the class, so it is not possible to get its
+ * identifier out.
+ */
+ std::string read_thread_id;
+ /**
+ * \brief identifer of the command thread
+ *
+ * This string has the identifier of the thread used to send motion commands to
+ * the segway platform. This string is initialized at contruction time by appending
+ * the unique identifier of the segway object and the string "_command_thread". This
+ * thread is only used internally to the class, so it is not possible to get its
+ * identifier out.
+ */
+ std::string command_thread_id;
+ /**
+ * \brief segway heartbeat thread
+ *
+ * This string has the idetifier of the thread used to check the periodic reception
+ * of the heartbeat signal send by the segway platform. This string is initialized
+ * at contruction time by appending the unique identifier of the segway object and
+ * the string "_heartbeat_thread". This thread is only used internally to the class,
+ * so it is not possible to get its identifier out.
+ */
+ std::string heartbeat_thread_id;
+ /**
+ * \brief identifier of the reception event of the communication device
+ *
+ * This string has the identifier of the event generated by the communcation device
+ * to signal de reception of new data. This event is not created by this class, but
+ * retrieved from the communication device when it is attached to this class. This
+ * event is used by the read thread to awake when new data is received.
+ */
+ std::string comm_rx_event;
+ /**
+ * \brief identifier of the event to finish the feedback thread
+ *
+ * This string has the identifier of the event used to signal the feedback thread
+ * to end its execution. This string is initialized at contruction time by appending
+ * the unique identifier of the segway object and the string "_finish_read_thread".
+ * This event is only used inside the class, so it is not possible to get its
+ * identifier out.
+ */
+ std::string read_finish_event;
+ /**
+ * \brief identifier of the event to finish the command thread
+ *
+ * This string has the identifier of the event used to signal the command thread
+ * to end its execution. This string is initialized at contruction time by appending
+ * the unique identifier of the segway object and the string "_finish_command_thread".
+ * This event is only used inside the class, so it is not possible to get its
+ * identifier out.
+ */
+ std::string command_finish_event;
+ /**
+ * \brief identifier of the event to signal that the platform is powered off
+ *
+ * This string has the identifier of the event used to signal that the platform is
+ * powered off. This string is initialized at contruction time by appending
+ * the unique identifier of the segway object and the string "_power_off_event".
+ * Use the get_power_off_event() function to get the event identifier from
+ * outside the class.
+ */
+ std::string power_off_event;
+ /**
+ * \brief identifier of the event to signal that the cable is disconnected
+ *
+ * This string has the identifier of the event used to signal that the USB
+ * cable is disconnected. This string is initialized at contruction time by
+ * appending the unique identifier of the segway object and the string
+ * "_cable_disconnected_event". Use the get_cable_disconnected_event()
+ * function to get the event identifier from outside the class.
+ */
+ std::string cable_disconnected_event;
+ /**
+ * \brief identifier of the event to signal the availability of new status data
+ *
+ * This string has the identifier of the event used to signal the availability
+ * of new status data. This string is initialized at contruction time by
+ * appending the unique identifier of the segway object and the string
+ * "_new_status_event". Use the get_new_status_event() function to
+ * get the event identifier from outside the class.
+ */
+ std::string new_status_event;
+ /**
+ * \brief identifeir of the event to signal the absence of the heartbeat signal
+ *
+ * This string has the identifier of the event used to signal that no heartbeat
+ * has been received for a while. This string is initialized at contruction time
+ * by appending the unique identifier of the segway object and the string
+ * "_no_heartbeat_event". Use the get_no_heartbeat_event() function to
+ * get the event identifier from outside the class.
+ */
+ std::string no_heartbeat_event;
+ /**
+ * \brief identifeir of the internal heartbeat event
+ *
+ * This string has the identifier of the heartbeat event .This string is
+ * initialized at contruction time by appending the unique identifier of
+ * the segway object and the string "_heartbeat_event". This event is only
+ * used inside the class, so it is not possible to get its identifier out.
+ */
+ std::string heartbeat_event;
+ /**
+ * \brief class attributes initialization
+ *
+ * This method initalizes all the internal class variables at construction time.
+ */
+ void init_attributes(void);
+ /**
+ * \brief ftdi server initialization
+ *
+ * This method initializes the ftdi_server to handle all the FTDI devices
+ * and also adds the specific PID of the segway platforms in order to be able
+ * to detect and list them.
+ */
+ void init_ftdi(void);
+ /**
+ * \brief function to initialize the class threads
+ *
+ * This method initializes all the internal threads of the class. This includes
+ * the read and command threads and also the heartbeat thread.
+ *
+ * All the threads are created and assigned their corresponding execution function
+ * but they are not started until the connection to the platform is done, either
+ * at construction time or when the connect function is called.
+ */
+ void init_threads(void);
+ /**
+ * \brief function to initialize the class events
+ *
+ * This function initializes all the internal and external events of the class.
+ *
+ */
+ void init_events(void);
+ protected:
+ // status variables
+ /**
+ * \brief left wheel velocity
+ *
+ * This value has the velocity of the left wheel in meters per second. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_left_wheel_velocity() function or the
+ * overloaded operator <<.
+ */
+ float left_wheel_velocity;
+ /**
+ * \brief right wheel velovity
+ *
+ * This value has the velocity of the right wheel in meters per second. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_right_wheel_velocity() function or the
+ * overloaded operator <<.
+ */
+ float right_wheel_velocity;
+ /**
+ * \brief pitch angle
+ *
+ * This value has the pitch angle of the platform in degrees. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_pitch_angle() function or the
+ * overloaded operator <<.
+ */
+ float pitch_angle;
+ /**
+ * \brief pitch rate
+ *
+ * This value has the pitch rate of the platform in degrees per second. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_pitch_rate() function or the
+ * overloaded operator <<.
+ */
+ float pitch_rate;
+ /**
+ * \brief roll angle
+ *
+ * This value has the roll angle of the platform in degrees. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_roll_angle() function or the
+ * overloaded operator <<.
+ */
+ float roll_angle;
+ /**
+ * \brief roll rate
+ *
+ * This value has the roll rate of the platform in degrees per second. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_roll_rate() function or the
+ * overloaded operator <<.
+ */
+ float roll_rate;
+ /**
+ * \brief yaw rate
+ *
+ * This value has the yaw rate of the platform in degrees per second. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_yaw_rate() function or the
+ * overloaded operator <<.
+ */
+ float yaw_rate;
+ /**
+ * \brief number of frames per second
+ *
+ * This value has the number of servo frames in fames per second. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_servo_frames() function or the
+ * overloaded operator <<.
+ */
+ float servo_frames;
+ /**
+ * \brief left wheel displacement
+ *
+ * This value has the displacement of the left wheel in meters. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_left_wheel_displacement() function
+ * or the overloaded operator <<.
+ */
+ float left_wheel_displ;
+ /**
+ * \brief right wheel displacement
+ *
+ * This value has the displacement of the right wheel in meters. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_right_wheel_displacement() function
+ * or the overloaded operator <<.
+ */
+ float right_wheel_displ;
+ /**
+ * \brief forward displacement
+ *
+ * This value has the displacement of the whole platform in meters. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_forward_displacement() function
+ * or the overloaded operator <<.
+ */
+ float forward_displ;
+ /**
+ * \brief yaw displacement
+ *
+ * This value has the rotation of the whole platform in revolutions. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_yaw_displacement() function
+ * or the overloaded operator <<.
+ */
+ float yaw_displ;
+ /**
+ * \brief left motor torque
+ *
+ * This value has the torque of the left motor in Newtorn per meter. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_left_motor_torque() function
+ * or the overloaded operator <<.
+ */
+ float left_torque;
+ /**
+ * \brief right motor torque
+ *
+ * This value has the torque of the right motor in Newtorn per meter. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_right_motor_torque() function
+ * or the overloaded operator <<.
+ */
+ float right_torque;
+ /**
+ * \brief operation mode
+ *
+ * This value has the current operation mode of the platform. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_operation_mode() function
+ * or the overloaded operator <<. The possible operation modes are:
+ *
+ * - tractor
+ * - balance
+ * - power down
+ *
+ * See the documentation of the op_mode data type for more information on the
+ * possible operation modes.
+ */
+ op_mode mode;
+ /**
+ * \brief hardware operation mode
+ *
+ * This value has the current hardware operation mode of the platform. This
+ * value is periodically updated by the read thread and it is not possible to
+ * modify it otherwise. To get its value use the get_hardware_operation_mode()
+ * function or the overloaded operator <<. The possible operation modes are:
+ *
+ * - tractor
+ * - balance
+ * - power down
+ *
+ * See the documentation of the op_mode data type for more information on the
+ * possible operation modes.
+ *
+ * This operation mode coincides with the platfrom settings (which of the two
+ * blue buttons are on). This operation mode must coincide with the operation
+ * mode set by the user.
+ */
+ op_mode hardware_mode;
+ /**
+ * \brief gain schedule
+ *
+ * This value has the current gain schedule of the platform. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_gain_schedule() function
+ * or the overloaded operator <<. The possible gain schedules are:
+ *
+ * - light
+ * - tall
+ * - heavy.
+ *
+ * See the documentation of the gain data type for more information on the
+ * possible gain schedule.
+ */
+ gain gain_schedule;
+ /**
+ * \brief user battery
+ *
+ * This value has the voltage of the UI battery in Volts. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_ui_battery_voltage() function
+ * or the overloaded operator <<.
+ */
+ float ui_battery;
+ /**
+ * \brief power base battery
+ *
+ * This value has the voltage of the power base battery in Volts. This value is
+ * periodically updated by the read thread and it is not possible to modify it
+ * otherwise. To get its value use the get_powerbase_battery_voltage() function
+ * or the overloaded operator <<.
+ */
+ float powerbase_battery;
+ // command variables
+ /**
+ * \brief translation velocity
+ *
+ * This value has the desired translational velocity in meters per second. This
+ * values is updated each time the move() or stop() functions are called. Then
+ * this value is sent periodically to the segway platform by the command thread.
+ */
+ short int vT;
+ /**
+ * \brief rotation velocity
+ *
+ * This value has the desired rotational velocity in revolutions per second. This
+ * values is updated each time the move() or stop() functions are called. Then
+ * this value is sent periodically to the segway platform by the command thread.
+ */
+ short int vR;
+ /**
+ * \brief internal command queus
+ *
+ * This queue is used to store configuration commands to be send to the platform.
+ * Initially, this queue is empty, and its length increases each time a set() or
+ * reset() function is called. The command thread checks this queue every time it
+ * is about to send a new command to include any configuration data if needed.
+ */
+ std::queue< std::vector<unsigned char> > command_queue;
+ // methods
+ /**
+ * \brief function to compute checksum of USB packets
+ *
+ * This function computes the checksum of a given USB packet either received from
+ * the platform or to be send to it. When a new packet is received this function
+ * is used to check that the packet has no errors. Since the packet is sent with
+ * a checksum, the return value of this function should be 0.
+ *
+ * When a packet is about to be sent, this function is used to compute the
+ * checksum value for the packet. The returned value then is used to complete
+ * the data packet (18th bytes) before it is sent to the segway platform.
+ *
+ * This function is only used internally by the feedback and command threads,
+ * but it can not be used elsewhere.
+ *
+ * \param packet a reference to the packet on which to compute the chcksum. If
+ * the packet has been received it should already has a checksum
+ * value at the 18th position, and otherwise this position should
+ * be set to 0 to correctly compute the checksum.
+ *
+ * \return the value of the checksum. If the packet has been received, this value
+ * should be 0 for a valid packet and any other value otherwise. If the
+ * packet is to be sent, this value is the checksum to be used to complete
+ * the packet.
+ */
+ unsigned char compute_checksum(segway_packet *packet);
+ /**
+ * \brief function to read a whole packet from the segway platform
+ *
+ * This functions is used to read full packets from the segway platform. It first
+ * syncronizes the incomming data to detect the packet header, and then stores all
+ * the data of the packet in the given packet strcuture.
+ *
+ * This function reads all available data and returns immediatelly even if the
+ * whole packet has not been received. If several packets have been received, this
+ * function returns after reading the first one, and the function has to be recalled
+ * until no more data is available. This function is only used internally by the
+ * feedback thread.
+ *
+ * \param packet a reference to a packet structure in which to store the received
+ * packet. The necessary memory for this structure has to be allocated
+ * before calling this function. This contents of this structure are
+ * only valid when the return value of this function is true, otherwise
+ * its contents should be ignored, but not modified since it has
+ * partial packet information.
+ * \param packet_len an integer with the size of the read packet. If the packet is
+ * completely received in a single call, this value is always 18
+ * (the dafault length). In case the packet is not completelly
+ * received in a single call, this parameter is used to hold the
+ * actual number of bytes read, and the same variable must be used
+ * in the next call to this function to guarantee a correct reception
+ * of all data packets.
+ *
+ * \return a flag that indicated if a whole packet has been read (true) or not (false).
+ * only when the return value is true, the contents of the packet parameter
+ * can be used.
+ */
+ bool read_packet(segway_packet *packet,int *packet_len);
+ /**
+ * \brief function to parse whole packets read from the segway platform
+ *
+ * This function is used to extract the data contained into the data packets received.
+ * The checksum is first checked and if there is any error, an exception is thrown.
+ * Otherwise, the data from the packet is stored into the corresponding internal
+ * attributes of the class.
+ *
+ * This function blocks the status mutex to avoid errors while updating the internal
+ * attributes. This way all functions to get the current status will get blocked until
+ * the update is complete.
+ *
+ * \param packet a reference to a packet structure which stores the data of a new
+ * received packet. The packet passed to this function should be the
+ * one returned by the read_packet() function.
+ *
+ */
+ void parse_packet(segway_packet *packet);
+ /**
+ * \brief Thread function that reads data from segway
+ *
+ * This functions waits for any data to be received from the segway platform. When a whole
+ * packet is read from the robot, it is parsed and then the internal information of the class
+ * updated.
+ *
+ * This thread is created at construction time but it is not started until the communication
+ * device is attached to avoid errors. This thread is always active until the finish event is
+ * activated, which happens when the close() function is called.
+ *
+ * \return in this case, the function always return NULL, so no data is returned to the
+ * main thread.
+ */
+ void read_thread(void);
+ /**
+ * \brief start read thread
+ *
+ * This function starts the read thread.
+ *
+ * \param param a pointer to the object itself. since this function is statis, there exist no
+ * this pointer, so this parameter is used to access the internal attributes of
+ * the class.
+ */
+ static void *start_read_thread(void *param);
+ /**
+ * \brief Thread function that sends commands to the segway
+ *
+ * This function is used to send motion commands to the segway platform periodically. This
+ * functions gets the internal motion commands (translational and rotational velocities) from
+ * the class and sends it to the robot every 20ms. At this moment it is not possible to
+ * change the update drate of the motion commandsi.
+ *
+ * This thread also checks the internal command queue to check if there is any configuration
+ * data avaialble. If so, it reads the contents of the first commans and includes it into the
+ * packet data to be send. If no new configuration data is available, this thread only sends
+ * motion commands.
+ *
+ * This thread is created at construction time but it is not started until the communication
+ * device is attached to avoid errors. This thread is always active until the finish event is
+ * activated, which happens when the close() function is called. If this thread ends unexpectly
+ * the robot will stop after a few moments, since it requires constant commands from the host
+ * computer to continued operation.
+ *
+ * To change the current motion command, use the move() function. The stop() function should
+ * be used to immediatelly stop the motors. These functions will modify the internal attributes
+ * of the class, and this thread will be the one responsible of sending the new commands to
+ * the robot.
+ *
+ * \return in this case, the function always return NULL, so no data is returned to the
+ * main thread.
+ *
+ */
+ void command_thread(void);
+ /**
+ * \brief start command thread
+ *
+ * This function starts the command thread.
+ *
+ * \param param a pointer to the object itself. since this function is statis, there exist no
+ * this pointer, so this parameter is used to access the internal attributes of
+ * the class.
+ */
+ static void *start_command_thread(void *param);
+ /**
+ * \brief heartbeat thread function
+ *
+ * This threads waits for the heartbeat event generated by the reception of a specific
+ * data packet from the segway platform for a given ammount of time.
+ *
+ * While this data packet is received the thread does nothing. When the communications
+ * are interrupted or the power is turned off, the allowed ammount of time elapses
+ * and the external no_heartbeat_event is set to notify the error to the user. The
+ * operation of this thread is equivalent to a watchdog.
+ *
+ * If the problem is solved, the event is reset and the normal operation of the
+ * driver continues.
+ */
+ static void *heartbeat_thread(void *param);
+ public:
+ /**
+ * \brief constructor
+ *
+ * This constructor creates and initializes all the threads and events nedded by the class.
+ * However, the threads are not started until the connect() function is called. This
+ * constructor accepts a string with the serial number of the segway platform, which is used to
+ * create the unique identifiers of both events and threads.
+ *
+ * If no serial number is provided, the class tries to automatically connect to the
+ * platfrom. In this case is more or less than one platforms are connected to the
+ * computer, the constructor throws an exception.
+ *
+ * \param desc_serial a null terminated string with the description or the serial
+ * number of the segway platform to associate to the driver.
+ * This string can be obtanied through the CFTDIServer or else
+ * hardcoded if known.
+ */
+ CSegwayRMP200(const std::string& desc_serial="");
+ /**
+ * \brief function to get the identifeir of the segway
+ *
+ * This function is used to get the unique identifier of the segway platform associated
+ * to the driver.
+ *
+ * \return a null terminated string with the identifier of the segway.
+ */
+ std::string get_id(void);
+ /**
+ * \brief function to get the identifier of the cable disconnected event
+ *
+ * This function returns the identifier of the cable disconnected event. This
+ * event can be used by an external application to monitor the state of the
+ * segway platform.
+ */
+ std::string get_cable_disconnected_event(void);
+ /**
+ * \brief function to get the identifier of the power off event
+ *
+ * This function returns the identifier of the power off event. This
+ * event can be used by an external application to monitor the state of the
+ * segway platform.
+ */
+ std::string get_power_off_event(void);
+ /**
+ * \brief function to get the no heartbeat event
+ *
+ * This function returns the identifier of the cable no heartbeat event. This
+ * event can be used by an external applciation to monitor the state of the
+ * segway platform.
+ */
+ std::string get_no_heartbeat_event(void);
+ /**
+ * \brief function to get the new status data available event
+ *
+ * This function returns the identifier of the new status data available event.
+ * This event can be used by an external application to know when there exist
+ * new data, and avoid polling the driver too fast or too slowly.
+ */
+ std::string get_new_status_event(void);
+ // configuration functions
+ /**
+ * \brief function to set the velocity scale factor
+ *
+ * This function is used to set the desired scale factor for the velocity. This scale factor
+ * is used to change the maximum velocity allowed. The given motion commands are multiplied
+ * by this scale factor, thus limiting the maximum possible speed.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ * \param factor the desired scale factor for the velocity. The possible values for this
+ * parameter must be limited between 0.0 and 1.0. Any other value is not
+ * allowed and will throw an exception.
+ */
+ void set_velocity_scale_factor(float factor);
+ /**
+ * \brief function to set the acceleration scale factor
+ *
+ * This function is used to set the desired scale factor for the acceleration. This scale factor
+ * is used to change the maximum acceleration allowed. The given motion commands are multiplied
+ * by this scale factor, thus limiting the maximum possible acceleration.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ * \param factor the desired scale factor for the acceleration. The possible values for this
+ * parameter must be limited between 0.0 and 1.0. Any other value is not
+ * allowed and will throw an exception.
+ */
+ void set_acceleration_scale_factor(float factor);
+ /**
+ * \brief function to set the turnrate scale factor
+ *
+ * This function is used to set the desired scale factor for the turn rate. This scale factor
+ * is used to change the maximum turn rate allowed. The given motion commands are multiplied
+ * by this scale factor, thus limiting the maximum possible turn speed.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ * \param factor the desired scale factor for the turn rate. The possible values for this
+ * parameter must be limited between 0.0 and 1.0. Any other value is not
+ * allowed and will throw an exception
+ */
+ void set_turnrate_scale_factor(float factor);
+ /**
+ * \brief function to set the gain schedule
+ *
+ * This function is used to change the current gain schedule of the segway platform. The gain
+ * schedule is used to improve the balance capabilities of the platform. Depending on the
+ * weight of the payload and on its distribution, this parameter must be changed.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ * \param value the desired value for the gain schedule. This parameter should be one of the
+ * following values:
+ *
+ * - light
+ * - tall
+ * - heavy
+ *
+ * See the documentation on the gain data type for more information on the
+ * different meaning of the gain schedules.
+ */
+ void set_gain_schedule(gain value);
+ /**
+ * \brief function to set the current limit scale factor
+ *
+ * This function is used to set the desired scale factor for the current limit. This scale
+ * factor is used to change the maximum motor current allowed.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ * \param factor the desired scale factor for the current limit. The possible values for this
+ * parameter must be limited between 0.0 and 1.0. Any other value is not
+ * allowed and will throw an exception
+ */
+ void set_currentlimit_scale_factor(float factor);
+ /**
+ * \brief function to lock the balance function
+ *
+ * This function is used to block the balance mode.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ */
+ void lock_balance(void);
+ /**
+ * \brief function to unlock the balance function
+ *
+ * This function is used to unblock the balance mode.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ */
+ void unlock_balance(void);
+ /**
+ * \brief function to set the operation mode
+ *
+ * This function is used to change the current operation mode of the segway platform. The
+ * operation mode is used to change the working mode of the segway platform.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ * \param mode the desired value for the operation mode. This parameter should be one of the
+ * following values:
+ *
+ * - tractor
+ * - balance
+ * - power down
+ *
+ * See the documentation on the op_mode data type for more information on the
+ * different meaning of the operation modes.
+ */
+ void set_operation_mode(op_mode mode);
+ /**
+ * \brief function to reset the right wheel integrator
+ *
+ * This function is used to reeet the right wheel integrator of the segway platform. This
+ * integrator holds the total distance traveled by the right wheel.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ */
+ void reset_right_wheel_integrator(void);
+ /**
+ * \brief function to reset the left wheel integrator
+ *
+ * This function is used to reset the left wheel integrator of the segway platform. This
+ * integrator holds the total distance traveled by the left wheel.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ */
+ void reset_left_wheel_integrator(void);
+ /**
+ * \brief function to reset the yaw integrator
+ *
+ * This function is used to reset the yaw integrator of the segway platform. This
+ * integrator holds the total rotation performed by the robot.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ */
+ void reset_yaw_integrator(void);
+ /**
+ * \brief function to reset the forward integrator
+ *
+ * This function is used to reset the forward integrator of the segway platform. This
+ * integrator holds the total forward displacement of the robot.
+ *
+ * This configuration information is processed but it is not immediatelly send to the platform.
+ * Instead, it is stored internally and send by the command thread the next time a new motion
+ * command has to be send. Therefore, there may exist a small delay between this call and the
+ * actual change in the configuration.
+ *
+ * This function can be called at any time, but it is recomended to use it only once at the
+ * configuration stage of the robot, before sending motion commands to it.
+ *
+ */
+ void reset_forward_integrator(void);
+ /**
+ * \brief function to connect to the hardware platform
+ *
+ * This function is used to connect the driver with the hardware platform. The
+ * communication device is only created and initialized when this function is
+ * called, so it is not possible to send or receive data to or from the platform
+ * until then. When this function is called, both the command and feedback threads
+ * are started. Otherwise, the communication is not possible.
+ *
+ * At contruction time this function is automatically called so the segway is
+ * ready right after it is created. Howerver, it is possible to call this function
+ * again in case the object msu be closed due to an error or because the target
+ * platform changed.
+ *
+ * If a serial number is provided to this function, it tries to connect to the
+ * platform which has the same serial number. If no serial number is provided, it
+ * tries to automatically connect to any platform it may find. If more or less than
+ * one segway platforms are connected to the computer, this function will throw
+ * an exception.
+ *
+ * If the cable is disconnected, it throws a CSegwayCableDisconnectedException class and
+ * also activates the cable_diconnected_event event. If the power is turner off, it
+ * throws a CSegwayPowerOffException class and also activates the power_off_event event.
+ * Any other exception id rethrown without being processed.
+ * This function throws a CSegwayRMP200Exception exception to report any error.
+ *
+ * \param desc_serial a null terminated string with the description or the serial
+ * number of the segway platform to associate to the driver.
+ * This string can be obtanied through the CFTDIServer or else
+ * hardcoded if known.
+ *
+ */
+ void connect(const std::string& desc_serial="");
+ // status functions
+ /**
+ * \brief function to return the pitch angle
+ *
+ * This function returns the current pitch angle in degrees. This function
+ * only returns the value of the internal attribute, but it does not access
+ * the hardware platform. This value is periodically updated by the feedback
+ * thread.
+ *
+ * \return the current pitch angle in degrees.
+ */
+ float get_pitch_angle(void);
+ /**
+ * \brief function to return the pitch rate
+ *
+ * This function returns the current pitch rate in degrees per second. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the current pitch rate in degrees per second.
+ */
+ float get_pitch_rate(void);
+ /**
+ * \brief function to return the roll angle
+ *
+ * This function returns the current roll angle in degrees. This function
+ * only returns the value of the internal attribute, but it does not access
+ * the hardware platform. This value is periodically updated by the feedback
+ * thread.
+ *
+ * \return the current roll angle in degrees.
+ */
+ float get_roll_angle(void);
+ /**
+ * \brief function to return the roll rate
+ *
+ * This function returns the current roll rate in degrees per second. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the current roll rate in degrees per second.
+ */
+ float get_roll_rate(void);
+ /**
+ * \brief function to return the left wheel velocity
+ *
+ * This function returns the current left wheel velocity in meters per second.
+ * This function only returns the value of the internal attribute, but it
+ * does not access the hardware platform. This value is periodically updated
+ * by the feedback thread.
+ *
+ * \return the current left wheel in meters per second.
+ */
+ float get_left_wheel_velocity(void);
+ /**
+ * \brief function to return the right wheel velocity
+ *
+ * This function returns the current right wheel velocity in meters per second.
+ * This function only returns the value of the internal attribute, but it
+ * does not access the hardware platform. This value is periodically updated
+ * by the feedback thread.
+ *
+ * \return the current right wheel in meters per second.
+ */
+ float get_right_wheel_velocity(void);
+ /**
+ * \brief function to return the yaw rate
+ *
+ * This function returns the current yaw rate in degrees per second.
+ * This function only returns the value of the internal attribute, but it
+ * does not access the hardware platform. This value is periodically updated
+ * by the feedback thread.
+ *
+ * \return the current yaw rate in revolutions per second.
+ *
+ */
+ float get_yaw_rate(void);
+ /**
+ * \brief function to return the number of servo frames per second
+ *
+ * This function returns the current number of servo frames per second. This
+ * function only returns the value of the internal attribute, but it does
+ * not access the hardware platform. This value is periodically updated by
+ * the feedback thread.
+ *
+ * \return the current number of servo frames per second.
+ */
+ float get_servo_frames(void);
+ /**
+ * \brief function to return the left wheel displacement
+ *
+ * This function returns the current left wheel displacement in meters. This
+ * function only returns the value of the internal attribute, but it does
+ * not access the hardware platform. This value is periodically updated by
+ * the feedback thread.
+ *
+ * \return the current left wheel displacement in meters.
+ */
+ float get_left_wheel_displacement(void);
+ /**
+ * \brief function to return the right wheel displacement
+ *
+ * This function returns the current right wheel displacement in meters. This
+ * function only returns the value of the internal attribute, but it does
+ * not access the hardware platform. This value is periodically updated by
+ * the feedback thread.
+ *
+ * \return the current right wheel displacement in meters.
+ */
+ float get_right_wheel_displacement(void);
+ /**
+ * \brief function to return the total forward displacement
+ *
+ * This function returns the current forward displacement in meters. This
+ * function only returns the value of the internal attribute, but it does
+ * not access the hardware platform. This value is periodically updated by
+ * the feedback thread.
+ *
+ * \return the current forward displacement in meters.
+ *
+ */
+ float get_forward_displacement(void);
+ /**
+ * \brief function to return the total yaw displacement
+ *
+ * This function returns the current yaw displacement in revolutions. This
+ * function only returns the value of the internal attribute, but it does
+ * not access the hardware platform. This value is periodically updated by
+ * the feedback thread.
+ *
+ * \return the current yaw displacement in revolutions per second.
+ *
+ */
+ float get_yaw_displacement(void);
+ /**
+ * \brief function to return the current left motot torque
+ *
+ * This function returns the current left motor torque in Nm. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the current left motor torque in Nm.
+ */
+ float get_left_motor_torque(void);
+ /**
+ * \brief function to return the current right motor torque
+ *
+ * This function returns the current right motor torque in Nm. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the current right motor torque in Nm.
+ *
+ */
+ float get_right_motor_torque(void);
+ /**
+ * \brief function to return the current operation mode
+ *
+ * This function returns the current operation mode of the segway platform. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the current operation mode being used. The possible values are:
+ *
+ * - tractor
+ * - balance
+ * - power down
+ *
+ */
+ op_mode get_operation_mode(void);
+ /**
+ * \brief function to return the current hardware operation mode
+ *
+ * This function returns the current hardware operation mode of the segway
+ * platform. This function only returns the value of the internal attribute,
+ * but it does not access the hardware platform. This value is periodically
+ * updated by the feedback thread.
+ *
+ * The value of this attribute is fixed by the state of the two blue buttons
+ * on the platform.
+ *
+ * \return the current operation mode being used. The possible values are:
+ *
+ * - tractor
+ * - balance
+ * - power down
+ *
+ */
+ op_mode get_hardware_operation_mode(void);
+ /**
+ * \brief function to get the current gain schedule
+ *
+ * This function returns the current gain schedule of the segway platform. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the current gain schedule being used. The possible values are:
+ *
+ * - light
+ * - tall
+ * - heavy
+ *
+ */
+ gain get_gain_schedule(void);
+ /**
+ * \brief function to return the value of the user battery voltage
+ *
+ * This function returns the current voltage of the user battery. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the value in volts of the user battery.
+ */
+ float get_ui_battery_voltage(void);
+ /**
+ * \brief function to get the current value of the powerbase battery voltage
+ *
+ * This function returns the current voltage of the powerbase battery. This
+ * function only returns the value of the internal attribute, but it does not
+ * access the hardware platform. This value is periodically updated by the
+ * feedback thread.
+ *
+ * \return the value in volts of the powerbase battery.
+ *
+ */
+ float get_powerbase_battery_voltage(void);
+ /**
+ * \brief function to reset the segway platform
+ *
+ * This function is used to reset the segway platform to its default state. When
+ * this function is called al configuration paramerets previoulsy set return to
+ * its default value. Contrary to what happens with the motion commands, this
+ * function immediatelly sends a data packet to the segway platform.
+ *
+ * This function throws a CSegwayRMP200Exception exception to report any error.
+ *
+ */
+ void reset(void);
+ /**
+ * \brief function to set new translational and rotational velocities
+ *
+ * This function is used to set a new motion command on the segway. This function
+ * sets the internal translational and rotational velocities to the desired values,
+ * and it is the command thread which actually sends the new comamnd to the robot.
+ *
+ * This function can be called at any time to set up a new motion command to the
+ * robot. The command thread sends the current motion command to the robot every
+ * 20ms, so if this function is called more often, some of the commands will not
+ * be executed. This function throws a CSegwayRMP200Exception to report errors.
+ *
+ * If the desired operation mode set by the user with the set_operation_mode()
+ * function does not coincide with the hardware operation mode defined by the
+ * state of the two blue buttons on the platform, this function throws an exception.
+ *
+ * \param vT desired translational velocity in meters per second. This parameter
+ * is limited to 12 kilometers per hour in both directions (both signs).
+ * However, the given value is affected by the velocity scale factor set
+ * by the set_velocity_scale_factor() function, so the actual maximum
+ * speed can be lower.
+ *
+ * \param vR desired rotational velocity in revolutions per second. This parameter
+ * is limited to 0.6 revolutions per second in both directions (both
+ * signs). However, the given value is affectd by the turnrate scale
+ * factor set by the set_turnrate_scale_factor() function, so the actual
+ * maximum turnrate can be lower.
+ */
+ void move(float vT,float vR);
+ /**
+ * \brief function to stop all motion
+ *
+ * This function is used to stop all motion on the segway. This function sets the
+ * internal translational and rotational velocities to 0.0, and it is the command
+ * thread which actually sends the new comamnd to the robot.
+ *
+ * This function can be called at any time to stop the current motion of the robot.
+ * This function throws a CSegwayRMP200Exception to report errors.
+ *
+ */
+ void stop(void);
+ /**
+ * \brief function to close the segway driver
+ *
+ * This function stops all internal threads by activating the the associated finish
+ * events, but it does not destroy them, since it is possible to reconnect the object
+ * to a new platform, in which case the thread will be restarted.
+ *
+ * This function also closes the communication device and frees all the associated
+ * resources.
+ *
+ */
+ void close(void);
+ // operators
+ /**
+ * \brief display operator overloading
+ *
+ * This operator is used to show the current state of an object of this class
+ * onto the standard ouput, file or any output stream. The information shown
+ * is already formated as shown below:
+ *
+ * \verbatim
+ * Pitch angle: <pitch_angle> degrees
+ * Pitch rate: <pitch_rate> degrees/s
+ * Roll angle: <roll_angle> degrees
+ * Roll rate: <roll_rate> degrees/s
+ * Left wheel velocity: <left_wheel_velocity> m/s
+ * Right wheel velocity: <right_wheel_velocity> m/s
+ * Yaw rate: <yaw_rate> degrees/s
+ * Servo frames: <servo_frames> frames/s
+ * Left wheel displacement: <left_wheel_displ> m
+ * Right wheel displacement: <right_wheel_displ> m
+ * Forward displacement: <forward_displ> m
+ * Yaw displacement: <yaw_displ> rev
+ * Left motor torque: <left_torque> Nm
+ * Right motor torque: <right_torque> Nm
+ * Operation mode: <op_mode>
+ * Hardware operation mode : <hardware_op_mode>
+ * Gain schedule: <gain_schedule>
+ * UI battery voltage: <ui_battery> V
+ * Powerbase battery voltage: <powerbase_battery> V
+ * \endverbatim
+ */
+ friend std::ostream& operator<< (std::ostream& out, CSegwayRMP200& segway);
+ /**
+ * \brief destructor
+ *
+ * This destructor is called when the object is about to be destroyed. It calls
+ * the close() function to safely stop all the internal threads, and then deletes
+ * all the internal threads and events.
+ *
+ */
+ ~CSegwayRMP200();
+};
+
+#endif
+
diff --git a/src/segway_rmp200_exceptions.cpp b/src/segway_rmp200_exceptions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..59d3e8c7c1a183302c5ba20d0c6ec0a2cbdee349
--- /dev/null
+++ b/src/segway_rmp200_exceptions.cpp
@@ -0,0 +1,12 @@
+#include "segway_rmp200_exceptions.h"
+#include <string.h>
+#include <stdio.h>
+
+const std::string segway_rmp200_exception_msg="[CSegwayRMP200 class] - ";
+
+CSegwayRMP200Exception::CSegwayRMP200Exception(const std::string& where,const std::string& error_msg,const std::string& segway_id):CException(where,segway_rmp200_exception_msg)
+{
+ this->error_msg+=error_msg;
+ this->error_msg+=" - ";
+ this->error_msg+=segway_id;
+}
diff --git a/src/segway_rmp200_exceptions.h b/src/segway_rmp200_exceptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..99fc2ac89e97ab29a4c5e1eb2a1a5ff929ddcef2
--- /dev/null
+++ b/src/segway_rmp200_exceptions.h
@@ -0,0 +1,58 @@
+#ifndef _SEGWAY_RMP200_EXCEPTIONS
+#define _SEGWAY_RMP200_EXCEPTIONS
+
+#include "exceptions.h"
+
+/**
+ * \brief Generic segway RMP 200 exception class
+ *
+ * This class implements the exceptions for the CSegwayRMP200 class. In addition
+ * to the basic error message provided by the base class CException, this
+ * exception class provides also the unique identifier of the segway robot
+ * that generated the exception.
+ *
+ * Also, similarly to other exception classes, it appends a class identifer
+ * string ("[CSegwayRMP200 class] - ") to the error message in order to identify the
+ * class that generated the exception.
+ *
+ * The base class can be used to catch any exception thrown by the application
+ * or also, this class can be used in order to catch only exceptions generated
+ * by CComm objects.
+ */
+class CSegwayRMP200Exception : public CException
+{
+ public:
+ /**
+ * \brief Constructor
+ *
+ * The constructor calls the base class constructor to add the general
+ * exception identifier and then adds the class identifier string
+ * "[CSegwayRMP200 class]" and the supplied error message.
+ *
+ * It also appends the unique identifier of the segway robot
+ * that generated the exception. So, the total exception message will
+ * look like this:
+ *
+ * \verbatim
+ * [Exception caught] - <where>
+ * [CSegwayRMP200 class] - <error message> - <segway id>
+ * \endverbatim
+ *
+ * \param where a null terminated string with the information about the name
+ * of the function, the source code filename and the line where
+ * the exception was generated. This string must be generated
+ * by the _HERE_ macro.
+ *
+ * \param error_msg a null terminated string that contains the error message.
+ * This string may have any valid character and there is no
+ * limit on its length.
+ *
+ * \param segway_id a null terminated string that contains the segway robot
+ * unique identifier. This string must be the one used to create
+ * the communication device.
+ *
+ */
+ CSegwayRMP200Exception(const std::string& where,const std::string& error_msg,const std::string& segway_id);
+};
+
+#endif