ViSP  2.6.2
vpRobotBiclopsController.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotBiclopsController.cpp 3800 2012-06-21 15:53:49Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for the Biclops robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <signal.h>
43 #include <string.h>
44 #ifdef UNIX
45 # include <unistd.h>
46 #endif
47 #include <visp/vpConfig.h>
48 
49 #ifdef VISP_HAVE_BICLOPS
50 
51 /* Headers */
52 #include <visp/vpRobotBiclops.h>
53 #include <visp/vpRobotBiclopsController.h>
54 #include <visp/vpRobotException.h>
55 
56 //#define VP_DEBUG // Activate the debug mode
57 //#define VP_DEBUG_MODE 20 // Activate debug level 1 and 2
58 
59 #include <visp/vpDebug.h>
60 
61 
62 
63 /* ----------------------------------------------------------------------- */
64 /* --- CONSTRUCTOR ------------------------------------------------------ */
65 /* ---------------------------------------------------------------------- */
66 
71 {
72  stopControllerThread_ = false;
73  axisMask = Biclops::PanMask
74  + Biclops::TiltMask
75  /*+ Biclops::VergeMask*/; // add this if you want verge.
76 
77  panAxis = NULL;
78  tiltAxis = NULL;
79  vergeAxis = NULL;
80 
81  // Set Debug level depending on how much info you want to see about
82  // the inner workings of the API. Level 2 is highest with 0 being
83  // the default (i.e., no messages).
84  biclops.SetDebugLevel(0);
85 
86  // initialize the shared data structure
87  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
88  shm.status[i] = STOP;
89  shm.q_dot[i] = 0.;
90  shm.actual_q[i] = 0.;
91  shm.jointLimit[i] = false;
92  }
93 
94 }
95 
102 {
103 }
104 
118 void
119 vpRobotBiclopsController::init(const char *configfile)
120 {
121  vpDEBUG_TRACE (12, "Initialize biclops.");
122  bool binit = false;
123  for (int i=0; i<1; i++) {
124  try {
125  std::cout << "Try to initialize biclops head " << std::endl;
126  binit = biclops.Initialize(configfile);
127  usleep(100000);
128  if (binit) {
129  // Initialization completed successfully. Close the config file.
130  std::cout << "Initialization succeed...\n";
131  break;
132  }
133  else {
134  std::cout << "Initialization failed...\n";
135  }
136  }
137  catch(...)
138  {
139  std::cout << "Initialization failed..."<< std::endl;
140  }
141  }
142 
143  if (! binit) {
144  std::cout << "Cannot initialize biclops head. " << std::endl;
145  std::cout << "Check if the serial cable is connected." << std::endl;
146  std::cout << "Check if the robot is powered on." << std::endl;
147  std::cout << "Check if you try to open the good serial port." << std::endl;
148  std::cout << "Try to power off/on and restart..." << std::endl;
149 
151  "Cannot initialize biclops head.");
152 
153  }
154 
155 
156  vpDEBUG_TRACE(12, "Biclops initialized");
157 
158  // Get shortcut references to each axis.
159  panAxis = biclops.GetAxis(Biclops::Pan);
160  tiltAxis = biclops.GetAxis(Biclops::Tilt);
161  if ((axisMask & Biclops::VergeMask) != 0)
162  vergeAxis = biclops.GetAxis(Biclops::Verge);
163 
164 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
165  if (!panAxis -> GetHomedState() || !tiltAxis -> GetHomedState()) {
166  vpDEBUG_TRACE(12, "Biclops is not homed");
167  }
168 #else // old API
169  if (!panAxis -> IsAlreadyHomed() || !tiltAxis -> IsAlreadyHomed()) {
170  vpDEBUG_TRACE(12, "Biclops is not homed");
171  }
172 #endif
173 
174  //Execute the homing sequence for all axes.
175  vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
176  vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
177  if ( biclops.HomeAxes(axisMask))
178  vpDEBUG_TRACE(12, "Homing sequence succeed.");
179  else {
180  vpERROR_TRACE("Homing sequence failed. Program is stopped");
182  "Cannot open connexion with biclops");
183  }
184 
185  // Get the currently defined (default) motion profiles.
186  // PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile;
187  panAxis->GetProfile(panProfile);
188  tiltAxis->GetProfile(tiltProfile);
189  if ((axisMask & Biclops::VergeMask) != 0)
190  vergeAxis->GetProfile(vergeProfile);
191 }
192 
208 void
210  const double percentVelocity )
211 {
212  if (q.getRows() != vpBiclops::ndof )
213  {
214  vpERROR_TRACE ("Bad dimension for positioning vector.");
216  "Bad dimension for positioning vector.");
217  }
218 
219  panAxis -> SetProfileMode(PMDTrapezoidalProfile);
220  tiltAxis -> SetProfileMode(PMDTrapezoidalProfile);
221 
222  // Create the list of axes we want to coordinate
223  PMDUtils::AxisList axisList;
224  axisList.push_back(panAxis);
225  axisList.push_back(tiltAxis);
226 
227 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
228  // Get the currently defined (default) motion profiles.
229  PMDAxisControl::Profile panProfile,tiltProfile;
230  panAxis->GetProfile(panProfile);
231  tiltAxis->GetProfile(tiltProfile);
232 
233  // Set a position to move to by modifying the respective profiles.
234  // NOTE: profile values are in revolutions, so here we convert
235  // from degrees (divide by 360) for readability.
236  panProfile.pos = PMDUtils::RadsToRevs(q[0]);
237  panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
238  * percentVelocity / 100.);
239 
240  tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
241  tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
242  * percentVelocity / 100.);
243 
244  // Inform the controller of the new desired position.
245  panAxis->SetProfile(panProfile);
246  tiltAxis->SetProfile(tiltProfile);
247 
248 #else // old API
249 
250  PMDAxisControl::CountsProfile desired_profile;
251 
252  // Set a position to move to by modifying the respective profiles.
253  // NOTE: profile values are in revolutions, so here we convert
254  // from degrees (divide by 360) for readability.
255  panProfile.pos = PMDUtils::RadsToRevs(q[0]);
256  panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
257  * percentVelocity / 100.);
258 
259  vpDEBUG_TRACE(12, "Speed percent: %lf",
260  vpBiclops::speedLimit * percentVelocity / 100.);
261 
262  panAxis -> ProfileToCounts(panProfile, desired_profile);
263  vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
264  vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
265 
266  panAxis -> SetProfile(desired_profile);
267 
268  // Set a position to move to by modifying the respective profiles.
269  // NOTE: profile values are in revolutions, so here we convert
270  // from degrees (divide by 360) for readability.
271  tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
272  tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
273  * percentVelocity / 100.);
274 
275  tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
276  vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
277  vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
278 
279  tiltAxis -> SetProfile(desired_profile);
280 #endif
281 
282  // Coordinate motion
283  PMDUtils::Coordinate(axisList);
284  biclops.Move(Biclops::PanMask + Biclops::TiltMask/*, 0*/); //
285 
286 }
287 
297 void
299 {
300  if (q_dot.getRows() != vpBiclops::ndof )
301  {
302  vpERROR_TRACE ("Bad dimension for velocity vector.");
304  "Bad dimension for velocity vector.");
305  }
306 
307 
308 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
309  // Get the currently defined (default) motion profiles.
310  PMDAxisControl::Profile panProfile, tiltProfile;
311  panAxis->GetProfile(panProfile);
312  tiltAxis->GetProfile(tiltProfile);
313 
314  // Set a position to move to by modifying the respective profiles.
315  // NOTE: profile values are in revolutions, so here we convert
316  // from degrees (divide by 360) for readability.
317  panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
318  tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
319 
320  // Inform the controller of the new desired position.
321  panAxis->SetProfile(panProfile);
322  tiltAxis->SetProfile(tiltProfile);
323 
324  panAxis -> SetProfileMode(PMDVelocityContouringProfile);
325  tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
326 #else // old API
327  panAxis -> SetProfileMode(PMDVelocityContouringProfile);
328  tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
329 \
330  PMDAxisControl::CountsProfile desired_profile;
331 
332  // Set a position to move to by modifying the respective profiles.
333  // NOTE: profile values are in revolutions, so here we convert
334  // from degrees (divide by 360) for readability.
335  panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
336 
337  panAxis -> ProfileToCounts(panProfile, desired_profile);
338  panAxis -> SetProfile(desired_profile);
339 
340  // Set a position to move to by modifying the respective profiles.
341  // NOTE: profile values are in revolutions, so here we convert
342  // from degrees (divide by 360) for readability.
343  tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
344 
345  tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
346  tiltAxis -> SetProfile(desired_profile);
347 #endif
348  // Coordinate motion
349  biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); //
350 }
351 
361 {
362  vpDEBUG_TRACE (12, "Start vpRobotBiclopsController::getPosition() ");
364  PMDint32 panpos, tiltpos;
365 
366  panAxis -> GetPosition(panpos);
367  tiltAxis -> GetPosition(tiltpos);
368 
369  q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
370  q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
371 
372  vpCDEBUG(11) << "++++++++ Mesure : " << q.t();
373  vpDEBUG_TRACE (12, "End vpRobotBiclopsController::getPosition()");
374 
375  return q;
376 }
377 
387 {
389  PMDint32 panpos, tiltpos;
390 
391  panAxis -> GetActualPosition(panpos);
392  tiltAxis -> GetActualPosition(tiltpos);
393 
394  q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
395  q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
396 
397  return q;
398 }
399 
409 {
411  PMDint32 pan_vel, tilt_vel;
412 
413  panAxis -> GetVelocity(pan_vel);
414  tiltAxis -> GetVelocity(tilt_vel);
415 
416  q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
417  q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
418 
419  return q_dot;
420 }
421 
431 {
433  PMDint32 pan_vel, tilt_vel;
434 
435  panAxis -> GetActualVelocity(pan_vel);
436  tiltAxis -> GetActualVelocity(tilt_vel);
437 
438  q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
439  q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
440 
441  return q_dot;
442 }
443 
450 void
452 {
453  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
454  vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm_.q_dot[i]);
455  }
456  memcpy(&this->shm, &shm_, sizeof(shmType));
457  //this->shm = shm_;
458  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
459  vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
460  }
461 }
462 
469 vpRobotBiclopsController::shmType
471 {
472  shmType tmp_shm;
473 
474  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
475  vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
476  }
477  memcpy(&tmp_shm, &this->shm, sizeof(shmType));
478  //tmp_shm = shm;
479  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
480  vpDEBUG_TRACE(13, "tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
481  }
482 
483  return tmp_shm;
484 }
485 
486 /*
487  * Local variables:
488  * c-basic-offset: 2
489  * End:
490  */
491 
492 #endif
493 
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
void setVelocity(const vpColVector &q_dot)
Error that can be emited by the vpRobot class and its derivates.
static const unsigned int ndof
Definition: vpBiclops.h:127
#define vpERROR_TRACE
Definition: vpDebug.h:379
static const float speedLimit
Definition: vpBiclops.h:134
void init(const char *configfile)
void setPosition(const vpColVector &q, const double percentVelocity)
vpRowVector t() const
transpose of Vector
#define vpCDEBUG(niv)
Definition: vpDebug.h:478
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157