test_segwayRMP400.cpp 1.68 KB
Newer Older
Guillem Alenyà's avatar
Guillem Alenyà committed
1
#include "segway_RMP400.h"
2
#include "segway_RMP400_exception.h"
3
#include "eventexceptions.h"
Guillem Alenyà's avatar
Guillem Alenyà committed
4
5
#include <iostream>

6
using namespace std;
Guillem Alenyà's avatar
Guillem Alenyà committed
7
8

int main(int argc, char *argv[])
9
10
11
12
13
{
  CEventServer *event_server=CEventServer::instance();
  std::list<std::string> events;
  unsigned int i=0;
  
14
  try
Guillem Alenyà's avatar
Guillem Alenyà committed
15
  {
16
17
    cout << "TEST SEGWAY RMP 400 DRIVER" << endl;
    CSegwayRMP400 segway;
18
19
    segway.connect("00000269","00000261");
    segway.reset_integrators();
20
21
    cout << "Platform started" << endl;
    sleep(1);
22
23
    cout << "start moving forward" << endl;
    events.push_back(segway.get_new_status_event_id());
24
    segway.move(0.05,0.0); // 5cm per second 10 seconds = half a meter
25
    for(i=0;i<10;i++)
26
    {
27
28
29
30
31
32
      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;
      }
33
    }
34
    segway.stop();
35
36
    sleep(2);
    cout << "Rotating 360 degrees with center of rotation on the side of the base" << endl;
37
    segway.move(0.0,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn 
38
                           // center right on the side of the base (about 54 cm from center of base)
39
40
41
42
43
    for(i=0;i<10;i++)
    {
      sleep(1);
      cout << segway << endl;
    }
44
    segway.stop();
45
46
    sleep(2);
    cout << "Rotating backwards 360 degrees with center of rotation 1m away from the base center" << endl;
47
    segway.move(-0.1,0.1); // one 10th rev per second, 10 seconds = 360 degrees turn BACKWARDS
48
49
50
51
52
    for(i=0;i<10;i++)
    {
      sleep(1);
      cout << segway << endl;
    }
53
    segway.close();
54
55
56
57
  }
  catch (CException & e)
  {
    cout<< e.what().c_str()<<endl;
58
  }
Guillem Alenyà's avatar
Guillem Alenyà committed
59
}