ViSP  2.9.0
vpRobotBiclopsController.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotBiclopsController.cpp 4604 2014-01-21 14:15:23Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // 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  shm.status[i] = STOP;
93  }
94 
95 }
96 
103 {
104 }
105 
119 void
120 vpRobotBiclopsController::init(const char *configfile)
121 {
122  vpDEBUG_TRACE (12, "Initialize biclops.");
123  bool binit = false;
124  for (int i=0; i<1; i++) {
125  try {
126  std::cout << "Try to initialize biclops head " << std::endl;
127  binit = biclops.Initialize(configfile);
128  usleep(100000);
129  if (binit) {
130  // Initialization completed successfully. Close the config file.
131  std::cout << "Initialization succeed...\n";
132  break;
133  }
134  else {
135  std::cout << "Initialization failed...\n";
136  }
137  }
138  catch(...)
139  {
140  std::cout << "Initialization failed..."<< std::endl;
141  }
142  }
143 
144  if (! binit) {
145  std::cout << "Cannot initialize biclops head. " << std::endl;
146  std::cout << "Check if the serial cable is connected." << std::endl;
147  std::cout << "Check if the robot is powered on." << std::endl;
148  std::cout << "Check if you try to open the good serial port." << std::endl;
149  std::cout << "Try to power off/on and restart..." << std::endl;
150 
152  "Cannot initialize biclops head.");
153 
154  }
155 
156 
157  vpDEBUG_TRACE(12, "Biclops initialized");
158 
159  // Get shortcut references to each axis.
160  panAxis = biclops.GetAxis(Biclops::Pan);
161  tiltAxis = biclops.GetAxis(Biclops::Tilt);
162  if ((axisMask & Biclops::VergeMask) != 0)
163  vergeAxis = biclops.GetAxis(Biclops::Verge);
164 
165 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
166  if (!panAxis -> GetHomedState() || !tiltAxis -> GetHomedState()) {
167  vpDEBUG_TRACE(12, "Biclops is not homed");
168  }
169 #else // old API
170  if (!panAxis -> IsAlreadyHomed() || !tiltAxis -> IsAlreadyHomed()) {
171  vpDEBUG_TRACE(12, "Biclops is not homed");
172  }
173 #endif
174 
175  //Execute the homing sequence for all axes.
176  vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
177  vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
178  if ( biclops.HomeAxes(axisMask))
179  vpDEBUG_TRACE(12, "Homing sequence succeed.");
180  else {
181  vpERROR_TRACE("Homing sequence failed. Program is stopped");
183  "Cannot open connexion with biclops");
184  }
185 
186  // Get the currently defined (default) motion profiles.
187  // PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile;
188  panAxis->GetProfile(panProfile);
189  tiltAxis->GetProfile(tiltProfile);
190  if ((axisMask & Biclops::VergeMask) != 0)
191  vergeAxis->GetProfile(vergeProfile);
192 }
193 
209 void
211  const double percentVelocity )
212 {
213  if (q.getRows() != vpBiclops::ndof )
214  {
215  vpERROR_TRACE ("Bad dimension for positioning vector.");
217  "Bad dimension for positioning vector.");
218  }
219 
220  panAxis -> SetProfileMode(PMDTrapezoidalProfile);
221  tiltAxis -> SetProfileMode(PMDTrapezoidalProfile);
222 
223  // Create the list of axes we want to coordinate
224  PMDUtils::AxisList axisList;
225  axisList.push_back(panAxis);
226  axisList.push_back(tiltAxis);
227 
228 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
229  // Get the currently defined (default) motion profiles.
230  PMDAxisControl::Profile panProfile,tiltProfile;
231  panAxis->GetProfile(panProfile);
232  tiltAxis->GetProfile(tiltProfile);
233 
234  // Set a position to move to by modifying the respective profiles.
235  // NOTE: profile values are in revolutions, so here we convert
236  // from degrees (divide by 360) for readability.
237  panProfile.pos = PMDUtils::RadsToRevs(q[0]);
238  panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
239  * percentVelocity / 100.);
240 
241  tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
242  tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
243  * percentVelocity / 100.);
244 
245  // Inform the controller of the new desired position.
246  panAxis->SetProfile(panProfile);
247  tiltAxis->SetProfile(tiltProfile);
248 
249 #else // old API
250 
251  PMDAxisControl::CountsProfile desired_profile;
252 
253  // Set a position to move to by modifying the respective profiles.
254  // NOTE: profile values are in revolutions, so here we convert
255  // from degrees (divide by 360) for readability.
256  panProfile.pos = PMDUtils::RadsToRevs(q[0]);
257  panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
258  * percentVelocity / 100.);
259 
260  vpDEBUG_TRACE(12, "Speed percent: %lf",
261  vpBiclops::speedLimit * percentVelocity / 100.);
262 
263  panAxis -> ProfileToCounts(panProfile, desired_profile);
264  vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
265  vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
266 
267  panAxis -> SetProfile(desired_profile);
268 
269  // Set a position to move to by modifying the respective profiles.
270  // NOTE: profile values are in revolutions, so here we convert
271  // from degrees (divide by 360) for readability.
272  tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
273  tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
274  * percentVelocity / 100.);
275 
276  tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
277  vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
278  vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
279 
280  tiltAxis -> SetProfile(desired_profile);
281 #endif
282 
283  // Coordinate motion
284  PMDUtils::Coordinate(axisList);
285  biclops.Move(Biclops::PanMask + Biclops::TiltMask/*, 0*/); //
286 
287 }
288 
298 void
300 {
301  if (q_dot.getRows() != vpBiclops::ndof )
302  {
303  vpERROR_TRACE ("Bad dimension for velocity vector.");
305  "Bad dimension for velocity vector.");
306  }
307 
308 
309 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
310  // Get the currently defined (default) motion profiles.
311  PMDAxisControl::Profile panProfile, tiltProfile;
312  panAxis->GetProfile(panProfile);
313  tiltAxis->GetProfile(tiltProfile);
314 
315  // Set a position to move to by modifying the respective profiles.
316  // NOTE: profile values are in revolutions, so here we convert
317  // from degrees (divide by 360) for readability.
318  panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
319  tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
320 
321  // Inform the controller of the new desired position.
322  panAxis->SetProfile(panProfile);
323  tiltAxis->SetProfile(tiltProfile);
324 
325  panAxis -> SetProfileMode(PMDVelocityContouringProfile);
326  tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
327 #else // old API
328  panAxis -> SetProfileMode(PMDVelocityContouringProfile);
329  tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
330 \
331  PMDAxisControl::CountsProfile desired_profile;
332 
333  // Set a position to move to by modifying the respective profiles.
334  // NOTE: profile values are in revolutions, so here we convert
335  // from degrees (divide by 360) for readability.
336  panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
337 
338  panAxis -> ProfileToCounts(panProfile, desired_profile);
339  panAxis -> SetProfile(desired_profile);
340 
341  // Set a position to move to by modifying the respective profiles.
342  // NOTE: profile values are in revolutions, so here we convert
343  // from degrees (divide by 360) for readability.
344  tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
345 
346  tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
347  tiltAxis -> SetProfile(desired_profile);
348 #endif
349  // Coordinate motion
350  biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); //
351 }
352 
362 {
363  vpDEBUG_TRACE (12, "Start vpRobotBiclopsController::getPosition() ");
365  PMDint32 panpos, tiltpos;
366 
367  panAxis -> GetPosition(panpos);
368  tiltAxis -> GetPosition(tiltpos);
369 
370  q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
371  q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
372 
373  vpCDEBUG(11) << "++++++++ Mesure : " << q.t();
374  vpDEBUG_TRACE (12, "End vpRobotBiclopsController::getPosition()");
375 
376  return q;
377 }
378 
388 {
390  PMDint32 panpos, tiltpos;
391 
392  panAxis -> GetActualPosition(panpos);
393  tiltAxis -> GetActualPosition(tiltpos);
394 
395  q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
396  q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
397 
398  return q;
399 }
400 
410 {
412  PMDint32 pan_vel, tilt_vel;
413 
414  panAxis -> GetVelocity(pan_vel);
415  tiltAxis -> GetVelocity(tilt_vel);
416 
417  q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
418  q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
419 
420  return q_dot;
421 }
422 
432 {
434  PMDint32 pan_vel, tilt_vel;
435 
436  panAxis -> GetActualVelocity(pan_vel);
437  tiltAxis -> GetActualVelocity(tilt_vel);
438 
439  q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
440  q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
441 
442  return q_dot;
443 }
444 
451 void
453 {
454  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
455  vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm_.q_dot[i]);
456  }
457  memcpy(&this->shm, &shm_, sizeof(shmType));
458  //this->shm = shm_;
459  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
460  vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
461  }
462 }
463 
470 vpRobotBiclopsController::shmType
472 {
473  shmType tmp_shm;
474 
475  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
476  vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
477  }
478  memcpy(&tmp_shm, &this->shm, sizeof(shmType));
479  //tmp_shm = shm;
480  for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
481  vpDEBUG_TRACE(13, "tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
482  }
483 
484  return tmp_shm;
485 }
486 
487 /*
488  * Local variables:
489  * c-basic-offset: 2
490  * End:
491  */
492 
493 #endif
494 
#define vpDEBUG_TRACE
Definition: vpDebug.h:482
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:395
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(level)
Definition: vpDebug.h:506
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:161