iCub-main
tutorial_periodic_thread.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 #include <yarp/os/Network.h>
4 #include <yarp/os/PeriodicThread.h>
5 #include <yarp/os/Time.h>
6 #include <yarp/os/Property.h>
7 #include <yarp/dev/ControlBoardInterfaces.h>
8 #include <yarp/dev/PolyDriver.h>
9 #include <yarp/sig/Vector.h>
10 
11 #include <string>
12 #include <iostream>
13 
14 using namespace yarp::os;
15 using namespace yarp::dev;
16 using namespace yarp::sig;
17 
18 using namespace std;
19 
20 class ControlThread: public PeriodicThread
21 {
22  PolyDriver dd;
23  IVelocityControl *ivel;
24  IEncoders *iencs;
25  Vector encoders;
26  Vector commands;
27  int count;
28 public:
29  ControlThread(double period):PeriodicThread(period){}
30 
31  bool threadInit()
32  {
33  //initialize here variables
34  printf("ControlThread:starting\n");
35 
36  Property options;
37  options.put("device", "remote_controlboard");
38  options.put("local", "/local/head");
39 
40  //substitute icubSim with icub for use with the real robot
41  options.put("remote", "/icubSim/head");
42 
43  dd.open(options);
44 
45  dd.view(iencs);
46  dd.view(ivel);
47 
48  if ( (!iencs) || (!ivel) )
49  return false;
50 
51  int joints;
52 
53  iencs->getAxes(&joints);
54 
55  encoders.resize(joints);
56  commands.resize(joints);
57 
58  commands=10000;
59  ivel->setRefAccelerations(commands.data());
60 
61  count=0;
62  return true;
63  }
64 
66  {
67  printf("ControlThread:stopping the robot\n");
68 
69  ivel->stop();
70 
71  dd.close();
72 
73  printf("Done, goodbye from ControlThread\n");
74  }
75 
76  void run()
77  {
78  //do the work
79  iencs->getEncoders(encoders.data());
80 
81  count++;
82 
83  if (count%2)
84  commands=5;
85  else
86  commands=-5;
87 
88  ivel->velocityMove(commands.data());
89 
90  printf(".");
91  }
92 };
93 
94 int main(int argc, char *argv[])
95 {
96  Network yarp;
97 
98  if (!yarp.checkNetwork())
99  {
100  printf("No yarp network, quitting\n");
101  return 1;
102  }
103 
104 
105  ControlThread myThread(4.0); //period is 4s
106 
107  myThread.start();
108 
109  bool done=false;
110  double startTime=Time::now();
111  while(!done)
112  {
113  if ((Time::now()-startTime)>5)
114  done=true;
115  }
116 
117  myThread.stop();
118 
119  return 0;
120 }
ControlThread(double period)
void commands(void)
static long startTime
Definition: iCub_Sim.cpp:75
bool done
Definition: main.cpp:42
Copyright (C) 2008 RobotCub Consortium.
int main(int argc, char *argv[])