Afegida opcio a l'exemple per tal de llencar el programa sense que el robot es...

Afegida opcio a l'exemple per tal de llencar el programa sense que el robot es mogui. Actualitzada la api

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)

src/examples/CMakeLists.txt

 # 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)
 
 

src/examples/test_segwayRMP400.cpp

 #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;
 }