iCub-main
lookAtLocation.cpp
Go to the documentation of this file.
1 #include <cstdio>
2 #include <vector>
3 #include <yarp/os/all.h>
4 #include <yarp/sig/all.h>
5 #include <yarp/dev/all.h>
6 using namespace std;
7 using namespace yarp::os;
8 using namespace yarp::sig;
9 using namespace yarp::dev;
10 int main() {
11  Network yarp; // set up yarp
12  BufferedPort<Vector> targetPort;
13  targetPort.open("/tutorial/target/in");
14  Network::connect("/tutorial/target/out","/tutorial/target/in");
15 
16  Property options;
17  options.put("device", "remote_controlboard");
18  options.put("local", "/tutorial/motor/client");
19  options.put("remote", "/icubSim/head");
20  PolyDriver robotHead(options);
21  if (!robotHead.isValid()) {
22  printf("Cannot connect to robot head\n");
23  return 1;
24  }
25  IControlMode *mod;
26  IPositionControl *pos;
27  IVelocityControl *vel;
28  IEncoders *enc;
29  robotHead.view(mod);
30  robotHead.view(pos);
31  robotHead.view(vel);
32  robotHead.view(enc);
33  if (mod==NULL || pos==NULL || vel==NULL || enc==NULL) {
34  printf("Cannot get interface to robot head\n");
35  robotHead.close();
36  return 1;
37  }
38  int jnts = 0;
39  pos->getAxes(&jnts);
40  Vector setpoints;
41  setpoints.resize(jnts);
42 
43  // enable velocity control mode
44  vector<int> modes(jnts,VOCAB_CM_VELOCITY);
45  mod->setControlModes(modes.data());
46 
47  while (1) { // repeat forever
48  Vector *target = targetPort.read(); // read a target
49  if (target!=NULL) { // check we actually got something
50  printf("We got a vector containing");
51  for (size_t i=0; i<target->size(); i++) {
52  printf(" %g", (*target)[i]);
53  }
54  printf("\n");
55 
56  double x = (*target)[0];
57  double y = (*target)[1];
58  double conf = (*target)[2];
59 
60  x -= 320/2;
61  y -= 240/2;
62 
63  double vx = x*0.1;
64  double vy = -y*0.1;
65 
66  // prepare command
67  for (int i=0; i<jnts; i++) {
68  setpoints[i] = 0;
69  }
70 
71  if (conf>0.5) {
72  setpoints[3] = vy;
73  setpoints[4] = vx;
74  } else {
75  setpoints[3] = 0;
76  setpoints[4] = 0;
77  }
78  vel->velocityMove(setpoints.data());
79  }
80  }
81  return 0;
82 }
int jnts
Definition: main.cpp:60
int main()
Copyright (C) 2008 RobotCub Consortium.