iCub-main
eomcUtils.h
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 
4 /* Copyright (C) 2012 iCub Facility, Istituto Italiano di Tecnologia
5  * Author: Valentina Gaggero
6  * email: valentina.gaggero@iit.it
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * A copy of the license can be found at
12  * http://www.robotcub.org/icub/license/gpl.txt
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
17  * Public License for more details
18  */
19 
20 //here there is only a name space that contains all utils funtion of embObjMotionControl
21 
22 namespace yarp {
23  namespace dev {
24  namespace eomc
25 
26 {
27  enum { MAX_SHORT = 32767, MIN_SHORT = -32768, MAX_INT = 0x7fffffff, MIN_INT = 0x80000000, MAX_U32 = 0xffffffff, MIN_U32 = 0x00, MAX_U16 = 0xffff, MIN_U16 = 0x0000};
28  enum { CAN_SKIP_ADDR = 0x80 };
29 
30  // saturation check and rounding for 16 bit unsigned integer
31  int U_16(double x)
32  {
33  if (x <= double(MIN_U16) )
34  return MIN_U16;
35  else
36  if (x >= double(MAX_U16))
37  return MAX_U16;
38  else
39  return int(x + .5);
40  }
41 
42  // saturation check and rounding for 16 bit signed integer
43  short S_16(double x)
44  {
45  if (x <= double(-(MAX_SHORT))-1)
46  return MIN_SHORT;
47  else
48  if (x >= double(MAX_SHORT))
49  return MAX_SHORT;
50  else
51  if (x>0)
52  return short(x + .5);
53  else
54  return short(x - .5);
55  }
56 
57  // saturation check and rounding for 32 bit unsigned integer
58  int U_32(double x)
59  {
60  if (x <= double(MIN_U32) )
61  return MIN_U32;
62  else
63  if (x >= double(MAX_U32))
64  return MAX_U32;
65  else
66  return int(x + .5);
67  }
68 
69  // saturation check and rounding for 32 bit signed integer
70  int S_32(double x)
71  {
72  if (x <= double(-(MAX_INT))-1.0)
73  return MIN_INT;
74  else
75  if (x >= double(MAX_INT))
76  return MAX_INT;
77  else
78  if (x>0)
79  return int(x + .5);
80  else
81  return int(x - .5);
82  }
83 
84  bool EncoderType_iCub2eo(const string* in, uint8_t *out)
85  {
86  if (*in == "NONE")
87  {
88  *out = 0;
89  return true;
90  }
91  else if (*in == "AEA")
92  {
93  *out = 1;
94  return true;
95  }
96  else if (*in == "ROIE")
97  {
98  *out = 2;
99  return true;
100  }
101  else if (*in == "HALL_ADC")
102  {
103  *out = 3;
104  return true;
105  }
106  else if (*in == "MAIS")
107  {
108  *out = 4;
109  return true;
110  }
111  else if (*in == "OPTICAL_QUAD")
112  {
113  *out = 5;
114  return true;
115  }
116  else if (*in == "HALL_MOTOR_SENS")
117  {
118  *out = 6;
119  return true;
120  }
121  *out = 0;
122  return false;
123  }
124 
125  bool EncoderType_eo2iCub(const uint8_t *in, string* out)
126  {
127  if (*in == 0)
128  {
129  *out = "NONE";
130  return true;
131  }
132  else if (*in == 1)
133  {
134  *out = "AEA";
135  return true;
136  }
137  else if (*in == 2)
138  {
139  *out = "ROIE";
140  return true;
141  }
142  else if (*in == 3)
143  {
144  *out = "HALL_ADC";
145  return true;
146  }
147  else if (*in == 4)
148  {
149  *out = "MAIS";
150  return true;
151  }
152  else if (*in == 5)
153  {
154  *out = "OPTICAL_QUAD";
155  return true;
156  }
157  else if (*in == 6)
158  {
159  *out = "HALL_MOTOR_SENS";
160  return true;
161  }
162  else if(*in == 9)
163  {
164  *out = "AMO";
165  return true;
166  }
167  *out = "ERROR";
168  return false;
169  }
170 
171  void copyPid_iCub2eo(const Pid *in, eOmc_PID_t *out)
172  {
173  memset(out, 0, sizeof(eOmc_PID_t)); // marco.accame: it is good thing to clear the out struct before copying. this prevent future members of struct not yet managed to be dirty.
174  out->kp = (float) (in->kp);
175  out->ki = (float) (in->ki);
176  out->kd = (float) (in->kd);
177  out->limitonintegral = (float)(in->max_int);
178  out->limitonoutput = (float)(in->max_output);
179  out->offset = (float) (in->offset);
180  out->scale = (int8_t) (in->scale);
181  out->kff = (float) (in->kff);
182  out->stiction_down_val = (float)(in->stiction_down_val);
183  out->stiction_up_val = (float)(in->stiction_up_val);
184  }
185 
186  void copyPid_eo2iCub(eOmc_PID_t *in, Pid *out)
187  {
188  // marco.accame: in here i dont clear the out class because there is not a clear() method
189  out->kp = (double) in->kp;
190  out->ki = (double) in->ki;
191  out->kd = (double) in->kd;
192  out->max_int = (double) in->limitonintegral;
193  out->max_output = (double) in->limitonoutput;
194  out->offset = (double) in->offset;
195  out->scale = (double) in->scale;
196  out->setStictionValues(in->stiction_up_val, in->stiction_down_val);
197  out->setKff(in->kff);
198  }
199 
200 
201  bool controlModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
202  {
203  bool ret = true;
204 
205  switch(vocabMode)
206  {
207  case VOCAB_CM_IDLE:
208  embOut = eomc_controlmode_cmd_idle;
209  break;
210 
211  case VOCAB_CM_POSITION:
212  embOut = eomc_controlmode_cmd_position;
213  break;
214 
215  case VOCAB_CM_POSITION_DIRECT:
216  embOut = eomc_controlmode_cmd_direct;
217  break;
218 
219  case VOCAB_CM_VELOCITY:
220  embOut = eomc_controlmode_cmd_velocity;
221  break;
222 
223  case VOCAB_CM_MIXED:
224  embOut = eomc_controlmode_cmd_mixed;
225  break;
226 
227  case VOCAB_CM_TORQUE:
228  embOut = eomc_controlmode_cmd_torque;
229  break;
230 
231  case VOCAB_CM_IMPEDANCE_POS:
232  embOut = eomc_controlmode_cmd_impedance_pos;
233  break;
234 
235  case VOCAB_CM_IMPEDANCE_VEL:
236  embOut = eomc_controlmode_cmd_impedance_vel;
237  break;
238 
239  case VOCAB_CM_PWM:
240  embOut = eomc_controlmode_cmd_openloop;
241  break;
242 
243  case VOCAB_CM_CURRENT:
244  embOut = eomc_controlmode_cmd_current;
245  break;
246 
247  case VOCAB_CM_FORCE_IDLE:
248  embOut = eomc_controlmode_cmd_force_idle;
249  break;
250 
251  default:
252  ret = false;
253  break;
254  }
255  return ret;
256  }
257 
258  int controlModeStatusConvert_embObj2yarp(eOenum08_t embObjMode)
259  {
260  int vocabOut;
261  switch(embObjMode)
262  {
263  case eomc_controlmode_idle:
264  vocabOut = VOCAB_CM_IDLE;
265  break;
266 
267  case eomc_controlmode_position:
268  vocabOut = VOCAB_CM_POSITION;
269  break;
270 
271  case eomc_controlmode_velocity:
272  vocabOut = VOCAB_CM_VELOCITY;
273  break;
274 
275  case eomc_controlmode_direct:
276  vocabOut = VOCAB_CM_POSITION_DIRECT;
277  break;
278 
279  case eomc_controlmode_mixed:
280  case eomc_controlmode_velocity_pos: // they are the same, this will probably removed in the future
281  vocabOut = VOCAB_CM_MIXED;
282  break;
283 
284  case eomc_controlmode_torque:
285  vocabOut = VOCAB_CM_TORQUE;
286  break;
287 
288  case eomc_controlmode_calib:
289  vocabOut = VOCAB_CM_CALIBRATING;
290  break;
291 
292  case eomc_controlmode_impedance_pos:
293  vocabOut = VOCAB_CM_IMPEDANCE_POS;
294  break;
295 
296  case eomc_controlmode_impedance_vel:
297  vocabOut = VOCAB_CM_IMPEDANCE_VEL;
298  break;
299 
300  case eomc_controlmode_openloop:
301  vocabOut = VOCAB_CM_PWM;
302  break;
303 
304  case eomc_controlmode_current:
305  vocabOut = VOCAB_CM_CURRENT;
306  break;
307 
308  case eomc_controlmode_hwFault:
309  vocabOut = VOCAB_CM_HW_FAULT;
310  break;
311 
312  case eomc_controlmode_notConfigured:
313  vocabOut = VOCAB_CM_NOT_CONFIGURED;
314  break;
315 
316  case eomc_controlmode_configured:
317  vocabOut = VOCAB_CM_CONFIGURED;
318  break;
319 
320  default:
321  printf("embObj to yarp unknown controlmode %d\n", embObjMode);
322  vocabOut = VOCAB_CM_UNKNOWN;
323  break;
324  }
325  return vocabOut;
326  }
327 
328  bool controlModeStatusConvert_yarp2embObj(int vocabMode, eOmc_controlmode_t &embOut)
329  {
330  yError() << "controlModeStatusConvert_yarp2embObj" << " is not yet implemented for embObjMotionControl";
331  return false;
332  }
333 
334  int controlModeCommandConvert_embObj2yarp(eOmc_controlmode_command_t embObjMode)
335  {
336  yError() << "embObjMotionControl::controlModeCommandConvert_embObj2yarp" << " is not yet implemented for embObjMotionControl";
337  return 0;
338 
339  }
340  /*
341  eOmc_pidoutputtype_t pidOutputTypeConver_eomc2fw(PidAlgorithmType_t controlLaw)
342  {
343  switch(controlLaw)
344  {
345  case PidAlgo_simple:
346  return(eomc_pidoutputtype_pwm);
347 
348  case PIdAlgo_velocityInnerLoop:
349  return(eomc_pidoutputtype_vel);
350 
351  case PidAlgo_currentInnerLoop:
352  return(eomc_pidoutputtype_iqq);
353  default:
354  {
355  yError() << "pidOutputTypeConver_eomc2fw: unknown pid output type" ;
356  return(eomc_pidoutputtype_unknown);
357  }
358  }
359  }
360  */
361 
362  bool interactionModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
363  {
364  bool ret = true;
365 
366  switch(vocabMode)
367  {
368  case VOCAB_IM_STIFF:
369  embOut = eOmc_interactionmode_stiff;
370  break;
371 
372  case VOCAB_IM_COMPLIANT:
373  embOut = eOmc_interactionmode_compliant;
374  break;
375 
376  default:
377  ret = false;
378  break;
379  }
380  return ret;
381  }
382 
383 
384  bool interactionModeStatusConvert_embObj2yarp(eOenum08_t embObjMode, int &vocabOut)
385  {
386  bool ret = true;
387  switch(embObjMode)
388  {
389  case eOmc_interactionmode_stiff:
390  vocabOut = VOCAB_IM_STIFF;
391  break;
392 
393  case eOmc_interactionmode_compliant:
394  vocabOut = VOCAB_IM_COMPLIANT;
395  break;
396 
397  default:
398  vocabOut = 666; //VOCAB_CM_UNKNOWN;
399  yError() << "Received an unknown interactionMode from the EMS boards with value " << embObjMode;
400  // ret = false;
401  break;
402  }
403  return ret;
404  }
405 
406 
407 
408  }
409  }}
int S_32(double x)
Definition: eomcUtils.h:70
bool EncoderType_eo2iCub(const uint8_t *in, string *out)
Definition: eomcUtils.h:125
int controlModeStatusConvert_embObj2yarp(eOenum08_t embObjMode)
Definition: eomcUtils.h:258
int controlModeCommandConvert_embObj2yarp(eOmc_controlmode_command_t embObjMode)
Definition: eomcUtils.h:334
bool interactionModeStatusConvert_embObj2yarp(eOenum08_t embObjMode, int &vocabOut)
Definition: eomcUtils.h:384
bool controlModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
Definition: eomcUtils.h:201
void copyPid_eo2iCub(eOmc_PID_t *in, Pid *out)
Definition: eomcUtils.h:186
short S_16(double x)
Definition: eomcUtils.h:43
bool controlModeStatusConvert_yarp2embObj(int vocabMode, eOmc_controlmode_t &embOut)
Definition: eomcUtils.h:328
void copyPid_iCub2eo(const Pid *in, eOmc_PID_t *out)
Definition: eomcUtils.h:171
bool interactionModeCommandConvert_yarp2embObj(int vocabMode, eOenum08_t &embOut)
Definition: eomcUtils.h:362
int U_16(double x)
Definition: eomcUtils.h:31
bool EncoderType_iCub2eo(const string *in, uint8_t *out)
Definition: eomcUtils.h:84
int U_32(double x)
Definition: eomcUtils.h:58
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8