diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ffb000b2648c7ce6d50b9b4365536bfdd1817f88..c35ef62e8cff5f3efe1216a62657c126e537edfc 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -30,3 +30,5 @@ INSTALL(TARGETS segway_rmp_400
INSTALL(FILES ${headers} DESTINATION include/iridrivers)
INSTALL(FILES ../Findsegway_rmp_400.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+
+ADD_SUBDIRECTORY(examples)
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 16a8a78939bf71849c7f161a9eafb39824d43672..9c02dfa64bcc11958546e354470a82cfe0b2b541 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,6 +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)
+TARGET_LINK_LIBRARIES(test_segwayRMP400 segway_rmp_400)
diff --git a/src/examples/test_segwayRMP400.cpp b/src/examples/test_segwayRMP400.cpp
index ff1826293e98a73a17ebb56fc5ca3d0aac7076eb..3f57ab2afcb63cc8f6789d7958d0cbb7454ccf15 100644
--- a/src/examples/test_segwayRMP400.cpp
+++ b/src/examples/test_segwayRMP400.cpp
@@ -1,75 +1,75 @@
#include "segway_RMP400.h"
+#include "segway_RMP400_exception.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;
- }
-}
-*/
+using namespace std;
int main(int argc, char *argv[])
{
+ cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
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++)
+ try
{
- sleep(1);
- std::cout << (*segway) << std::endl;
+ segway->start();
+ segway->reset_integrators();
+ cout << "Platform started" << endl;
+ }
+ catch (CException & e)
+ {
+ cout<< e.what().c_str()<<endl;
+ return false;
}
- 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);
+ char c;
+ do{
+ cout << "Do you want to move the robot? (y/n):" << endl;
+ cin >> c;
+ }while(c!='y' && c!='n');
+ if(c=='n')
+ {
+ cout << "Press any key+intro to close the program" << endl;
+ cin >> c;
+ }else if(c=='y')
+ {
+ cout << "CAUTION!! THIS PROGRAM MOVES THE SEGWAYRMP400" << endl;
+ cout << "You need at least 4x4 meter clearance around the robot for these tests!" << endl;
+ cout << "No obstacle avoidance is implemented. If there is something in the way the robot will hit it!" << endl;
+ cout << "Moving 0.5 meters at 0.05m/sec" << endl;
+ sleep(1);
+ segway->move(0.05,0.0); // 5cm per second 10 seconds = half a meter
+ int i;
+ for(i=0;i<10;i++)
+ {
+ sleep(1);
+ cout << (*segway) << endl;
+ }
+ 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);
+ cout << "Rotating 360 degrees with center of rotation on the side of the base" << endl;
+ sleep(1);
+ 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)
+ 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();
+ cout << "Rotating 360 degrees with center of rotation 1m away from the base center" << endl;
+ sleep(1);
+ 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);
+ cout << "Rotating backwards 360 degrees with center of rotation 1m away from the base center" << endl;
+ sleep(1);
+ 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
+ }
+ segway->stop();
delete segway;
return 0;
}