test_segwayRMP400.cpp

#include "segway_RMP400.h"
#include "segway_RMP400_exception.h"
#include "eventexceptions.h"
#include <iostream>

using namespace std;

int main(int argc, char *argv[])
{
  CEventServer *event_server=CEventServer::instance();
  std::list<std::string> events;
  unsigned int i=0;
  
  try
  {
    cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
    CSegwayRMP400 segway;
    segway.connect("00000269","00000261");
    segway.reset_integrators();
    cout << "Platform started" << endl;
    sleep(1);
    cout << "start moving forward" << endl;
    events.push_back(segway.get_new_status_event_id());
    segway.move(0.05,0.0); // 5cm per second 10 seconds = half a meter
    for(i=0;i<10;i++)
    {
      try{
        event_server->wait_first(events,1000);
        cout << segway << endl;
      }catch(CEventTimeoutException &e){
        std::cout << "No status data received for over a second" << std::endl;
      }
    }
    segway.stop();
    sleep(2);
    cout << "Rotating 360 degrees with center of rotation on the side of the base" << endl;
    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)
    for(i=0;i<10;i++)
    {
      sleep(1);
      cout << segway << endl;
    }
    segway.stop();
    sleep(2);
    cout << "Rotating backwards 360 degrees with center of rotation 1m away from the base center" << endl;
    segway.move(-0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn BACKWARDS
    for(i=0;i<10;i++)
    {
      sleep(1);
      cout << segway << endl;
    }
    segway.close();
  }
  catch (CException & e)
  {
    cout<< e.what().c_str()<<endl;
  }
}