test_segwayRMP400.cpp 2.11 KB
Newer Older
Guillem Alenyà's avatar
Guillem Alenyà committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
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;
}