ViSP  2.8.0
vpRobotCycab.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotCycab.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 car-like Cycab mobile robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpConfig.h>
43 #include <visp/vpRobotCycab.h>
44 
45 #ifdef VISP_HAVE_CYCAB
46 
55 vpRobotCycab::vpRobotCycab()
56 {
57 #ifdef VISP_HAVE_CYCABTK_OLD
58  // Old low level controller based on Syndex (obsolete)
59  end = false;
60  cycab = NULL;
61  sprintf(servername, "cycab-hf1");
62  cycab = new EtherCycab(servername, &LTR);
63  if (!cycab->doInitialization(&end, 50, 0, false)) {
64  printf("ERROR: Can't connect to %s\n", servername);
65  exit(-1);
66  }
67  cycab->releaseSecurity();
68  printf("Control limits : phi in [%f:%f], V in [%f:%f]\n",
69  cycab->minAllowedSteering(),
70  cycab->maxAllowedSteering(),
71  cycab->minAllowedSpeed(),
72  cycab->maxAllowedSpeed());
73 #elif defined VISP_HAVE_CYCABTK
74  // New low level controller based on Syndex (to use)
75  std::string name = "cycab";
76  CycabCarCommand cycab_command;
77  setDualSteering(false);
78  store.connect();
79  cycab_commandId = store.lookupVariable( name + cycabCarCommandSuffix );
80  cycab_stateId = store.lookupVariable( name + cycabStateSuffix );
81  store.writeVariable( cycab_commandId, cycab_command );
82 #endif
83 }
84 
88 vpRobotCycab::~vpRobotCycab()
89 {
90 #ifdef VISP_HAVE_CYCABTK_OLD
91  if (cycab != NULL) delete cycab;
92  cycab = NULL;
93 #endif
94 }
95 
103 void vpRobotCycab::setDualSteering(bool dual)
104 {
105  dualSteering = dual;
106 }
107 
114 void vpRobotCycab::setCommand(double v, double phi)
115 {
116 #ifdef VISP_HAVE_CYCABTK_OLD
117  // Old low level controller based on Syndex (obsolete)
118  cycab->sendCommands(v, phi);
119 #elif defined VISP_HAVE_CYCABTK
120  // New low level controller based on Syndex (to use)
121  CycabCarCommand cycab_command;
122  store.readVariable( cycab_commandId, cycab_command );
123  cycab_command.v = v;
124  cycab_command.phi = phi;
125  cycab_command.manualDriving = false;
126  cycab_command.dualSteering = dualSteering;
127  store.writeVariable( cycab_commandId, cycab_command );
128 #endif
129 }
130 
140 void vpRobotCycab::getOdometry(double &vmean, double &phi)
141 {
142  double timestamp;
143  getOdometry(vmean, phi, timestamp);
144 }
145 
157 void vpRobotCycab::getOdometry(double &vmean, double &phi, double &timestamp)
158 {
159  vmean = 0.;
160  phi=0.;
161  timestamp=0.;
162 
163 #ifdef VISP_HAVE_CYCABTK_OLD
164  // Old low level controller based on Syndex (obsolete)
165  Record rec;
166  double dsl,dsr,lphi,ltime;
167  cycab->waitUpdate();
168  cycab->getState(&rec, &dsl, &dsr, &lphi, &ltime);
169  // printf("Ktate %f : phi %f vl %f vr %f\n\t",ltime,
170  // lphi,rec.vmsec[REAR][LEFT],rec.vmsec[REAR][RIGHT]);
171  // calculate speed
172  vmean = (rec.vmsec[REAR][LEFT] + rec.vmsec[REAR][RIGHT])*0.5;
173  phi = lphi;
174  timestamp = ltime;
175 #elif defined VISP_HAVE_CYCABTK
176  // New low level controller based on Syndex (to use)
177  CycabState cycab_state;
178  store.readVariable(cycab_stateId, cycab_state);
179  vmean = (cycab_state.v_rear_left + cycab_state.v_rear_right)*0.5;
180  //printf("vl %f vr %f\n", cycab_state.v_rear_left, cycab_state.v_rear_right);
181  phi = cycab_state.phi_front;
182  timeval tp = store.getTimestamp(cycab_stateId);
183  timestamp = 1000.0*tp.tv_sec + tp.tv_usec/1000.0;
184 #endif
185 }
197 void vpRobotCycab::getOdometry(double &vfl, double &vfr,
198  double &vrl, double &vrr,
199  double &phi)
200 {
201  double timestamp;
202  getOdometry(vfl, vfr, vrl, vrr, phi, timestamp);
203 }
204 
218 void vpRobotCycab::getOdometry(double &vfl, double &vfr,
219  double &vrl, double &vrr,
220  double &phi, double &timestamp)
221 {
222  vfr = vfr = vrl = vrr = 0.;
223  phi=0.;
224  timestamp=0.;
225 
226 #ifdef VISP_HAVE_CYCABTK_OLD
227  // Old low level controller based on Syndex (obsolete)
228  Record rec;
229  double dsl,dsr,lphi,ltime;
230  cycab->waitUpdate();
231  cycab->getState(&rec, &dsl, &dsr, &lphi, &ltime);
232  // printf("Ktate %f : phi %f vl %f vr %f\n\t",ltime,
233  // lphi,rec.vmsec[REAR][LEFT],rec.vmsec[REAR][RIGHT]);
234  // calculate speed
235  vfl = rec.vmsec[FRONT][LEFT];
236  vfr = rec.vmsec[FRONT][RIGHT];
237  vrl = rec.vmsec[REAR][LEFT];
238  vrr = rec.vmsec[REAR][RIGHT];
239  phi = lphi;
240  timestamp = ltime;
241 #elif defined VISP_HAVE_CYCABTK
242  // New low level controller based on Syndex (to use)
243  CycabState cycab_state;
244  store.readVariable(cycab_stateId, cycab_state);
245  vfl = cycab_state.v_front_left;
246  vfr = cycab_state.v_front_right;
247  vrl = cycab_state.v_rear_left;
248  vrr = cycab_state.v_rear_right;
249 
250  phi = cycab_state.phi_front;
251  timeval tp = store.getTimestamp(cycab_stateId);
252  timestamp = 1000.0*tp.tv_sec + tp.tv_usec/1000.0;
253 #endif
254 }
255 
262 void vpRobotCycab::getJoystickPosition(double &x, double &y)
263 {
264  double timestamp;
265  getJoystickPosition(x, y, timestamp);
266 }
267 
276 void vpRobotCycab::getJoystickPosition(double &x, double &y, double &timestamp)
277 {
278 #ifdef VISP_HAVE_CYCABTK_OLD
279  // Old low level controller based on Syndex (obsolete)
280  x = 0.;
281  y = 0.;
282  timestamp = 0.;
283 #elif defined VISP_HAVE_CYCABTK
284  // New low level controller based on Syndex (to use)
285  CycabState cycab_state;
286  store.readVariable(cycab_stateId, cycab_state);
287  x = cycab_state.joy_x;
288  y = cycab_state.joy_y;
289  timeval tp = store.getTimestamp(cycab_stateId);
290  timestamp = 1000.0*tp.tv_sec + tp.tv_usec/1000.0;
291 #endif
292 }
293 
294 
295 #endif