diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9c9ddbc81d445cd3643a06eed8e9293803370495
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,85 @@
+# 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/)
+ADD_SUBDIRECTORY(test/)
+
+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_400.cmake b/Findsegway_rmp_400.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..767d062b06efe438545b84c0c8998d86c95b9277
--- /dev/null
+++ b/Findsegway_rmp_400.cmake
@@ -0,0 +1,20 @@
+FIND_PATH(segway_rmp_400_INCLUDE_DIR segway_RMP400.h segway_RMP400_exceptions.h /usr/include/iridrivers /usr/local/include/iridrivers)
+
+FIND_LIBRARY(segway_rmp_400_LIBRARY
+ NAMES segway_rmp_400
+ PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
+
+IF (segway_rmp_400_INCLUDE_DIR AND segway_rmp_400_LIBRARY)
+ SET(segway_rmp_400_FOUND TRUE)
+ENDIF (segway_rmp_400_INCLUDE_DIR AND segway_rmp_400_LIBRARY)
+
+IF (segway_rmp_400_FOUND)
+ IF (NOT segway_rmp_400_FIND_QUIETLY)
+ MESSAGE(STATUS "Found Segway RMP400 driver: ${segway_rmp_400_LIBRARY}")
+ ENDIF (NOT segway_rmp_400_FIND_QUIETLY)
+ELSE (segway_rmp_400_FOUND)
+ IF (segway_rmp_400_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find segway RMP200 driver")
+ ENDIF (segway_rmp_400_FIND_REQUIRED)
+ENDIF (segway_rmp_400_FOUND)
+
diff --git a/ReadMe.txt b/ReadMe.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d598803afb1927a434e90332e4479694a76c55ec
--- /dev/null
+++ b/ReadMe.txt
@@ -0,0 +1,19 @@
+Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+Author cetto (cetto@iri.upc.edu)
+All rights reserved.
+
+This file is part of Segway RMP 400 driver library
+Segway RMP400 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..c16199478444307494a28c86170d0552fc303278
--- /dev/null
+++ b/doc/doxygen_project_name.conf
@@ -0,0 +1 @@
+PROJECT_NAME = "Segway RMP400 driver"
diff --git a/doc/main.dox b/doc/main.dox
new file mode 100644
index 0000000000000000000000000000000000000000..a7afebb8613a1de25b9d1b5d5b027a6a52df3eec
--- /dev/null
+++ b/doc/main.dox
@@ -0,0 +1,107 @@
+/*! \mainpage segway RMP400 driver
+
+ \section Introduction
+
+ \subsection Pre-Requisites
+
+ This package requires of the following libraries and packages
+ - <A href="http://www.cmake.org">cmake</A>, a cross-platform build system.
+ - <A href="http://www.doxygen.org">doxygen</a> and
+ <A href="http://www.graphviz.org">graphviz</a> to generate the documentation.
+ - stdc++,
+ .
+
+ Under linux all of these utilities are available in ready-to-use packages.
+
+ Under MacOS most of the packages are available via <a href="http://www.finkproject.org/">fink</a>. <br>
+
+ \subsection Compilation
+
+ Just download this package, uncompress it, and execute
+ - cd build
+ - cmake ..
+ .
+ to generate the makefile and then
+ - make
+ .
+ to obtain the shared library (in this case called <em>iriutils.so</em>) and
+ also all the example programs.
+
+ The <em>cmake</em> only need to be executed once (make will automatically call
+ <em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
+
+ To generate this documentation type
+ - make doc
+ .
+
+ The files in the <em>build</em> directory are genetated by <em>cmake</em>
+ and <em>make</em> and can be safely removed.
+ After doing so you will need to call cmake manually again.
+
+ \subsection Configuration
+
+ The default build mode is DEBUG. That is, objects and executables
+ include debug information.
+
+ The RELEASE build mode optimizes for speed. To build in this mode
+ execute
+ - cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+ .
+ The release mode will be kept until next time cmake is executed.
+
+ \subsection Installation
+
+ In order to be able to use the library, it it necessary to copy it into the system.
+ To do that, execute
+
+ - make install
+ .
+
+ as root and the shared libraries will be copied to <em>/usr/local/lib/iriutils</em> directory
+ and the header files will be copied to <em>/usr/local/include/iriutils</em> dierctory. At
+ this point, the library may be used by any user.
+
+ To remove the library from the system, exceute
+ - make uninstall
+ .
+ as root, and all the associated files will be removed from the system.
+
+ \section Customization
+
+ To build a new application using these library, first it is necessary to locate if the library
+ has been installed or not using the following command
+
+ - FIND_PACKAGE(library_name REQUIRED)
+
+ In the case that the package is present, it is necessary to add the header files directory to
+ the include directory path by using
+
+ - INCLUDE_DIRECTORIES(${library_name_INCLUDE_DIR})
+
+ Finally, it is also nevessary to link with the desired libraries by using the following command
+
+ - TARGET_LINK_LIBRARIES(<executable name> ${library_name_LIBRARY})
+ .
+
+ \section License
+
+ This package is licensed under a
+ <a href="http://www.gnu.org/licenses/lgpl.html">
+ LGPL 3.0 License</a>.
+
+ \section Disclaimer
+
+ This is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+ */
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ffb000b2648c7ce6d50b9b4365536bfdd1817f88
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,32 @@
+# edit the following line to add all the source code files of the library
+SET(sources segway_RMP400.cpp segway_RMP400_exception.cpp)
+# edit the following line to add all the header files of the library
+SET(headers segway_RMP400.h segway_RMP400_exception.h)
+
+# edit the following line to find the necessary packages
+FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
+FIND_PACKAGE(segway_rmp_200 REQUIRED)
+
+# edit the following line to add the necessary include directories
+INCLUDE_DIRECTORIES(.)
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${segway_rmp_200_INCLUDE_DIR})
+
+ADD_LIBRARY(segway_rmp_400 SHARED ${sources})
+
+#edit the following line to add the necessary system libraries (if any)
+TARGET_LINK_LIBRARIES(segway_rmp_400 ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(segway_rmp_400 ${comm_LIBRARY})
+TARGET_LINK_LIBRARIES(segway_rmp_400 ${segway_rmp_200_LIBRARY})
+
+INSTALL(TARGETS segway_rmp_400
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib/iridrivers
+ ARCHIVE DESTINATION lib/iridrivers
+)
+
+INSTALL(FILES ${headers} DESTINATION include/iridrivers)
+
+INSTALL(FILES ../Findsegway_rmp_400.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..16a8a78939bf71849c7f161a9eafb39824d43672
--- /dev/null
+++ b/src/examples/CMakeLists.txt
@@ -0,0 +1,6 @@
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_segwayRMP400 test_segwayRMP400.cpp)
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_segwayRMP400 ${segway_rmp_200_LIBRARY} segway_rmp_200)
+
+
diff --git a/src/examples/test_segwayRMP400.cpp b/src/examples/test_segwayRMP400.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ff1826293e98a73a17ebb56fc5ca3d0aac7076eb
--- /dev/null
+++ b/src/examples/test_segwayRMP400.cpp
@@ -0,0 +1,75 @@
+#include "segway_RMP400.h"
+#include <iostream>
+
+void wait()
+{
+// std::puts("Press any key to continue...");
+// std::getchar();
+ sleep(1);
+}
+
+/*void tensecs(CSegwayRMP400* segway)
+{
+ int i;
+ //std::string info;
+// std::ostream info;
+
+ for(i=0;i<10;i++)
+ {
+ sleep(1);
+// segway->get_info(info);
+ std::cout << (*segway) << std::endl;
+ }
+}
+*/
+
+int main(int argc, char *argv[])
+{
+ CSegwayRMP400 *segway;
+
+ segway=new CSegwayRMP400();
+ segway->set_operation_mode(tractor);
+ segway->reset_integrators();
+
+ std::cout << "CAUTION!! THIS PROGRAM MOVES THE SEGWAYRMP400" << std::endl;
+ std::cout << "You need at least 4x4 meter clearance around the robot for these tests!" << std::endl;
+ std::cout << "No obstacle avoidance is implemented. If there is something in the way the robot will hit it!" << std::endl;
+ std::cout << "Moving 0.5 meters at 0.05m/sec" << std::endl;
+ wait();
+ segway->move(0.05,0.0); // 5cm per second 10 seconds = half a meter
+ int i;
+ for(i=0;i<10;i++)
+ {
+ sleep(1);
+ std::cout << (*segway) << std::endl;
+ }
+ segway->stop();
+ sleep(2);
+
+ std::cout << "Rotating 360 degrees with center of rotation on the side of the base" << std::endl;
+ wait();
+ segway->move(0.0,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn
+ // center right on the side of the base (about 54 cm from center of base)
+// tensecs(segway);
+ segway->stop();
+ sleep(2);
+
+ std::cout << "Rotating 360 degrees with center of rotation 1m away from the base center" << std::endl;
+ wait();
+ segway->move(0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn
+ // center of rotation vT/vR = 1meter from the center of the base
+ // tensecs(segway);
+ segway->stop();
+ sleep(2);
+
+ std::cout << "Rotating backwards 360 degrees with center of rotation 1m away from the base center" << std::endl;
+ wait();
+ segway->move(-0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn BACKWARDS
+ // center of rotation vT/vR = 1meter from the center of the base
+ // tensecs(segway);
+ segway->stop();
+
+
+ delete segway;
+ return 0;
+}
diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e33cc09ed9556d121c1b8b94846d2d81ee0a2df1
--- /dev/null
+++ b/src/segway_RMP400.cpp
@@ -0,0 +1,274 @@
+#include "segway_RMP400.h"
+#include "segway_RMP400_exception.h"
+#include <iridrivers/segway_rmp200_exceptions.h>
+#include <cmath>
+
+CSegwayRMP400::CSegwayRMP400() :
+ status_(rmp400_off),
+ forward_displacement_(0.0),
+ yaw_displacement_(0.0),
+ yaw_rate_ (0.0),
+ wheel_radius_(default_wheel_radius_),
+ use_correction_wheel_factor_(true)
+{
+ scan_ftdi_for_segways();
+}
+
+CSegwayRMP400::CSegwayRMP400(const std::string serial1, const std::string serial2) :
+ status_(rmp400_off),
+ forward_displacement_(0.0),
+ yaw_displacement_(0.0),
+ yaw_rate_ (0.0),
+ wheel_radius_(default_wheel_radius_),
+ use_correction_wheel_factor_(true)
+{
+ if (serial1 == serial2)
+ throw RMP400IdemSerialException(_HERE_, serial1);
+
+ scan_ftdi_for_segways();
+
+ for (int i = 0; i < serial_ftdi_segway_devices_.size(); i++) {
+ std::string serial = serial_ftdi_segway_devices_[i];
+
+ if ((serial != serial1) && (serial != serial2))
+ throw RMP400NotFoundSerialException(_HERE_, serial);
+ }
+}
+
+void
+CSegwayRMP400::scan_ftdi_for_segways()
+{
+ CFTDIServer * ftdi_server_ = CFTDIServer::instance();
+ ftdi_server_->add_custom_PID(CSegwayRMP200::pid);
+
+ std::vector<int> ftdi_devs = ftdi_server_->get_ids_by_description(CSegwayRMP200::description);
+
+ for (int i = 0;i < ftdi_devs.size();i++) {
+ serial_ftdi_segway_devices_.push_back(
+ ftdi_server_->get_serial_number(ftdi_devs.at(i)));
+ }
+
+ if (serial_ftdi_segway_devices_.size() != NUM_SEGWAY_200)
+ throw RMP400NotValidNumberRMP200Exception(_HERE_, serial_ftdi_segway_devices_.size());
+}
+
+void
+CSegwayRMP400::start()
+{
+ for (int i = 0;i < NUM_SEGWAY_200 ;i++) {
+
+ try
+ {
+ CSegwayRMP200 * segway = new CSegwayRMP200(serial_ftdi_segway_devices_[i]);
+ segways_.push_back(segway);
+ }
+ catch (CSegwayRMP200Exception & e)
+ {
+ // Something failed while starting engines
+ // clean up previous (if any) started engine
+ for (int i = 0; i < segways_.size(); i++) {
+ segways_[i]->stop();
+ delete segways_[i];
+ }
+
+ status_ = rmp400_off;
+ throw;
+ }
+ }
+
+ status_ = rmp400_connected;
+}
+
+std::ostream &
+operator<< (std::ostream& data, CSegwayRMP400 & segway)
+{
+ for (int i=0; i<segway.segways_.size(); i++)
+ {
+ data << "Segway unit: " << i << std::endl;
+ data << "Pitch angle: " << segway.segways_[i]->get_pitch_angle() << " degrees" << std::endl;
+ data << "Pitch rate " << segway.segways_[i]->get_pitch_rate() << " degrees/s" << std::endl;
+ data << "Roll angle: " << segway.segways_[i]->get_roll_angle() << " degrees" << std::endl;
+ data << "Roll rate: " << segway.segways_[i]->get_roll_rate() << " degrees/s" << std::endl;
+ data << "Left wheel velocity: " << segway.segways_[i]->get_left_wheel_velocity() << " m/s" << std::endl;
+ data << "Right wheel velocity: " << segway.segways_[i]->get_right_wheel_velocity() << " m/s" << std::endl;
+ data << "Yaw rate: " << segway.segways_[i]->get_yaw_rate() << " degrees/s" << std::endl;
+ data << "Servo frames: " << segway.segways_[i]->get_servo_frames() << " frames/s" << std::endl;
+ data << "Left wheel displacement: " << segway.segways_[i]->get_left_wheel_displacement() << " m" << std::endl;
+ data << "Right wheel displacement: " << segway.segways_[i]->get_right_wheel_displacement() << " m" << std::endl;
+ data << "Forward displacement: " << segway.segways_[i]->get_forward_displacement() << " m" << std::endl;
+ data << "Yaw displacement: " << segway.segways_[i]->get_yaw_displacement() << " rev" << std::endl;
+ data << "Left motor torque: " << segway.segways_[i]->get_left_motor_torque() << " Nm" << std::endl;
+ data << "Right motor torque: " << segway.segways_[i]->get_right_motor_torque() << " Nm" << std::endl;
+ if(segway.segways_[i]->get_operation_mode()==tractor)
+ data << "Operation mode: tractor" << std::endl;
+ else
+ data << "Operation mode: power down" << std::endl;
+ if(segway.segways_[i]->get_gain_schedule()==light)
+ data << "Gain schedule: light" << std::endl;
+ else if(segway.segways_[i]->get_gain_schedule()==tall)
+ data << "Gain schedule: tall" << std::endl;
+ else
+ data << "Gain schedule: heavy" << std::endl;
+ data << "UI battery voltage: " << segway.segways_[i]->get_ui_battery_voltage() << " V" << std::endl;
+ data << "Powerbase battery voltage: " << segway.segways_[i]->get_powerbase_battery_voltage() << " V" << std::endl << std::endl;
+ }
+ data << std::endl;
+}
+
+void
+CSegwayRMP400::set_operation_mode(op_mode mode)
+{
+ if (mode == balance)
+ throw RMP400InvalidBalanceModeOperation(_HERE_);
+
+ for (int i=0;i < segways_.size();i++)
+ {
+ segways_[i]->set_operation_mode(mode);
+ usleep(10000);
+ }
+}
+
+void
+CSegwayRMP400::need_to_be_connected() const
+{
+ if (! is_connected())
+ throw RMP400NoConnected(_HERE_);
+}
+
+void
+CSegwayRMP400::disable_corretion_wheel_factor()
+{
+ use_correction_wheel_factor_ = false;
+}
+
+void
+CSegwayRMP400::fix_with_wheel_factor(float & value)
+{
+ /* corretion factor is the diference between real wheel and supposed
+ * firmware wheel (probaby segway using the rmp200 value) */
+ value = value * (wheel_radius_ / RMP200_DEFAULT_WHEEL_RADIUS);
+}
+
+void
+CSegwayRMP400::set_wheel_radius(const float radius)
+{
+ wheel_radius_ = radius;
+}
+
+float
+CSegwayRMP400::get_forward_displacement()
+{
+ need_to_be_connected();
+
+ // The get_forward_displacement function in segway RMP 200 doesn't
+ // return the accumulate value of displacement, wheel_displacement
+ // function do. Use them instead.
+
+ double fwd_displ_segway0 = (segways_[0]->get_right_wheel_displacement() +
+ segways_[0]->get_left_wheel_displacement()) / 2;
+ double fwd_displ_segway1 = (segways_[1]->get_right_wheel_displacement() +
+ segways_[1]->get_left_wheel_displacement()) / 2;
+
+ forward_displacement_ = (fwd_displ_segway0 + fwd_displ_segway1) / 2;
+
+ if (use_correction_wheel_factor_)
+ fix_with_wheel_factor(forward_displacement_);
+
+ return forward_displacement_;
+}
+
+float
+CSegwayRMP400::get_yaw_displacement()
+{
+ need_to_be_connected();
+
+ float yaw_displacement_mts = (segways_[0]->get_yaw_displacement() +
+ segways_[1]->get_yaw_displacement()) / 2;
+
+ if (use_correction_wheel_factor_)
+ fix_with_wheel_factor(yaw_displacement_mts);
+
+ yaw_displacement_ = yaw_displacement_mts * 2 * PI; // convert to radians
+ return yaw_displacement_;
+}
+
+float
+CSegwayRMP400::get_yaw_rate()
+{
+ need_to_be_connected();
+
+ yaw_rate_ = get_yaw_displacement() * (PI / 180);
+ return yaw_rate_;
+}
+
+void
+CSegwayRMP400::reset_integrators()
+{
+ need_to_be_connected();
+
+ do {
+ for (int i=0; i < segways_.size(); i++) {
+ segways_[i]->reset_right_wheel_integrator();
+ segways_[i]->reset_left_wheel_integrator();
+ segways_[i]->reset_yaw_integrator();
+ segways_[i]->reset_forward_integrator();
+ }
+ // Need to leave the segwayrmp 200 driver to read need values
+ sleep(1);
+ } while (! is_valid_reset_integrators());
+}
+
+bool
+CSegwayRMP400::is_valid_reset_integrators()
+{
+ float fd = get_forward_displacement();
+ float yd = get_yaw_displacement();
+
+ if (std::fabs(0.0 - fd) > 0.1) {
+ std::cerr << "[segway] Invalid forward reset value " << fd << std::endl;
+ return false;
+ }
+
+ if (std::fabs(0.0 - yd) > 0.1) {
+ std::cerr << "[segway] Invalid yaw reset value " << yd << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+void
+CSegwayRMP400::move(float vT, float vR)
+{
+ need_to_be_connected();
+
+ float vRradsec = vR * 2.0 * PI; // revs per sec to rads per sec
+
+ if (fabs(vT) < fabs(RMP400_b * vRradsec / 2)) {
+ // minimum translational velocity needed to enforce rotation about a ICR outside of the robot base
+ if(vT>=0.0)
+ vT = fabs(RMP400_b * vRradsec / 2);
+ else
+ vT = -fabs(RMP400_b * vRradsec / 2);
+ }
+
+ for (int i=0 ;i < segways_.size(); i++)
+ segways_[i]->move(vT,vR);
+}
+
+void
+CSegwayRMP400::stop()
+{
+ need_to_be_connected();
+
+ for (int i=0;i < segways_.size();i++)
+ segways_[i]->stop();
+
+ status_ = rmp400_off;
+}
+
+CSegwayRMP400::~CSegwayRMP400()
+{
+ for (int i=0;i < segways_.size(); i++)
+ delete segways_[i];
+}
diff --git a/src/segway_RMP400.h b/src/segway_RMP400.h
new file mode 100644
index 0000000000000000000000000000000000000000..595f51b01eed759e39dc6e524ac06683c5f141c6
--- /dev/null
+++ b/src/segway_RMP400.h
@@ -0,0 +1,230 @@
+#ifndef _SEGWAYRMP400_H
+#define _SEGWAYRMP400_H
+
+#include <segway_rmp200.h>
+#include <ftdiserver.h>
+#include <math.h>
+#include <vector>
+#include <string>
+#include <iostream>
+
+#define RMP400_b 0.54 // distance between weels for one rmp unit in meters
+#define PI 3.141592
+#define RMP200_DEFAULT_WHEEL_RADIUS 0.24
+
+const int NUM_SEGWAY_200 = 2;
+
+enum SegwayRMP400_status { rmp400_off, rmp400_connected };
+
+class CSegwayRMP400
+{
+ private:
+ std::vector<CSegwayRMP200 *> segways_;
+ /* vector of valid segway serials found in the ftdi bus */
+ std::vector<std::string> serial_ftdi_segway_devices_;
+
+ /**
+ * \brief Segway status
+ */
+ SegwayRMP400_status status_;
+
+ /**
+ * \brief forward displacement
+ *
+ * This value has the displacement of the whole platform in meters
+ * TODO: should be a thread periodically updating this value
+ */
+ float forward_displacement_;
+
+ /**
+ * \brief yaw displacement
+ *
+ * This value has the rotation of the whole platform in revolutions.
+ * TODO: should be a thread periodically updating this value
+ */
+ float yaw_displacement_;
+
+ /**
+ * \brief yaw rate
+ *
+ * This value has the yaw rate of the platform in degrees per second.
+ * TODO: should be a thread periodically updating this value
+ */
+ float yaw_rate_;
+
+ /**
+ * \brief default rmp400 wheel radius when inflated to 10 psi (in meters)
+ */
+ static const float default_wheel_radius_ = 0.26;
+
+ /**
+ * \brief wheel radius in meters
+ *
+ */
+ float wheel_radius_;
+
+ /**
+ * \brief whether use or not the correction factor to fix wheel size
+ * (default is true). See fix_with_wheel_factor for more info.
+ *
+ */
+ bool use_correction_wheel_factor_;
+
+ /**
+ * \brief apply a corretion factor to the value given
+ *
+ * It is suspected that segway is using the RMP200 wheel radius in RMP400
+ * firmware which gives a bad measure for displacements. This functions
+ * calculate the relation between radius and fix the value given using this
+ * factor.
+ */
+ void fix_with_wheel_factor(float & value);
+
+ /**
+ * \brief disable use of correction wheel factor for displacements
+ *
+ * Check fix_with_wheel_factor for more info.
+ */
+ void disable_corretion_wheel_factor();
+
+ void scan_ftdi_for_segways();
+
+ /**
+ * \brief set a different wheel radius (in meters)
+ *
+ * Default, when wheels are inflated to 10psi, is 0.26m. If the segway has
+ * a different meassure, please use this function before starting to operate
+ * as normal.
+ */
+ void set_wheel_radius(const float radius);
+
+ /*
+ * \brief check if the driver is already connected (made a start() call).
+ *
+ * It will return an RMP400NoConnected exception if fail
+ */
+ void need_to_be_connected() const;
+
+ /* \brief aux function to check if platform is really doing the reset in
+ * integrators.
+ */
+ bool is_valid_reset_integrators();
+
+ protected:
+ /**
+ * \brief allowed modes:
+ * - tractor
+ * - power_dow
+ */
+ op_mode mode;
+
+ public:
+ CSegwayRMP400();
+ CSegwayRMP400(const std::string serial1, const std::string serial2);
+
+ ~CSegwayRMP400();
+
+ /**
+ * \brief function to set the operation mode
+ *
+ * Operation modes: tractor or power_down.
+ * In the RMP400 the balance mode is disabled.
+ */
+ void set_operation_mode(op_mode mode);
+
+ /**
+ * \brief function to check if segway is already connected
+ */
+ bool is_connected() const;
+
+ /**
+ * \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.
+ *
+ * Both fordward displacement from segways are sum and a mean is calculated
+ *
+ * \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 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.
+ *
+ * Both yaw displacement from segways are sum and a mean is calculated
+ *
+ * \return the current yaw displacement in revolutions per second.
+ *
+ */
+ float get_yaw_displacement(void);
+
+ /**
+ * \brief function to return the yaw rate
+ *
+ * This function returns the current yaw rate in revolutions 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.
+ *
+ * Both yaw rate from segways are sum and a mean is calculated
+ *
+ * \return the current yaw rate in revolutions per second.
+ *
+ */
+ float get_yaw_rate(void);
+
+ /**
+ * \brief function to reset integrators
+ *
+ * This function sets all encoder integrators to zero in both platforms.
+ * Currently this function seems NOT to be reliable since hardware not
+ * always is doing the proper work.
+ */
+ void reset_integrators();
+
+ /**
+ * \brief function to move the platform
+ *
+ * This function sets the rotational and translational velocities to a desired value
+ * vT is in m/sec, vR is in rev/sec
+ *
+ * Rotation about the center of the platform with skid steer is disabled.
+ * Skid steering is not recommended since it produces significant tire wear and unnecessary battery consumption.
+ * Odometry readings during skid steering are useless since they depend heavily on soil characteristics.
+ * A minimum Instantaneous Center of Rotation (ICR) located on the side of the platform is enforced (0.54cm).
+ */
+ void move(float vT, float vR);
+
+ /* \brief function to connect segways RMP200
+ *
+ * This functions start the vehicle
+ */
+ void start();
+
+ /* \brief function to stop the platform
+ *
+ * This function stops the vehicle
+ */
+ void stop();
+ friend std::ostream& operator<< (std::ostream& out, CSegwayRMP400& segway);
+};
+
+inline bool
+CSegwayRMP400::is_connected() const
+{
+ return (status_ == rmp400_connected);
+}
+
+
+#endif
+
+
diff --git a/src/segway_RMP400_exception.cpp b/src/segway_RMP400_exception.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4daace416d4161548e29f76e451a38c2abd5076a
--- /dev/null
+++ b/src/segway_RMP400_exception.cpp
@@ -0,0 +1,56 @@
+#include "segway_RMP400_exception.h"
+#include <string>
+#include <sstream>
+
+const std::string segway_rmp400_exception_msg = "[CSegwayRMP400 class] - ";
+
+CSegwayRMP400Exception::CSegwayRMP400Exception(const std::string & where,
+ const std::string & error_msg,
+ const std::string & segway_id) throw () :
+ CException(where, segway_rmp400_exception_msg + error_msg + "-" + segway_id)
+{
+}
+
+CSegwayRMP400Exception::CSegwayRMP400Exception(const std::string & where,
+ const std::string & error_msg) throw () :
+ CException(where, segway_rmp400_exception_msg)
+{
+}
+
+RMP400IdemSerialException::RMP400IdemSerialException(const std::string & where,
+ const std::string & serial) throw () :
+ CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
+{
+ error_msg += "Serial ports are the same for two segways: " + serial;
+}
+
+RMP400NotFoundSerialException::RMP400NotFoundSerialException(const std::string & where,
+ const std::string & serial) throw () :
+
+ CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
+{
+ error_msg += "Serial port found: " + serial + " not inside valid ports supplied";
+}
+
+
+RMP400NotValidNumberRMP200Exception::RMP400NotValidNumberRMP200Exception(const std::string & where,
+ const int & number_found) throw () :
+ CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
+{
+ std::stringstream s;
+ s << number_found;
+
+ error_msg += "Found " + s.str() + " RMP200 units which is not valid for this device";
+}
+
+RMP400InvalidBalanceModeOperation::RMP400InvalidBalanceModeOperation(const std::string & where) throw () :
+ CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
+{
+ error_msg += "Balance mode for the segway RMP400 is completly crazy";
+}
+
+RMP400NoConnected::RMP400NoConnected(const std::string & where) throw () :
+ CSegwayRMP400Exception(where, segway_rmp400_exception_msg)
+{
+ error_msg += "Segway RMP 400 has not been connected";
+}
diff --git a/src/segway_RMP400_exception.h b/src/segway_RMP400_exception.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd8b98751a49e547507e73852772d29664c75cd1
--- /dev/null
+++ b/src/segway_RMP400_exception.h
@@ -0,0 +1,100 @@
+#ifndef _SEGWAY_RMP400_EXCEPTIONS
+#define _SEGWAY_RMP400_EXCEPTIONS
+
+#include "exceptions.h"
+
+/**
+ * \brief Generic segway RMP 400 exception class
+ *
+ * This class implements the exceptions for the CSegwayRMP400 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 ("[CSegwayRMP400 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 CSegwayRMP400Exception : 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
+ * "[CSegwayRMP400 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>
+ * [CSegwayRMP400 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.
+ *
+ */
+ CSegwayRMP400Exception(const std::string& where,const std::string& error_msg,const std::string& segway_id) throw();
+
+ CSegwayRMP400Exception(const std::string& where,const std::string& error_msg) throw();
+};
+
+class RMP400IdemSerialException :
+ public CSegwayRMP400Exception
+{
+ public:
+ /* Constructor */
+ RMP400IdemSerialException(const std::string & where, const std::string & serial) throw ();
+};
+
+class RMP400NotFoundSerialException :
+ public CSegwayRMP400Exception
+{
+ public:
+ /* Constructor */
+ RMP400NotFoundSerialException(const std::string & where, const std::string & serial) throw ();
+};
+
+class RMP400NotValidNumberRMP200Exception :
+ public CSegwayRMP400Exception
+{
+ public:
+ /* Constructor */
+ RMP400NotValidNumberRMP200Exception(const std::string & where, const int & number) throw ();
+};
+
+class RMP400InvalidBalanceModeOperation :
+ public CSegwayRMP400Exception
+{
+ public:
+ /* Constructor */
+ RMP400InvalidBalanceModeOperation(const std::string & where) throw ();
+};
+
+class RMP400NoConnected :
+ public CSegwayRMP400Exception
+{
+ public:
+ /* constructor */
+ RMP400NoConnected(const std::string & where) throw ();
+};
+
+#endif
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b212e3776b808a04784cc57fc512f20a160de8bd
--- /dev/null
+++ b/test/CMakeLists.txt
@@ -0,0 +1,20 @@
+# edit the following line to find the necessary packages
+FIND_LIBRARY(GTEST gtest)
+FIND_PACKAGE(segway_rmp_200)
+
+FIND_LIBRARY (GTEST_LIBRARY
+ NAMES gtest)
+
+IF (GTEST_LIBRARY)
+ message (STATUS "Found gtest library: testing suite will be compiled")
+ # edit the following line to add the necessary include directories
+ INCLUDE_DIRECTORIES(../src)
+ INCLUDE_DIRECTORIES(${segway_rmp_200_INCLUDE_DIR})
+
+ ADD_EXECUTABLE(test_basic basic.cpp)
+ ADD_EXECUTABLE(test_movement movement.cpp)
+ TARGET_LINK_LIBRARIES(test_basic segway_rmp_400 ${GTEST_LIBRARY})
+ TARGET_LINK_LIBRARIES(test_movement segway_rmp_400 ${GTEST_LIBRARY})
+else ()
+ message (STATUS "Not Found gtest library: testing suite won't be compiled")
+endif ()
diff --git a/test/basic.cpp b/test/basic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0decba65d4f967476597da5c0c91c6e6ad4fbc4
--- /dev/null
+++ b/test/basic.cpp
@@ -0,0 +1,62 @@
+/* vim: set sw=4 sts=4 et foldmethod=syntax : */
+
+#include <gtest/gtest.h>
+#include "segway_RMP400.h"
+#include "exceptions.h"
+
+int g_argc;
+char** g_argv;
+
+TEST(Driver, constructor_destructor)
+{
+ ASSERT_NO_THROW ({
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ delete segway;
+ });
+}
+
+TEST(Driver, startstop)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ ASSERT_NO_THROW(segway->start());
+ ASSERT_NO_THROW(segway->stop());
+ delete segway;
+}
+
+TEST(Driver, startstoptwice)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ ASSERT_NO_THROW(segway->start());
+ ASSERT_NO_THROW(segway->stop());
+ ASSERT_NO_THROW(segway->start());
+ ASSERT_NO_THROW(segway->stop());
+ delete segway;
+}
+
+TEST(Driver, connected)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ ASSERT_FALSE(segway->is_connected());
+ segway->start();
+ ASSERT_TRUE(segway->is_connected());
+ segway->stop();
+ ASSERT_FALSE(segway->is_connected());
+ delete segway;
+}
+
+TEST(Driver, reset_integrators)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ ASSERT_NO_THROW(segway->reset_integrators());
+ segway->stop();
+ delete segway;
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ g_argc = argc;
+ g_argv = argv;
+ return RUN_ALL_TESTS();
+}
diff --git a/test/movement.cpp b/test/movement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98ff4fa282f3c98e983b7a60e13f7c033391c81b
--- /dev/null
+++ b/test/movement.cpp
@@ -0,0 +1,104 @@
+/* vim: set sw=4 sts=4 et foldmethod=syntax : */
+
+#include <gtest/gtest.h>
+#include "segway_RMP400.h"
+#include "exceptions.h"
+
+int g_argc;
+char** g_argv;
+
+TEST(Driver, basic_move)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ ASSERT_NO_THROW(segway->move(0.3,0.3));
+ sleep(3);
+ segway->move(0.0,0.0);
+ segway->stop();
+ delete segway;
+}
+
+TEST(Driver, reset_integrators_after_move)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ segway->reset_integrators();
+ segway->move(0.2,0.0);
+ sleep(2);
+ segway->move(0.0,0.2);
+ sleep(2);
+ segway->move(0.0,0.0);
+ sleep(2);
+ segway->reset_integrators();
+ ASSERT_NEAR(segway->get_forward_displacement(), 0.0, 0.1);
+ ASSERT_NEAR(segway->get_yaw_displacement(), 0.0, 0.1);
+ segway->stop();
+ delete segway;
+}
+
+TEST(Driver, get_forward_displacement)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ segway->reset_integrators();
+ segway->move(0.2,0.0);
+ sleep(3);
+ ASSERT_GT(segway->get_forward_displacement(), 0.0);
+ segway->stop();
+ delete segway;
+}
+
+TEST(Driver, get_forward_negative_displacement)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ segway->reset_integrators();
+ segway->move(-0.2,0.0);
+ sleep(3);
+ ASSERT_LT(segway->get_forward_displacement(), 0.0);
+ segway->stop();
+ delete segway;
+}
+
+TEST(Driver, get_yaw_displacement)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ segway->reset_integrators();
+ segway->move(0.0,0.2);
+ sleep(3);
+ ASSERT_GT(segway->get_yaw_displacement(), 0.0);
+ segway->stop();
+ delete segway;
+}
+
+TEST(Driver, get_yaw_negative_displacement)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ segway->reset_integrators();
+ segway->move(0.0,-0.2);
+ sleep(3);
+ ASSERT_LT(segway->get_yaw_displacement(), 0.0);
+ segway->stop();
+ delete segway;
+}
+
+TEST(Driver, get_yaw_rate)
+{
+ CSegwayRMP400 * segway = new CSegwayRMP400();
+ segway->start();
+ segway->move(0.0,0.3);
+ sleep(3);
+ ASSERT_GT(segway->get_yaw_rate(), 0.0);
+ segway->stop();
+ delete segway;
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ g_argc = argc;
+ g_argv = argv;
+ return RUN_ALL_TESTS();
+}