iCub-main
CanBusMotionControl.cpp
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) 2008 Robotcub Consortium
5 * Author: Lorenzo Natale
6 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
7 *
8 */
13 
15 
16 #include <cstdarg>
17 #include <cstdio>
18 #include <cstring>
19 #include <cmath>
20 
21 #include <string>
22 #include <iostream>
23 #include <algorithm>
24 
25 #include <yarp/os/Time.h>
26 #include <yarp/dev/PolyDriver.h>
27 #include <ace/config.h>
28 #include <ace/Log_Msg.h>
29 #include <yarp/os/LogStream.h>
30 #include <yarp/os/Log.h>
31 
32 //the following activates the DEBUG macro in canControlUtil.h
33 //#define CAN_DEBUG
34 //#define CANBUSMC_DEBUG
35 
36 #include "ThreadTable2.h"
37 #include "ThreadPool2.h"
38 
40 #include "CanBusMotionControl.h"
41 
42 #include "can_string_generic.h"
44 #include "messages.h"
45 #include <yarp/os/Log.h>
46 #include <yarp/os/LogStream.h>
47 #include <yarp/dev/ControlBoardInterfacesImpl.h>
48 
49 #include "canControlConstants.h"
50 #include "canControlUtils.h"
51 
52 #ifdef WIN32
53  #pragma warning(once:4355)
54 #endif
55 
56 const int REPORT_PERIOD=6; //seconds
57 const double BCAST_STATUS_TIMEOUT=6; //seconds
58 
59 using namespace std;
60 using namespace yarp;
61 using namespace yarp::os;
62 using namespace yarp::dev;
63 
64 inline void PRINT_CAN_MESSAGE(const char *str, CanMessage &m)
65 {
66 #ifdef CANBUSMC_DEBUG
67  yDebug("%s", str);
68  yDebug(" S:%d R:%d Ch:%d M:%d\n", getSender(m), getRcp(m), m.getData()[0]&0x80, m.getData()[0]&0x7F);
69 #endif
70 }
71 
72 inline bool NOT_YET_IMPLEMENTED(const char *txt)
73 {
74  yError("%s not yet implemented for CanBusMotionControl\n", txt);
75 
76  return false;
77 }
78 
79 inline bool NOT_YET_IMPLEMENTED_WARNING(const char *txt)
80 {
81  yWarning("%s not yet implemented for CanBusMotionControl\n", txt);
82 
83  return false;
84 }
85 
86 inline bool DEPRECATED (const char *txt)
87 {
88  yError("%s has been deprecated for CanBusMotionControl\n", txt);
89 
90  return false;
91 }
92 
93 
94 //generic function that check is key1 is present in input bottle and that the result has size elements
95 // return true/false
96 bool validate(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
97 {
98  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
99  if (tmp.isNull())
100  {
101  yError("%s not found\n", key1.c_str());
102  return false;
103  }
104  if(tmp.size()!=size)
105  {
106  yError("%s incorrect number of entries\n", key1.c_str());
107  return false;
108  }
109  out=tmp;
110  return true;
111 }
112 
113 bool validate_optional(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
114 {
115  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
116  if (tmp.isNull())
117  {
118  return false;
119  }
120  if(tmp.size()!=size)
121  {
122  yError("%s incorrect number of entries\n", key1.c_str());
123  return false;
124  }
125  out=tmp;
126  return true;
127 }
128 
130 
134 {
135  void resetStats()
136  {
137  _count=0;
138  _minDt=10e22;
139  _maxDt=0;
140  _accDt=0;
141  _lastDt=0;
142  _stamp=Time::now();
143  }
144 
146  {
147  _value=0;
148  resetStats();
149  }
150 
151  int _value;
152  double _stamp;
153  double _accDt;
154  int _count;
155  double _maxDt;
156  double _minDt;
157  double _lastDt;
158 
159  inline void update(int value)
160  { _value=value; }
161 
162  inline int getValue()
163  { return _value; }
164 
165  inline void getStats(int &it, double &dT, double &min, double &max)
166  {
167  it=_count;
168  if (_count==0)
169  {
170  dT=0;
171  min=0;
172  max=0;
173  }
174  else
175  {
176  dT=_accDt/_count;
177  min=_minDt;
178  max=_maxDt;
179  }
180  //scale to ms
181  dT*=1000;
182  min*=1000;
183  max*=1000;
184  }
185 
186  inline void update(int value, double st)
187  {
188  double tmpDt=st-_stamp;
189  _value=value;
190  _stamp=st;
191  _accDt+=tmpDt;
192  _lastDt=tmpDt;
193  if (tmpDt>_maxDt)
194  _maxDt=tmpDt;
195  if (tmpDt<_minDt)
196  _minDt=tmpDt;
197 
198  _count++;
199  }
200 
201  inline double getStamp()
202  {
203  return _stamp;
204  }
205 };
206 
207 
208 
209 
210 
213 {
214 public:
215  // msg 1
220 
221  // msg 2
222  short _pid_value;
223  double _update_v;
224 
225  // msg 3
226  short _axisStatus;
229  unsigned char _controlmodeStatus;
230  double _update_e;
231 
232  // msg 3b
233  unsigned char _interactionmodeStatus;
234  double _update_e2;
235 
236  // msg 4
237  short _current;
238  double _update_c;
239 
240  // msg 4b
243  double _update_r;
244 
245  // msg 4c
248  double _update_s;
249 
250  // msg 4d (special message)
251  double _torque;
252  double _update_t;
253 
254  // msg 6
255  unsigned int _canTxError;
256  // msg 7
257  unsigned int _canRxError;
258 
259  // Main Loop Overflow Counter
261 
262  int _address;
263 
265  bool isFaultOk () { return (!_axisStatus ) ; }
266  bool isFaultUndervoltage () { return _axisStatus & 0x01; }
267  bool isFaultOverload () { return _axisStatus & 0x02; }
268  bool isOverCurrent () { return _axisStatus & 0x08; }
269  bool isFaultExternal () { return _axisStatus & 0x04; }
270  bool isHallSensorError () { return _axisStatus & 0x10; }
271  bool isAbsEncoderError () { return _axisStatus & 0x20; }
272  bool isOpticalEncoderError () { return _axisStatus & 0x40; }
273  bool isCanTxOverflow () { return _canStatus & 0x01; }
274  bool isCanBusOff () { return _canStatus & 0x02; }
275  bool isCanTxError () { return _canStatus & 0x04; }
276  bool isCanRxError () { return _canStatus & 0x08; }
277  bool isCanTxOverrun () { return _canStatus & 0x10; }
278  bool isCanRxWarning () { return _canStatus & 0x20; }
279  bool isCanRxOverrun () { return _canStatus & 0x40; }
280  bool isMainLoopOverflow () { return _boardStatus & 0x01; }
281  bool isOverTempCh1 () { return _boardStatus & 0x02; }
282  bool isOverTempCh2 () { return _boardStatus & 0x04; }
283  bool isTempErrorCh1 () { return _boardStatus & 0x08; }
284  bool isTempErrorCh2 () { return _boardStatus & 0x10; }
285  void ControlStatus (int net, short controlmode,short addr)
286  {
287  switch (controlmode)
288  {
289  case icubCanProto_controlmode_idle:
290  yInfo ("[%d] board %d MODE_IDLE \r\n", net, addr);
291  break;
292  case icubCanProto_controlmode_position:
293  yInfo ("[%d] board %d MODE_POSITION \r\n", net, addr);
294  break;
295  case icubCanProto_controlmode_velocity:
296  yInfo ("[%d] board %d MODE_VELOCITY \r\n", net, addr);
297  break;
298  case icubCanProto_controlmode_torque:
299  yInfo ("[%d] board %d MODE_TORQUE \r\n", net, addr);
300  break;
301  case icubCanProto_controlmode_impedance_pos:
302  yInfo ("[%d] board %d MODE_IMPEDANCE_POS \r\n", net, addr);
303  break;
304  case icubCanProto_controlmode_impedance_vel:
305  yInfo ("[%d] board %d MODE_IMPEDANCE_VEL \r\n", net, addr);
306  break;
307  default:
308  yError ("[%d] board %d MODE_UNKNOWN \r\n", net, addr);
309  break;
310  }
311  }
312 
313 
314  void zero (void)
315  {
316  _position_joint._value = 0;
317  _position_rotor._value = 0;
318  _speed_rotor._value = 0;
319  _accel_rotor._value = 0;
320  _pid_value = 0;
321  _current = 0;
322  _axisStatus=0;
323  _canStatus=0;
324  _boardStatus=0;
325  _controlmodeStatus=0;
326  _interactionmodeStatus=0;
327  _position_error = 0;
328  _torque_error = 0;
329  _speed_joint = 0;
330  _accel_joint = 0;
331 
332  _torque=0;
333  _update_t = .0;
334 
335  _update_v = .0;
336  _update_e = .0;
337  _update_e2= .0;
338  _update_c = .0;
339  _update_r = .0;
340  _update_s = .0;
341 
342  _address=-1;
343  _canTxError=0;
344  _canRxError=0;
345  _mainLoopOverflowCounter=0;
346  }
347 };
348 
349 
350 #include <stdarg.h>
351 #include <stdio.h>
352 const int PRINT_BUFFER_LENGTH=255;
354 {
355 private:
357  void operator= (const CanBusResources&);
358  PolyDriver polyDriver;
359 
360 public:
361  ICanBus *iCanBus;
362  ICanBufferFactory *iBufferFactory;
363  ICanBusErrors *iCanErrors;
364 
365  CanBusResources ();
366  ~CanBusResources ();
367 
368  bool initialize (const CanBusMotionControlParameters& parms);
369  bool initialize (yarp::os::Searchable &config);
370  bool uninitialize ();
371  bool read ();
372 
373  bool startPacket ();
374  bool addMessage (int msg_id, int joint);
375  // add message, append request to request list
376  bool addMessage (int id, int joint, int msg_id);
377 
378  bool writePacket ();
379 
380  bool printMessage (const CanMessage &m);
381  bool dumpBuffers (void);
382  inline int getJoints (void) const { return _njoints; }
383  inline bool getErrorStatus (void) const { return _error_status;}
384 
385  void printMessage(const char *fmt, ...)
386  {
387  va_list ap;
388  va_start(ap, fmt);
389 #ifdef WIN32
390  _vsnprintf(buffer, DEBUG_PRINTF_BUFFER_LENGTH, fmt, ap);
391 #else
392  vsnprintf(buffer, DEBUG_PRINTF_BUFFER_LENGTH, fmt, ap);
393 #endif
394  yDebug("%s", buffer);
395  va_end(ap);
396  }
397 
399 public:
400  enum { CAN_TIMEOUT = 20, CAN_POLLING_INTERVAL = 10 };
401 
402  // HANDLE _handle;/// the actual ddriver handle.
404  unsigned int _timeout;
405 
407  int _speed;
409 
414 
415  unsigned int _readMessages;
416  unsigned int _writeMessages;
417  unsigned int _echoMessages;
418 
419  CanBuffer _readBuffer;
420  CanBuffer _writeBuffer;
421  CanBuffer _replyBuffer;
422  CanBuffer _echoBuffer;
423 
425 
426  unsigned char _my_address;
427  unsigned char _destinations[CAN_MAX_CARDS];
428  int _velShifts[CAN_MAX_CARDS];
429  unsigned char *_destInv;
430  std::string* _jointNames;
431 
432  int _njoints;
433 
435 
437  int _filter;
438 
439  char _printBuffer[16384];
441 };
442 inline CanBusResources& RES(void *res) { return *(CanBusResources *)res; }
443 
444 class TBR_CanBackDoor: public BufferedPort<Bottle>
445 {
446  CanBusResources *bus;
447  std::recursive_mutex *backdoor_mutex;
448  TBR_AnalogSensor *ownerSensor;
449  bool canEchoEnabled;
450 
451 public:
453  {
454  bus=0;
455  ownerSensor=0;
456  canEchoEnabled=false;
457  backdoor_mutex=nullptr;
458  }
459 
460  void setUp(CanBusResources *p, std::recursive_mutex *mut, bool echo, TBR_AnalogSensor *owner)
461  {
462  backdoor_mutex=mut;
463  bus=p;
464  ownerSensor= owner;
465  useCallback();
466  canEchoEnabled = echo;
467  }
468 
469  virtual void onRead(Bottle &b);
470 };
471 
472 
473 void TBR_CanBackDoor::onRead(Bottle &b)
474 {
475  if (!backdoor_mutex)
476  return;
477 
478  double dval[6] = {0,0,0,0,0,0};
479 
480  std::lock_guard<std::recursive_mutex> lck(*backdoor_mutex);
481  //RANDAZ_TODO: parse vector b
482  int len = b.size();
483  int commandId = b.get(0).asInt32();
484  int i=0;
485 
486  static double timePrev=Time::now();
487  static int count_timeout=0;
488  static int count_saturation_i[6]={0,0,0,0,0,0};
489  static int count_saturation=0;
490  double timeNow=Time::now();
491  double diff = timeNow - timePrev;
492 
493  if (diff > 0.012)
494  {
495  count_timeout++;
496  yWarning("PORT: %s **** TIMEOUT : %f COUNT: %d \n", this->getName().c_str(), diff, count_timeout);
497  }
498  timePrev=Time::now();
499 
500  switch (commandId)
501  {
502  case 1: //shoulder torque message
503  dval[0] = b.get(1).asFloat64(); //shoulder 1 pitch
504  dval[1] = b.get(2).asFloat64(); //shoulder 2 roll
505  dval[2] = b.get(3).asFloat64(); //shoulder 3 yaw
506  dval[3] = b.get(4).asFloat64(); //elbow
507  dval[4] = b.get(5).asFloat64(); //wrist pronosupination
508  dval[5] = 0;
509  break;
510  case 2: //legs torque message
511  dval[0] = b.get(1).asFloat64(); //hip pitch
512  dval[1] = b.get(2).asFloat64(); //hip roll
513  dval[2] = b.get(3).asFloat64(); //hip yaw
514  dval[3] = b.get(4).asFloat64(); //knee
515  dval[4] = b.get(5).asFloat64(); //ankle pitch
516  dval[5] = b.get(6).asFloat64(); //ankle roll
517  break;
518  case 3:
519  dval[0] = b.get(6).asFloat64(); //wrist yaw
520  dval[1] = b.get(7).asFloat64(); //wrist pitch
521  dval[2] = 0;
522  dval[3] = 0;
523  dval[4] = 0;
524  dval[5] = 0;
525  break;
526  case 4:
527  dval[0] = b.get(1).asFloat64(); //torso yaw (respect gravity)
528  dval[1] = b.get(2).asFloat64(); //torso roll (lateral movement)
529  dval[2] = b.get(3).asFloat64(); //torso pitch (front-back movement)
530  dval[3] = 0;
531  dval[4] = 0;
532  dval[5] = 0;
533  break;
534  default:
535  yWarning("Got unexpected message on backdoor: %s\n", this->getName().c_str());
536  return;
537  break;
538  }
539 
540  if (bus && ownerSensor)
541  {
542  int fakeId = 0;
543  int boardId = ownerSensor->getId();
544  short int val[6] = {0,0,0,0,0,0};
545  static double curr_time = Time::now();
546  for (i=0; i<6; i++)
547  {
548  double fullScale = ownerSensor->getScaleFactor()[i];
549  if (dval[i] > fullScale)
550  {
551  if (Time::now() - curr_time > 2)
552  {
553  yWarning("PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", this->getName().c_str(), i, dval[i], count_saturation);
554  curr_time = Time::now();
555  }
556  dval[i] = fullScale;
557  count_saturation++;
558  }
559  else if (dval[i] < -fullScale)
560  {
561  if (Time::now() - curr_time > 2)
562  {
563  yWarning("PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", this->getName().c_str(), i, dval[i], count_saturation);
564  curr_time = Time::now();
565  }
566  dval[i] = -fullScale;
567  count_saturation++;
568  }
569  val[i] = (short int)(dval[i] / fullScale * 0x7fff)+0x8000; //check this!
570  }
571 
572  bus->startPacket();
573  fakeId = 0x300 + (boardId<<4)+ 0x0A;
574  bus->_writeBuffer[0].setId(fakeId);
575  bus->_writeBuffer[0].getData()[1]=(val[0] >> 8) & 0xFF;
576  bus->_writeBuffer[0].getData()[0]= val[0] & 0xFF;
577  bus->_writeBuffer[0].getData()[3]=(val[1] >> 8) & 0xFF;
578  bus->_writeBuffer[0].getData()[2]= val[1] & 0xFF;
579  bus->_writeBuffer[0].getData()[5]=(val[2] >> 8) & 0xFF;
580  bus->_writeBuffer[0].getData()[4]= val[2] & 0xFF;
581  bus->_writeBuffer[0].setLen(6);
582  bus->_writeMessages++;
583  bus->writePacket();
584 
585  if (canEchoEnabled)
586  {
587  if (bus->_readMessages<BUF_SIZE-1 && bus->_echoMessages<BUF_SIZE-1)
588  {
589  bus->_echoBuffer[bus->_echoMessages].setId(fakeId);
590  bus->_echoBuffer[bus->_echoMessages].setLen(6);
591  for (i=0; i<6; i++)
592  bus->_echoBuffer[bus->_echoMessages].getData()[i]=bus->_writeBuffer[0].getData()[i];
593  bus->_echoMessages++;
594  }
595  else
596  {
597  yError("Echobuffer full \n");
598  }
599  }
600 
601  bus->startPacket();
602  fakeId = 0x300 + (boardId<<4)+ 0x0B;
603  bus->_writeBuffer[0].setId(fakeId);
604  bus->_writeBuffer[0].getData()[1]=(val[3] >> 8) & 0xFF;
605  bus->_writeBuffer[0].getData()[0]= val[3] & 0xFF;
606  bus->_writeBuffer[0].getData()[3]=(val[4] >> 8) & 0xFF;
607  bus->_writeBuffer[0].getData()[2]= val[4] & 0xFF;
608  bus->_writeBuffer[0].getData()[5]=(val[5] >> 8) & 0xFF;
609  bus->_writeBuffer[0].getData()[4]= val[5] & 0xFF;
610  bus->_writeBuffer[0].setLen(6);
611  bus->_writeMessages++;
612  bus->writePacket();
613 
614  if (canEchoEnabled)
615  {
616  if (bus->_readMessages<BUF_SIZE-1 && bus->_echoMessages<BUF_SIZE-1)
617  {
618  bus->_echoBuffer[bus->_echoMessages].setId(fakeId);
619  bus->_echoBuffer[bus->_echoMessages].setLen(6);
620  for (i=0; i<6; i++)
621  bus->_echoBuffer[bus->_echoMessages].getData()[i]=bus->_writeBuffer[0].getData()[i];
622  bus->_echoMessages++;
623  }
624  else
625  {
626  yError("Echobuffer full \n");
627  }
628  }
629 
630  }
631 }
632 
634 {
635  jointsNum=njoints;
636  estim_params = new SpeedEstimationParameters [jointsNum];
637  if (estim_parameters!=0)
638  memcpy(estim_params, estim_parameters, sizeof(SpeedEstimationParameters)*jointsNum);
639  else
640  memset(estim_params, 0, sizeof(SpeedEstimationParameters)*jointsNum);
641 }
642 
644 {
645  jointsNum=njoints;
646  impLimits = new ImpedanceLimits [jointsNum];
647  if (impLimits != 0)
648  memcpy(impLimits, imped_limits, sizeof(ImpedanceLimits)*jointsNum);
649  else
650  memset(impLimits, 0, sizeof(ImpedanceLimits)*jointsNum);
651 }
652 
653 axisPositionDirectHelper::axisPositionDirectHelper(int njoints, const int *aMap, const double *angToEncs, double* _stepLimit)
654 {
655  jointsNum=njoints;
656  helper = new ControlBoardHelper(njoints, aMap, angToEncs, 0, 0); //NB: zeros=0 is mandatory for this algorithm
657  maxHwStep = new double [jointsNum];
658  maxUserStep = new double [jointsNum];
659  for (int i=0; i<jointsNum; i++)
660  {
661  int hw_i=0;
662  double hw_ang=0;
663  helper->posA2E(_stepLimit[i], i, hw_ang, hw_i);
664  maxHwStep[hw_i] = fabs(hw_ang); //NB: fabs() is mandatory for this algorithm (because angToEncs is signed)
665  maxUserStep[i] = _stepLimit[i];
666  }
667 }
668 
669 double axisPositionDirectHelper::getSaturatedValue (int hw_j, double hw_curr_value, double hw_ref_value)
670 {
671  //here hw_j isthe joint number after the axis_map; hw_value is in encoder ticks
672  double hw_maxval = hw_curr_value + getMaxHwStep(hw_j);
673  double hw_minval = hw_curr_value - getMaxHwStep(hw_j);
674  double hw_val = (std::min)((std::max)(hw_ref_value, hw_minval), hw_maxval);
675 
676  return hw_val;
677 }
678 
679 CanBusMotionControl::torqueControlHelper::torqueControlHelper(int njoints, double* p_angleToEncoders, double* p_newtonsTosens )
680 {
681  jointsNum=njoints;
682  newtonsToSensor = new double [jointsNum];
683  angleToEncoders = new double [jointsNum];
684 
685  if (p_angleToEncoders!=0)
686  memcpy(angleToEncoders, p_angleToEncoders, sizeof(double)*jointsNum);
687  else
688  for (int i=0; i<jointsNum; i++) {angleToEncoders[i]=1.0;}
689 
690  if (p_newtonsTosens!=0)
691  memcpy(newtonsToSensor, p_newtonsTosens, sizeof(double)*jointsNum);
692  else
693  for (int i=0; i<jointsNum; i++) {newtonsToSensor[i]=1.0;}
694 }
695 
696 axisTorqueHelper::axisTorqueHelper(int njoints, int* id, int* chan, double* maxTrq, double* newtons2sens )
697 {
698  jointsNum=njoints;
699  torqueSensorId = new int [jointsNum];
700  torqueSensorChan = new int [jointsNum];
701  maximumTorque = new double [jointsNum];
702  newtonsToSensor = new double [jointsNum];
703  if (id!=0)
704  memcpy(torqueSensorId, id, sizeof(int)*jointsNum);
705  else
706  memset(torqueSensorId, 0, sizeof(int)*jointsNum);
707  if (chan!=0)
708  memcpy(torqueSensorChan, chan, sizeof(int)*jointsNum);
709  else
710  memset(torqueSensorChan, 0, sizeof(int)*jointsNum);
711  if (maxTrq!=0)
712  memcpy(maximumTorque, maxTrq, sizeof(double)*jointsNum);
713  else
714  memset(maximumTorque, 0, sizeof(double)*jointsNum);
715  if (newtons2sens!=0)
716  memcpy(newtonsToSensor, newtons2sens, sizeof(double)*jointsNum);
717  else
718  memset(newtonsToSensor, 0, sizeof(double)*jointsNum);
719 
720 }
721 
723 data(0)
724 {
725  isVirtualSensor=false;
726  timeStamp=0;
727  status=IAnalogSensor::AS_OK;
728  useCalibration=0;
729  scaleFactor=0;
730 
731  counterSat=0;
732  counterError=0;
733  counterTimeout=0;
734  backDoor=0;
735 }
736 
738 {
739  if (!data)
740  delete data;
741  if (!scaleFactor)
742  delete scaleFactor;
743 }
744 
746 {
747  return status;
748 }
749 
750 bool TBR_AnalogSensor::open(int channels, AnalogDataFormat f, short bId, short useCalib, bool isVirtualS)
751 {
752  if (data)
753  return false;
754  if (scaleFactor)
755  return false;
756 
757  data=new TBR_AnalogData(channels, channels+1);
758  scaleFactor=new double[channels];
759  int i=0;
760  for (i=0; i<channels; i++) scaleFactor[i]=1;
761  dataFormat=f;
762  boardId=bId;
763  useCalibration=useCalib;
764  isVirtualSensor=isVirtualS;
765  if (useCalibration==1 && dataFormat==TBR_AnalogSensor::ANALOG_FORMAT_16)
766  {
767  scaleFactor[0]=1;
768  scaleFactor[1]=1;
769  scaleFactor[2]=1;
770  scaleFactor[3]=1;
771  scaleFactor[4]=1;
772  scaleFactor[5]=1;
773  }
774 
775  return true;
776 }
777 
778 
780 {
781  return data->size();
782 }
783 
784 int TBR_AnalogSensor::read(yarp::sig::Vector &out)
785 {
786  // print errors
787  std::lock_guard<std::mutex> lck(mtx);
788 
789  if (!data)
790  {
791  return false;
792  }
793 
794  if (status!=IAnalogSensor::AS_OK)
795  {
796  switch (status)
797  {
798  case IAnalogSensor::AS_OVF:
799  {
800  counterSat++;
801  }
802  break;
803  case IAnalogSensor::AS_ERROR:
804  {
805  counterError++;
806  }
807  break;
808  case IAnalogSensor::AS_TIMEOUT:
809  {
810  counterTimeout++;
811  }
812  break;
813  default:
814  {
815  counterError++;
816  }
817  }
818  return status;
819  }
820 
821  out.resize(data->size());
822  for(int k=0;k<data->size();k++)
823  {
824  out[k]=(*data)[k];
825  }
826 
827  return status;
828 }
829 
831 {
832  return AS_OK;
833 }
834 
836 {
837  return AS_OK;
838 }
839 
840 bool TBR_AnalogSensor::decode16(const unsigned char *msg, int id, double *data)
841 {
842 
843  const char groupId=(id&0x00f);
844  int baseIndex=0;
845  {
846  switch (groupId)
847  {
848  case 0xA:
849  {
850  for(int k=0;k<3;k++)
851  {
852  data[k]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
853  if (useCalibration==1)
854  {
855  data[k]=data[k]*scaleFactor[k]/float(0x8000);
856  }
857  }
858  }
859  break;
860  case 0xB:
861  {
862  for(int k=0;k<3;k++)
863  {
864  data[k+3]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
865  if (useCalibration==1)
866  {
867  data[k+3]=data[k+3]*scaleFactor[k+3]/float(0x8000);
868  }
869  }
870  }
871  break;
872  case 0xC:
873  {} //skip these, they are not for us
874  break;
875  case 0xD:
876  {} //skip these, they are not for us
877  break;
878  default:
879  yError("Got unexpected class 0x3 msg(s)\n");
880  return false;
881  break;
882  }
883  //@@@DEBUG ONLY
884  //yDebug(" %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6]);
885  }
886 
887  return true;
888 }
889 
890 bool TBR_AnalogSensor::decode8(const unsigned char *msg, int id, double *data)
891 {
892  const char groupId=(id&0x00f);
893  int baseIndex=0;
894  {
895  switch (groupId)
896  {
897  case 0xC:
898  {
899  for(int k=0;k<=6;k++)
900  data[k]=msg[k];
901  }
902  break;
903  case 0xD:
904  {
905  for(int k=0;k<=7;k++)
906  data[7+k]=msg[k];
907  }
908  break;
909  case 0xA:
910  {} //skip these, they are not for us
911  break;
912  case 0xB:
913  {} //skip these, they are not for us
914  break;
915  default:
916  yWarning("Got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
917  return false;
918  break;
919  }
920  }
921  return true;
922 }
923 
924 
926 {
927  CanBusResources& r = RES (canbus);
928 
929  unsigned int i=0;
930  const int _networkN=r._networkN;
931 
932  bool ret=true; //return true by default
933 
934  std::lock_guard<std::mutex> lck(mtx);
935 
936  double timeNow=Time::now();
937  for (unsigned int buff_num=0; buff_num<2; buff_num++)
938  {
939  unsigned int size = 0;
940  CanBuffer* buffer_pointer=0;
941  if (buff_num==0)
942  {
943  size = r._readMessages;
944  buffer_pointer = &r._readBuffer;
945  }
946  else
947  {
948  size = r._echoMessages;
949  buffer_pointer = &r._echoBuffer;
950  }
951 
952  for (i = 0; i < size; i++)
953  {
954  unsigned int len=0;
955  unsigned int msgid=0;
956  unsigned char *buff=0;
957  CanMessage& m = (*buffer_pointer)[i];
958  buff=m.getData();
959  msgid=m.getId();
960  len=m.getLen();
961 
962  status=IAnalogSensor::AS_OK;
963  const char type=((msgid&0x700)>>8);
964  const char id=((msgid&0x0f0)>>4);
965 
966  if (type==0x03) //analog data
967  {
968  if (id==boardId)
969  {
970  timeStamp=Time::now();
971  switch (dataFormat)
972  {
973  case ANALOG_FORMAT_8:
974  ret=decode8(buff, msgid, data->getBuffer());
975  status=IAnalogSensor::AS_OK;
976  break;
977  case ANALOG_FORMAT_16:
978  if (len==6)
979  {
980  ret=decode16(buff, msgid, data->getBuffer());
981  status=IAnalogSensor::AS_OK;
982  }
983  else
984  {
985  if (len==7 && buff[6] == 1)
986  {
987  status=IAnalogSensor::AS_OVF;
988  }
989  else
990  {
991  status=IAnalogSensor::AS_ERROR;
992  }
993  ret=decode16(buff, msgid, data->getBuffer());
994  }
995  break;
996  default:
997  ret=false;
998  }
999  }
1000  }
1001  }
1002  }
1003 
1004  //if 100ms have passed since the last received message
1005  if (timeStamp+0.1<timeNow)
1006  {
1007  status=IAnalogSensor::AS_TIMEOUT;
1008  }
1009 
1010  return ret;
1011 }
1012 
1013 
1014 bool CanBusMotionControlParameters:: setBroadCastMask(Bottle &list, int MASK)
1015 {
1016  if (list.size()==2)
1017  {
1018  if (list.get(1).asInt32()==1)
1019  {
1020  for(int j=0;j<_njoints;j++)
1021  _broadcast_mask[j]|=(1<<(MASK-1));
1022  }
1023  return true;
1024  }
1025 
1026  if (list.size()==(_njoints+1))
1027  {
1028  for(int k=0;k<_njoints;k++)
1029  {
1030  if ((list.get(k+1).asInt32())==1)
1031  {
1032  int tmp=_axisMap[k];//remap
1033  _broadcast_mask[tmp]|=(1<<(MASK-1));
1034  }
1035  }
1036  return true;
1037  }
1038 
1039  return false;
1040 }
1041 
1042 bool CanBusMotionControlParameters::parsePosPidsGroup_OldFormat(Bottle& pidsGroup, int nj, Pid myPid[])
1043 {
1044  int j=0;
1045  for(j=0;j<nj;j++)
1046  {
1047  char tmp[80];
1048  sprintf(tmp, "Pid%d", j);
1049 
1050  Bottle &xtmp = pidsGroup.findGroup(tmp);
1051  myPid[j].kp = xtmp.get(1).asFloat64();
1052  myPid[j].kd = xtmp.get(2).asFloat64();
1053  myPid[j].ki = xtmp.get(3).asFloat64();
1054 
1055  myPid[j].max_int = xtmp.get(4).asFloat64();
1056  myPid[j].max_output = xtmp.get(5).asFloat64();
1057 
1058  myPid[j].scale = xtmp.get(6).asFloat64();
1059  myPid[j].offset = xtmp.get(7).asFloat64();
1060 
1061  if (xtmp.size()==10)
1062  {
1063  myPid[j].stiction_up_val = xtmp.get(8).asFloat64();
1064  myPid[j].stiction_down_val = xtmp.get(9).asFloat64();
1065  }
1066  }
1067  return true;
1068 }
1069 
1070 bool CanBusMotionControlParameters::parseTrqPidsGroup_OldFormat(Bottle& pidsGroup, int nj, Pid myPid[])
1071 {
1072  int j=0;
1073  for(j=0;j<nj;j++)
1074  {
1075  char tmp[80];
1076  sprintf(tmp, "TPid%d", j);
1077 
1078  Bottle &xtmp = pidsGroup.findGroup(tmp);
1079  myPid[j].kp = xtmp.get(1).asFloat64();
1080  myPid[j].kd = xtmp.get(2).asFloat64();
1081  myPid[j].ki = xtmp.get(3).asFloat64();
1082 
1083  myPid[j].max_int = xtmp.get(4).asFloat64();
1084  myPid[j].max_output = xtmp.get(5).asFloat64();
1085 
1086  myPid[j].scale = xtmp.get(6).asFloat64();
1087  myPid[j].offset = xtmp.get(7).asFloat64();
1088 
1089  if (xtmp.size()==10)
1090  {
1091  myPid[j].stiction_up_val = xtmp.get(8).asFloat64();
1092  myPid[j].stiction_down_val = xtmp.get(9).asFloat64();
1093  }
1094  }
1095  return true;
1096 }
1098 {
1099  int j=0;
1100  Bottle xtmp;
1101 
1102  if (!validate(pidsGroup, xtmp, "kp", "Pid kp parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].kp = xtmp.get(j+1).asFloat64();
1103  if (!validate(pidsGroup, xtmp, "kd", "Pid kd parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].kd = xtmp.get(j+1).asFloat64();
1104  if (!validate(pidsGroup, xtmp, "ki", "Pid kp parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].ki = xtmp.get(j+1).asFloat64();
1105  if (!validate(pidsGroup, xtmp, "maxInt", "Pid maxInt parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].max_int = xtmp.get(j+1).asFloat64();
1106  if (!validate(pidsGroup, xtmp, "maxOutput", "Pid maxOutput parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].max_output = xtmp.get(j+1).asFloat64();
1107  if (!validate(pidsGroup, xtmp, "shift", "Pid shift parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].scale = xtmp.get(j+1).asFloat64();
1108  if (!validate(pidsGroup, xtmp, "ko", "Pid ko parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].offset = xtmp.get(j+1).asFloat64();
1109  if (!validate(pidsGroup, xtmp, "stictionUp", "Pid stictionUp", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
1110  if (!validate(pidsGroup, xtmp, "stictionDwn", "Pid stictionDwn", _njoints+1)) return false; for (j=0; j<_njoints; j++) myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
1111 
1112  //optional kff
1113  xtmp = pidsGroup.findGroup("kff");
1114  if (!xtmp.isNull())
1115  {
1116  for (j=0; j<_njoints; j++) myPid[j].kff = xtmp.get(j+1).asFloat64();
1117  }
1118  else
1119  {
1120  for (j=0; j<_njoints; j++) myPid[j].kff = 0;
1121  }
1122 
1123  return true;
1124 }
1125 
1127 {
1128  int j=0;
1129  Bottle xtmp;
1130  if (!validate(pidsGroup, xtmp, "stiffness", "Pid stiffness parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].stiffness = xtmp.get(j+1).asFloat64();
1131  if (!validate(pidsGroup, xtmp, "damping", "Pid damping parameter", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].damping = xtmp.get(j+1).asFloat64();
1132  return true;
1133 }
1134 
1136 {
1137  int j=0;
1138  Bottle xtmp;
1139 
1140  if (!validate(pidsGroup, xtmp, "debug0", "debug0", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[0] = xtmp.get(j+1).asFloat64();
1141  if (!validate(pidsGroup, xtmp, "debug1", "debug1", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[1] = xtmp.get(j+1).asFloat64();
1142  if (!validate(pidsGroup, xtmp, "debug2", "debug2", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[2] = xtmp.get(j+1).asFloat64();
1143  if (!validate(pidsGroup, xtmp, "debug3", "debug3", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[3] = xtmp.get(j+1).asFloat64();
1144  if (!validate(pidsGroup, xtmp, "debug4", "debug4", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[4] = xtmp.get(j+1).asFloat64();
1145  if (!validate(pidsGroup, xtmp, "debug5", "debug5", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[5] = xtmp.get(j+1).asFloat64();
1146  if (!validate(pidsGroup, xtmp, "debug6", "debug6", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[6] = xtmp.get(j+1).asFloat64();
1147  if (!validate(pidsGroup, xtmp, "debug7", "debug7", _njoints+1)) return false; for (j=0; j<_njoints; j++) vals[j].data[7] = xtmp.get(j+1).asFloat64();
1148 
1149  return true;
1150 }
1152 {
1153  if (!p.check("GENERAL","section for general motor control parameters")) {
1154  yError("Cannot understand configuration parameters\n");
1155  return false;
1156  }
1157 
1158  std::string dbg_string = p.toString().c_str();
1159  int i;
1160  int nj = p.findGroup("GENERAL").check("Joints",Value(1),
1161  "Number of degrees of freedom").asInt32();
1162  alloc(nj);
1163 
1164  // Check useRawEncoderData = do not use calibration data!
1165  bool useRawJointEncoderData = false;
1166  bool useRawMotorEncoderData = false;
1167  Value use_jnt_raw = p.findGroup("GENERAL").find("useRawEncoderData");
1168  Value use_mot_raw = p.findGroup("GENERAL").find("useRawMotorEncoderData");
1169 
1170  if(use_jnt_raw.isNull())
1171  {
1172  useRawJointEncoderData = false;
1173  }
1174  else
1175  {
1176  if(!use_jnt_raw.isBool())
1177  {
1178  yError("useRawEncoderData bool param is different from accepted values (true / false). Assuming false\n");
1179  useRawJointEncoderData = false;
1180  }
1181  else
1182  {
1183  useRawJointEncoderData = use_jnt_raw.asBool();
1184  if(useRawJointEncoderData)
1185  {
1186  yWarning("canBusMotionControl using raw data from encoders! Be careful See 'useRawEncoderData' param in config file \n");
1187  yWarning("DO NOT USE OR CALIBRATE THE ROBOT IN THIS CONFIGURATION! \n");
1188  yWarning("CHECK IF THE FAULT BUTTON IS PRESSED and press ENTER to continue \n");
1189  getchar();
1190  }
1191  }
1192  }
1193 
1194  if(use_mot_raw.isNull())
1195  {
1196  useRawMotorEncoderData = false;
1197  }
1198  else
1199  {
1200  if(!use_mot_raw.isBool())
1201  {
1202  yError("useRawEncoderData bool param is different from accepted values (true / false). Assuming false\n");
1203  useRawMotorEncoderData = false;
1204  }
1205  else
1206  {
1207  useRawMotorEncoderData = use_mot_raw.asBool();
1208  if(useRawMotorEncoderData)
1209  {
1210  yWarning("canBusMotionControl using raw data from motor encoders");
1211  }
1212  }
1213  }
1214  // Check useRawEncoderData = do not use calibration data!
1215  Value use_limitedPWM = p.findGroup("GENERAL").find("useLimitedPWM");
1216  if(use_limitedPWM.isNull())
1217  {
1218  _pwmIsLimited = false;
1219  }
1220  else
1221  {
1222  if(!use_limitedPWM.isBool())
1223  {
1224  _pwmIsLimited = false;
1225  }
1226  else
1227  {
1228  _pwmIsLimited = use_limitedPWM.asBool();
1229  }
1230  }
1231 
1233  Bottle& canGroup = p.findGroup("CAN");
1234 
1235  _networkName=canGroup.find("NetworkId").asString();
1236 
1237  if (canGroup.check("CanForcedDeviceNum"))
1238  {
1239  _networkN=canGroup.find("CanForcedDeviceNum").asInt32();
1240  }
1241  else
1242  _networkN=canGroup.check("CanDeviceNum",Value(-1), "numeric identifier of device").asInt32();
1243 
1244  // yDebug<<can.toString();
1245  if (_networkN<0)
1246  {
1247  yError("can network id not valid, skipping device\n");
1248  return false;
1249  }
1250 
1251  _my_address=canGroup.check("CanMyAddress",Value(0),
1252  "numeric identifier of my address").asInt32();
1253 
1254  _polling_interval=canGroup.check("CanPollingInterval",Value(20),
1255  "polling period").asInt32();
1256 
1257  _timeout=canGroup.check("CanTimeout",Value(20),"timeout period").asInt32();
1258 
1259  _txTimeout=canGroup.check("CanTxTimeout", Value(20), "tx timeout").asInt32();
1260  _rxTimeout=canGroup.check("CanRxTimeout", Value(20), "rx timeout").asInt32();
1261 
1262  // default values for CanTxQueueSize/CanRxQueueSize should be the
1263  // maximum, difficult to pick a correct value, let the driver
1264  // decide on this
1265  if (canGroup.check("CanTxQueueSize"))
1266  _txQueueSize=canGroup.find("CanTxQueueSize").asInt32();
1267  else
1268  _txQueueSize=-1;
1269 
1270  if (canGroup.check("CanRxQueueSize"))
1271  _rxQueueSize=canGroup.find("CanRxQueueSize").asInt32();
1272  else
1273  _rxQueueSize=-1;
1274 
1275  Bottle &canAddresses = canGroup.findGroup("CanAddresses",
1276  "a list of numeric identifiers");
1277  if (canAddresses.isNull())
1278  {
1279  yError("CanAddresses group not found in config file\n");
1280  return false;
1281  }
1282  for (i = 1; i < canAddresses.size(); i++) {
1283  _destinations[i-1] = (unsigned char)(canAddresses.get(i).asInt32());
1284  }
1285 
1287  Bottle& general = p.findGroup("GENERAL");
1288 
1289  Bottle xtmp;
1290  if (!validate(general, xtmp, "AxisMap", "a list of reordered indices for the axes", nj+1))
1291  return false;
1292 
1293  for (i = 1; i < xtmp.size(); i++)
1294  _axisMap[i-1] = xtmp.get(i).asInt32();
1295 
1296  if (!validate(general, xtmp, "AxisName", "a list of strings representing the axes names", nj + 1))
1297  {
1298  //return false; //this parameter is not yet mandatory
1299  }
1300  //beware: axis name has to be remapped here because they are not set using the toHw() helper function
1301  for (i = 1; i < xtmp.size(); i++)
1302  _axisName[_axisMap[i-1]] = xtmp.get(i).asString();
1303 
1304  if (!validate(general, xtmp, "AxisType", "a list of strings representing the axes types (revolute/prismatic)", nj + 1))
1305  {
1306  //return false; //this parameter is not yet mandatory
1307  }
1308  //beware: axis name has to be remapped here because they are not set using the toHw() helper function
1309  for (i = 1; i < xtmp.size(); i++)
1310  _axisType[_axisMap[i - 1]] = xtmp.get(i).asString();
1311 
1312  if (!validate(general, xtmp, "Encoder", "a list of scales for the joint encoders", nj+1))
1313  return false;
1314 
1315  int test = xtmp.size();
1316  for (i = 1; i < xtmp.size(); i++)
1317  _angleToEncoder[i-1] = xtmp.get(i).asFloat64();
1318 
1319  if (!validate_optional(general, xtmp, "Rotor", "a list of scales for the rotor encoders", nj+1))
1320  {
1321  yWarning("Rotor: Using default value = 1\n");
1322  for(i=1;i<nj+1; i++)
1323  _rotToEncoder[i-1] = 1.0;
1324  }
1325  else
1326  {
1327  int test = xtmp.size();
1328  for (i = 1; i < xtmp.size(); i++)
1329  _rotToEncoder[i-1] = xtmp.get(i).asFloat64();
1330  }
1331 
1332  if (!validate(general, xtmp, "fullscalePWM", " list of scales for the fullscalePWM conversion factor", nj + 1))
1333  {
1334  yError("fullscalePWM param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
1335  return false;
1336  }
1337  for (i = 1; i < xtmp.size(); i++)
1338  _dutycycleToPwm[i - 1] = xtmp.get(i).asFloat64()/100.0;
1339 
1340  if (!validate(general, xtmp, "ampsToSensor", "a list of scales for the ampsToSensor conversion factor", nj + 1))
1341  {
1342  yError("ampsToSensor param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
1343  return false;
1344  }
1345  for (i = 1; i < xtmp.size(); i++)
1346  _ampsToSensor[i - 1] = xtmp.get(i).asFloat64();
1347 
1348  if (!validate(general, xtmp, "Zeros","a list of offsets for the zero point", nj+1))
1349  return false;
1350 
1351  for (i = 1; i < xtmp.size(); i++)
1352  _zeros[i-1] = xtmp.get(i).asFloat64();
1353 
1354  if (useRawJointEncoderData)
1355  {
1356  for(i=1;i<nj+1; i++)
1357  {
1358  if (_angleToEncoder[i-1] > 0)
1359  _angleToEncoder[i-1] = 1;
1360  else
1361  _angleToEncoder[i-1] = -1;
1362 
1363  _zeros[i-1] = 0;
1364  }
1365  }
1366 
1367  if (useRawMotorEncoderData)
1368  {
1369  for(i=1;i<nj+1; i++)
1370  {
1371  if (_rotToEncoder[i-1] > 0)
1372  _rotToEncoder[i-1] = 1;
1373  else
1374  _rotToEncoder[i-1] = -1;
1375  }
1376  }
1377 
1378  if (!validate_optional(general, xtmp, "TorqueId","a list of associated joint torque sensor ids", nj+1))
1379  {
1380  yWarning("TorqueId: Using default value = 0 (disabled)\n");
1381  for(i=1;i<nj+1; i++)
1382  _torqueSensorId[i-1] = 0;
1383  }
1384  else
1385  {
1386  for (i = 1; i < xtmp.size(); i++) _torqueSensorId[i-1] = xtmp.get(i).asInt32();
1387  }
1388 
1389  if (!validate_optional(general, xtmp, "TorqueChan","a list of associated joint torque sensor channels", nj+1))
1390  {
1391  yWarning("TorqueChan: Using default value = 0 (disabled)\n");
1392  for(i=1;i<nj+1; i++)
1393  _torqueSensorChan[i-1] = 0;
1394  }
1395  else
1396  {
1397  for (i = 1; i < xtmp.size(); i++) _torqueSensorChan[i-1] = xtmp.get(i).asInt32();
1398  }
1399 
1400  if (!validate(general, xtmp, "TorqueMax","full scale value for a joint torque sensor", nj+1))
1401  {
1402  return false;
1403  }
1404  else
1405  {
1406  for (i = 1; i < xtmp.size(); i++)
1407  {
1408  _maxTorque[i-1] = xtmp.get(i).asInt32();
1409  _newtonsToSensor[i-1] = double(0x8000)/double(_maxTorque[i-1]);
1410  }
1411  }
1412 
1413  int j=0;
1415  {
1416  Bottle posPidsGroup;
1417  posPidsGroup=p.findGroup("POS_PIDS", "Position Pid parameters new format");
1418  if (posPidsGroup.isNull() == false)
1419  {
1420  yInfo("Position Pids section found, new format\n");
1421  if (!parsePidsGroup_NewFormat(posPidsGroup, _pids))
1422  {
1423  yError() << "Position Pids section: error detected in parameters syntax";
1424  return false;
1425  }
1426  else
1427  {
1428  yInfo("Position Pids successfully loaded\n");
1429  }
1430  }
1431  else
1432  {
1433  Bottle posPidsGroup2=p.findGroup("PIDS", "Position Pid parameters old format");
1434  if (posPidsGroup2.isNull()==false)
1435  {
1436  yWarning("Position Pids section found, old format\n");
1437  parsePosPidsGroup_OldFormat (posPidsGroup2, nj, _pids);
1438  }
1439  else
1440  {
1441  yError () << "no PIDS group found in config file, returning";
1442  return false;
1443  }
1444  }
1445  }
1446 
1448  {
1449  Bottle trqPidsGroup;
1450  Bottle trqPidsOldGroup;
1451  Bottle trqControlGroup;
1452  trqPidsGroup = p.findGroup("TRQ_PIDS", "Torque Pid parameters new format");
1453  trqPidsOldGroup = p.findGroup("TORQUE_PIDS", "Torque Pid parameters old format");
1454  trqControlGroup = p.findGroup("TORQUE_CONTROL", "Torque Control parameters");
1455 
1456  if (trqPidsGroup.isNull()==false)
1457  {
1458  yInfo("TRQ_PIDS: Torque Pids section found, new format\n");
1459  if (!parsePidsGroup_NewFormat (trqPidsGroup, _tpids))
1460  {
1461  yError () << "TRQ_PIDS: error detected in parameters syntax";
1462  return false;
1463  }
1464  else
1465  {
1466  xtmp = trqPidsGroup.findGroup("kbemf");
1467  {for (j=0;j<nj;j++) this->_bemfGain[j] = 0; }
1468 
1469  xtmp = trqPidsGroup.findGroup("ktau");
1470  {for (j=0;j<nj;j++) this->_ktau[j] = 1; }
1471 
1472  xtmp = trqPidsGroup.findGroup("filterType");
1473  {for (j=0;j<nj;j++) this->_filterType[j] = 3; }
1474 
1475  xtmp = trqPidsGroup.findGroup("controlUnits");
1477 
1478  _torqueControlEnabled = true;
1479  }
1480  }
1481  else if (trqPidsOldGroup.isNull()==false)
1482  {
1483  yWarning ("TORQUE_PIDS: Torque Pids section found, old format\n");
1484  if (!parseTrqPidsGroup_OldFormat (trqPidsOldGroup, nj, _tpids))
1485  {
1486  yError () << "TORQUE_PIDS: error detected in parameters syntax";
1487  return false;
1488  }
1489  else
1490  {
1491  xtmp = trqPidsGroup.findGroup("kbemf");
1492  {for (j=0;j<nj;j++) this->_bemfGain[j] = 0; }
1493 
1494  xtmp = trqPidsGroup.findGroup("ktau");
1495  {for (j=0;j<nj;j++) this->_ktau[j] = 1; }
1496 
1497  xtmp = trqPidsGroup.findGroup("filterType");
1498  {for (j=0;j<nj;j++) this->_filterType[j] = 3; }
1499 
1500  xtmp = trqPidsGroup.findGroup("controlUnits");
1502 
1503  _torqueControlEnabled = true;
1504  }
1505  }
1506  else if (trqControlGroup.isNull()==false)
1507  {
1508  yInfo("TORQUE_CONTROL section found\n");
1509  if (!parsePidsGroup_NewFormat (trqControlGroup, _tpids))
1510  {
1511  yError () << "TORQUE_CONTROL: error detected in parameters syntax";
1512  return false;
1513  }
1514  else
1515  {
1516  xtmp = trqControlGroup.findGroup("kbemf");
1517  if (!xtmp.isNull())
1518  {for (j=0;j<nj;j++) this->_bemfGain[j] = xtmp.get(j+1).asFloat64();}
1519  else
1520  {for (j=0;j<nj;j++) this->_bemfGain[j] = 0; yWarning ("TORQUE_PIDS: 'kbemf' param missing");}
1521 
1522  xtmp = trqControlGroup.findGroup("ktau");
1523  if (!xtmp.isNull())
1524  {for (j=0;j<nj;j++) this->_ktau[j] = xtmp.get(j+1).asFloat64();}
1525  else
1526  {for (j=0;j<nj;j++) this->_ktau[j] = 1.0; yWarning ("TORQUE_PIDS: 'ktau' param missing");}
1527 
1528  xtmp = trqControlGroup.findGroup("filterType");
1529  if (!xtmp.isNull())
1530  {for (j=0;j<nj;j++) this->_filterType[j] = xtmp.get(j+1).asInt32();}
1531  else
1532  {for (j=0;j<nj;j++) this->_filterType[j] = 3; yWarning ("TORQUE_PIDS: 'filterType' param missing");}
1533 
1534  xtmp = trqControlGroup.findGroup("controlUnits");
1535  if (!xtmp.isNull())
1536  {
1537  if (xtmp.toString()==std::string("metric_units")) {this->_torqueControlUnits=METRIC_UNITS;}
1538  else if (xtmp.toString()==std::string("machine_units")) {this->_torqueControlUnits=MACHINE_UNITS;}
1539  else {yError() << "invalid controlUnits value"; return false;}
1540  }
1541  else
1542  {
1543  yError ("TORQUE_PIDS: 'controlUnits' param missing. Cannot continue");
1544  return false;
1545  }
1546 
1547  _torqueControlEnabled = true;
1548  }
1549  }
1550  else
1551  {
1552  _torqueControlEnabled=false;
1553  yWarning("Torque control parameters not found for part %s, skipping...\n", _networkName.c_str());
1554  }
1555  }
1556 
1558  {
1559  Bottle &debugGroup=p.findGroup("DEBUG_PARAMETERS","DEBUG parameters");
1560  if (debugGroup.isNull()==false)
1561  {
1562  yDebug("DEBUG_PARAMETERS section found\n");
1563  if (!parseDebugGroup_NewFormat (debugGroup, _debug_params))
1564  {
1565  yError("DEBUG_PARAMETERS section: error detected in parameters syntax\n");
1566  return false;
1567  }
1568  else
1569  {
1570  yInfo("DEBUG_PARAMETERS section: parameters successfully loaded\n");
1571  }
1572  }
1573  else
1574  {
1575  yDebug("DEBUG parameters section NOT found, skipping...\n");
1576  }
1577  }
1578 
1580  {
1581  Bottle &impedanceGroup=p.findGroup("IMPEDANCE","IMPEDANCE parameters");
1582  if (impedanceGroup.isNull()==false)
1583  {
1584  yDebug("IMPEDANCE parameters section found\n");
1585  if (!parseImpedanceGroup_NewFormat (impedanceGroup, _impedance_params))
1586  {
1587  yError("IMPEDANCE section: error detected in parameters syntax\n");
1588  return false;
1589  }
1590  else
1591  {
1592  yInfo("IMPEDANCE section: parameters successfully loaded\n");
1593  }
1594  }
1595  else
1596  {
1597  yDebug("IMPEDANCE parameters section NOT found, skipping...\n");
1598  }
1599  }
1600 
1602  for(j=0;j<nj;j++)
1603  {
1604  _impedance_limits[j].min_damp= 0.001;
1605  _impedance_limits[j].max_damp= 9.888;
1606  _impedance_limits[j].min_stiff= 0.002;
1607  _impedance_limits[j].max_stiff= 9.889;
1608  _impedance_limits[j].param_a= 0.011;
1609  _impedance_limits[j].param_b= 0.012;
1610  _impedance_limits[j].param_c= 0.013;
1611  }
1612 
1614  Bottle &limits=p.findGroup("LIMITS");
1615  if (limits.isNull())
1616  {
1617  yError("Group LIMITS not found in configuration file\n");
1618  return false;
1619  }
1620 
1621  // motor oveload current
1622  if (!validate(limits, xtmp, "motorOverloadCurrents","a list of current limits", nj+1))
1623  return false;
1624  for(i=1;i<xtmp.size(); i++) _currentLimits[i-1]=xtmp.get(i).asFloat64();
1625 
1626  // Motor pwm limit
1627  if (!validate_optional(limits, xtmp, "motorPwmLimit", "a list of motor PWM limits", nj + 1))
1628  {
1629  yWarning("motorPwmLimit: Using default motorPwmLimit=1333\n");
1630  for (i = 1; i<nj + 1; i++)
1631  _motorPwmLimits[i - 1] = 1333; //Default value
1632  }
1633  else
1634  {
1635  for (i = 1; i < xtmp.size(); i++)
1636  {
1637  _motorPwmLimits[i - 1] = xtmp.get(i).asFloat64();
1638  if (_motorPwmLimits[i - 1] < 0)
1639  {
1640  yError() << "motorPwmLimit should be a positive value";
1641  return false;
1642  }
1643  }
1644  }
1645 
1646  // Max Joint velocity
1647  if (!validate_optional(limits, xtmp, "jntVelMax", "the maximum velocity command of a joint", nj + 1))
1648  {
1649  yWarning("jntVelMax: Using default jntVelMax=100 deg/s\n");
1650  for (i = 1; i<nj + 1; i++)
1651  _maxJntCmdVelocity[i - 1] = 100; //Default value
1652  }
1653  else
1654  {
1655  for (i = 1; i<xtmp.size(); i++)
1656  {
1657  _maxJntCmdVelocity[i - 1] = xtmp.get(i).asFloat64();
1658  if (_maxJntCmdVelocity[i - 1]<0)
1659  {
1660  yError() << "Invalid jntVelMax parameter <0\n";
1661  return false;
1662  }
1663  }
1664  }
1665 
1666  if (!validate_optional(limits, xtmp, "maxPosStep", "the maximum amplitude of a position direct step", nj+1))
1667  {
1668  yWarning("maxPosStep: Using default MaxPosStep=10 degs\n");
1669  for(i=1;i<nj+1; i++)
1670  _maxStep[i-1] = 10; //Default value
1671  }
1672  else
1673  {
1674  for(i=1;i<xtmp.size(); i++)
1675  {
1676  _maxStep[i-1]=xtmp.get(i).asFloat64();
1677  if (_maxStep[i-1]<0)
1678  {
1679  yError () << "Invalid MaxPosStep parameter <0\n";
1680  return false;
1681  }
1682  }
1683  }
1684 
1685  // max joint position
1686  if (!validate(limits, xtmp, "jntPosMax","a list of maximum angles (in degrees)", nj+1))
1687  return false;
1688  for(i=1;i<xtmp.size(); i++) _limitsMax[i-1]=xtmp.get(i).asFloat64();
1689 
1690  // min joint position
1691  if (!validate(limits, xtmp, "jntPosMin","a list of minimum angles (in degrees)", nj+1))
1692  return false;
1693  for(i=1;i<xtmp.size(); i++) _limitsMin[i-1]=xtmp.get(i).asFloat64();
1694  for (i = 0; i<nj; i++)
1695  {
1696  if (_limitsMax[i] < _limitsMin[i])
1697  {
1698  yError("invalid limit on joint %d : Max value (%f) < Min value(%f)\n", i, _limitsMax[i], _limitsMin[i]);
1699  return false;
1700  }
1701  }
1702 
1704  Bottle &velocityGroup=p.findGroup("VELOCITY");
1705  if (!velocityGroup.isNull())
1706  {
1708  if (!validate(velocityGroup, xtmp, "Shifts", "a list of shifts to be used in the vmo control", nj+1))
1709  {
1710  yWarning("Shifts: Using default Shifts=4\n");
1711  for(i=1;i<nj+1; i++)
1712  _velocityShifts[i-1] = 4; //Default value
1713  }
1714  else
1715  {
1716  for(i=1;i<xtmp.size(); i++)
1717  _velocityShifts[i-1]=xtmp.get(i).asInt32();
1718  }
1719 
1721  xtmp.clear();
1722  if (!validate(velocityGroup, xtmp, "Timeout", "a list of timeout to be used in the vmo control", nj+1))
1723  {
1724  yWarning("Timeout: Using default Timeout=1000, i.e 1s\n");
1725  for(i=1;i<nj+1; i++)
1726  _velocityTimeout[i-1] = 1000; //Default value
1727  }
1728  else
1729  {
1730  for(i=1;i<xtmp.size(); i++)
1731  _velocityTimeout[i-1]=xtmp.get(i).asInt32();
1732  }
1733 
1735  xtmp.clear();
1736  if (!validate_optional(velocityGroup, xtmp, "JNT_speed_estimation", "a list of shift factors used by the firmware joint speed estimator", nj+1))
1737  {
1738  yWarning("JNT_speed_estimation: Using default value=5\n");
1739  for(i=1;i<nj+1; i++)
1740  _estim_params[i-1].jnt_Vel_estimator_shift = 5; //Default value
1741  }
1742  else
1743  {
1744  for(i=1;i<xtmp.size(); i++)
1745  _estim_params[i-1].jnt_Vel_estimator_shift = xtmp.get(i).asInt32();
1746  }
1747 
1749  xtmp.clear();
1750  if (!validate_optional(velocityGroup, xtmp, "MOT_speed_estimation", "a list of shift factors used by the firmware motor speed estimator", nj+1))
1751  {
1752  yWarning("MOT_speed_estimation: Using default value=5\n");
1753  for(i=1;i<nj+1; i++)
1754  _estim_params[i-1].mot_Vel_estimator_shift = 5; //Default value
1755  }
1756  else
1757  {
1758  for(i=1;i<xtmp.size(); i++)
1759  _estim_params[i-1].mot_Vel_estimator_shift = xtmp.get(i).asInt32();
1760  }
1761 
1763  xtmp.clear();
1764  if (!validate_optional(velocityGroup, xtmp, "JNT_accel_estimation", "a list of shift factors used by the firmware joint speed estimator", nj+1))
1765  {
1766  yWarning("JNT_accel_estimation: Using default value=5\n");
1767  for(i=1;i<nj+1; i++)
1768  _estim_params[i-1].jnt_Acc_estimator_shift = 5; //Default value
1769  }
1770  else
1771  {
1772  for(i=1;i<xtmp.size(); i++)
1773  _estim_params[i-1].jnt_Acc_estimator_shift = xtmp.get(i).asInt32();
1774  }
1775 
1777  xtmp.clear();
1778  if (!validate_optional(velocityGroup, xtmp, "MOT_accel_estimation", "a list of shift factors used by the firmware motor speed estimator", nj+1))
1779  {
1780  yWarning("MOT_accel_estimation: Using default value=5\n");
1781  for(i=1;i<nj+1; i++)
1782  _estim_params[i-1].mot_Acc_estimator_shift = 5; //Default value
1783  }
1784  else
1785  {
1786  for(i=1;i<xtmp.size(); i++)
1787  _estim_params[i-1].mot_Acc_estimator_shift = xtmp.get(i).asInt32();
1788  }
1789 
1790  }
1791  else
1792  {
1793  yWarning("A suitable value for [VELOCITY] Shifts was not found. Using default Shifts=4\n");
1794  for(i=1;i<nj+1; i++)
1795  _velocityShifts[i-1] = 4; //Default value
1796 
1797  yWarning("A suitable value for [VELOCITY] Timeout was not found. Using default Timeout=1000, i.e 1s.\n");
1798  for(i=1;i<nj+1; i++)
1799  _velocityTimeout[i-1] = 1000; //Default value
1800 
1801  yWarning("A suitable value for [VELOCITY] speed estimation was not found. Using default shift factor=5.\n");
1802  for(i=1;i<nj+1; i++)
1803  {
1804  _estim_params[i-1].jnt_Vel_estimator_shift = 5; //Default value
1808  }
1809  }
1810 
1811  bool ret=true;
1812  if (!canGroup.findGroup("broadcast_pos").isNull())
1813  {
1814  xtmp=canGroup.findGroup("broadcast_pos");
1815  ret=ret&&setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__POSITION);
1816  }
1817 
1818  if (!canGroup.findGroup("broadcast_pid").isNull())
1819  {
1820  xtmp=canGroup.findGroup("broadcast_pid");
1821  ret=ret&&setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__PID_VAL);
1822  }
1823  if (!canGroup.findGroup("broadcast_fault").isNull())
1824  {
1825  xtmp=canGroup.findGroup("broadcast_fault");
1826  ret=ret&&setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__STATUS);
1827  }
1828  if (!canGroup.findGroup("broadcast_current").isNull())
1829  {
1830  xtmp=canGroup.findGroup("broadcast_current");
1831  ret=ret&&setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__CURRENT);
1832  }
1833  if (!canGroup.findGroup("broadcast_overflow").isNull())
1834  {
1835  xtmp=canGroup.findGroup("broadcast_overflow");
1836  ret=ret&&setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__OVERFLOW);
1837  }
1838  if (!canGroup.findGroup("broadcast_canprint").isNull())
1839  {
1840  xtmp=canGroup.findGroup("broadcast_canprint");
1841  ret=ret&&setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__PRINT);
1842  }
1843  if (!canGroup.findGroup("broadcast_vel_acc").isNull())
1844  {
1845  xtmp=canGroup.findGroup("broadcast_vel_acc");
1846  ret=ret&&setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__VELOCITY);
1847  }
1848 
1849  if (!canGroup.findGroup("broadcast_pid_err").isNull())
1850  {
1851  xtmp=canGroup.findGroup("broadcast_pid_err");
1852  setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__PID_ERROR); //@@@ not checking return value in order to keep this option not mandatory
1853  }
1854 
1855  if (!canGroup.findGroup("broadcast_rotor_pos").isNull())
1856  {
1857  xtmp=canGroup.findGroup("broadcast_rotor_pos");
1858  setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__MOTOR_POSITION);
1859  }
1860 
1861  if (!canGroup.findGroup("broadcast_rotor_vel_acc").isNull())
1862  {
1863  xtmp=canGroup.findGroup("broadcast_rotor_vel_acc");
1864  setBroadCastMask(xtmp, ICUBCANPROTO_PER_MC_MSG__MOTOR_SPEED);
1865  }
1866 
1867  if (!ret)
1868  yError("Invalid configuration file, check broadcast_* parameters\n");
1869 
1870  return ret;
1871 }
1872 
1874 {
1875  _networkN=0;
1876  _destinations=0;
1877  _axisMap=0;
1878  _angleToEncoder=0;
1879  _rotToEncoder=0;
1880  _zeros=0;
1881  _pids=0;
1882  _tpids=0;
1883  _torqueControlEnabled=false;
1884  _pwmIsLimited=false;
1885  _limitsMax=0;
1886  _limitsMin=0;
1887  _currentLimits=0;
1888  _velocityShifts=0;
1889  _optical_factor=0;
1890  _velocityTimeout=0;
1891  _torqueSensorId=0;
1893  _maxTorque=0;
1894  _newtonsToSensor=0;
1895  _ampsToSensor = 0;
1896  _dutycycleToPwm = 0;
1897  _debug_params=0;
1900  _estim_params=0;
1901  _bemfGain=0;
1902  _ktau=0;
1903  _axisName = 0;
1904  _axisType = 0;
1905  _motorPwmLimits = 0;
1906  _maxJntCmdVelocity = 0;
1907 
1908  _my_address = 0;
1909  _polling_interval = 10;
1910  _timeout = 20;
1911  _njoints = 0;
1912 
1913  _txQueueSize = 2047;
1914  _rxQueueSize = 2047;
1915  _txTimeout = 20;
1916  _rxTimeout = 20;
1917  _broadcast_mask=0;
1919 }
1920 
1922 {
1923  _networkN = 0;
1924  _destinations = allocAndCheck<unsigned char> (CAN_MAX_CARDS);
1925  _axisMap = allocAndCheck<int>(nj);
1926  _angleToEncoder = allocAndCheck<double>(nj);
1927  _rotToEncoder = allocAndCheck<double>(nj);
1928  _zeros = allocAndCheck<double>(nj);
1929  _torqueSensorId= allocAndCheck<int>(nj);
1930  _torqueSensorChan= allocAndCheck<int>(nj);
1931  _maxTorque=allocAndCheck<double>(nj);
1932  _newtonsToSensor=allocAndCheck<double>(nj);
1933  _ampsToSensor = allocAndCheck<double>(nj);
1934  _dutycycleToPwm = allocAndCheck<double>(nj);
1935  _bemfGain=allocAndCheck<double>(nj);
1936  _ktau=allocAndCheck<double>(nj);
1937  _maxStep=allocAndCheck<double>(nj);
1938  _maxJntCmdVelocity = allocAndCheck<double>(nj);
1939  _motorPwmLimits = allocAndCheck<double>(nj);
1940  _filterType=allocAndCheck<int>(nj);
1941  _axisName = new std::string[nj];
1942  _axisType = new std::string[nj];
1943 
1944  _pids=allocAndCheck<Pid>(nj);
1945  _tpids=allocAndCheck<Pid>(nj);
1946  _debug_params=allocAndCheck<DebugParameters>(nj);
1947  _impedance_params=allocAndCheck<ImpedanceParameters>(nj);
1948  _impedance_limits=allocAndCheck<ImpedanceLimits>(nj);
1949  _estim_params=allocAndCheck<SpeedEstimationParameters>(nj);
1950 
1951  _limitsMax=allocAndCheck<double>(nj);
1952  _limitsMin=allocAndCheck<double>(nj);
1953  _currentLimits=allocAndCheck<double>(nj);
1954  _optical_factor=allocAndCheck<double>(nj);
1955  _velocityShifts=allocAndCheck<int>(CAN_MAX_CARDS);
1956  _velocityTimeout=allocAndCheck<int>(CAN_MAX_CARDS);
1957  memset(_limitsMin, 0, sizeof(double)*nj);
1958  memset(_limitsMax, 0, sizeof(double)*nj);
1959  memset(_currentLimits, 0, sizeof(double)*nj);
1960  memset(_velocityShifts, 0, sizeof(int)*nj);
1961  memset(_optical_factor, 0, sizeof(double)*nj);
1962  memset(_velocityTimeout, 0, sizeof(int)*nj);
1963  memset(_maxStep, 0, sizeof(double)*nj);
1964  memset(_maxJntCmdVelocity, 0, sizeof(double)*nj);
1965  memset(_motorPwmLimits, 0, sizeof(double)*nj);
1966 
1967  _my_address = 0;
1968  _polling_interval = 10;
1969  _timeout = 20;
1970  _njoints = nj;
1971 
1972  _txQueueSize = 2047;
1973  _rxQueueSize = 2047;
1974  _txTimeout = 20;
1975  _rxTimeout = 20;
1976 
1977  _broadcast_mask=allocAndCheck<int>(nj);
1978  for(int j=0;j<nj;j++)
1979  {
1981  }
1982 
1983  return true;
1984 }
1985 
1987 {
1988  checkAndDestroy<double>(_zeros);
1989  checkAndDestroy<double>(_angleToEncoder);
1990  checkAndDestroy<double>(_rotToEncoder);
1991  checkAndDestroy<int>(_axisMap);
1992  checkAndDestroy<unsigned char>(_destinations);
1993  checkAndDestroy<int>(_velocityShifts);
1994  checkAndDestroy<double>(_optical_factor);
1995  checkAndDestroy<int>(_velocityTimeout);
1996  checkAndDestroy<int>(_torqueSensorId);
1997  checkAndDestroy<int>(_torqueSensorChan);
1998  checkAndDestroy<double>(_maxTorque);
1999  checkAndDestroy<double>(_newtonsToSensor);
2000  checkAndDestroy<double>(_ampsToSensor);
2001  checkAndDestroy<double>(_dutycycleToPwm);
2002  checkAndDestroy<double>(_bemfGain);
2003  checkAndDestroy<double>(_ktau);
2004  checkAndDestroy<double>(_maxStep);
2005  checkAndDestroy<double>(_maxJntCmdVelocity);
2006  checkAndDestroy<double>(_motorPwmLimits);
2007  checkAndDestroy<int>(_filterType);
2008  checkAndDestroy<std::string>(_axisName);
2009  checkAndDestroy<std::string>(_axisType);
2010 
2011  checkAndDestroy<Pid>(_pids);
2012  checkAndDestroy<Pid>(_tpids);
2013  checkAndDestroy<SpeedEstimationParameters>(_estim_params);
2014  checkAndDestroy<DebugParameters>(_debug_params);
2015  checkAndDestroy<ImpedanceParameters>(_impedance_params);
2016  checkAndDestroy<ImpedanceLimits>(_impedance_limits);
2017  checkAndDestroy<double>(_limitsMax);
2018  checkAndDestroy<double>(_limitsMin);
2019  checkAndDestroy<double>(_currentLimits);
2020  checkAndDestroy<int>(_broadcast_mask);
2021 }
2022 
2029 
2031 {
2032  iCanBus=0;
2033  iBufferFactory=0;
2034  iCanErrors=0;
2035 
2036  _timeout = CAN_TIMEOUT;
2037  _polling_interval = CAN_POLLING_INTERVAL;
2038  _speed = 0;
2039  _networkN = 0;
2040  _destInv=0;
2041  _jointNames = 0;
2042  _initialized=false;
2043 
2044  memset (_destinations, 0, sizeof(unsigned char) * CAN_MAX_CARDS);
2045  memset (_velShifts, 0, sizeof(int) * CAN_MAX_CARDS);
2046 
2047  _my_address = 0;
2048  _njoints = 0;
2049 
2050  _readMessages = 0;
2051  _writeMessages = 0;
2052  _echoMessages = 0;
2053  _bcastRecvBuffer = NULL;
2054 
2055  _error_status = true;
2056  requestsQueue=0;
2057 }
2058 
2060 {
2061  uninitialize();
2062 }
2063 
2064 bool CanBusResources::initialize (yarp::os::Searchable &config)
2065 {
2067  bool ret;
2068 
2069  if (!params.fromConfig(config))
2070  return false;
2071 
2072  polyDriver.open(config);
2073  if (!polyDriver.isValid())
2074  {
2075  yError("Could not instantiate CAN device\n");
2076  return false;
2077  }
2078 
2079  polyDriver.view(iCanBus);
2080  polyDriver.view(iBufferFactory);
2081  polyDriver.view(iCanErrors);
2082 
2083  if ((iCanBus==0) || (iBufferFactory==0))
2084  {
2085  yError("CanBusResources::initialize() could not get ICanBus or ICanBufferFactory interface");
2086  return false;
2087  }
2088 
2089  ret=initialize(params);
2090  return ret;
2091 }
2092 
2094 {
2095  yTrace("Calling CanBusResources::initialize\n");
2096  if (_initialized)
2097  return false;
2098 
2099  // general variable init.
2100  _speed = 0;
2101  _networkN = parms._networkN;
2102 
2103  _readMessages = 0;
2104  _writeMessages = 0;
2105  _error_status = true;
2106 
2107  memcpy (_destinations, parms._destinations, sizeof(unsigned char)*CAN_MAX_CARDS);
2108  memcpy (_velShifts, parms._velocityShifts, sizeof(int)*CAN_MAX_CARDS);
2109 
2110  for (int k = 0; k < CAN_MAX_CARDS; k++)
2111  _velShifts[k] = (1 << ((int) _velShifts[k]));
2112 
2113  _destInv=allocAndCheck<unsigned char>(CAN_MAX_CARDS);
2114 
2115  _my_address = parms._my_address;
2116  _polling_interval = parms._polling_interval;
2117  _timeout = parms._timeout;
2118  _njoints = parms._njoints;
2119 
2120  _jointNames = new std::string[_njoints];
2121  for (int i = 0; i < _njoints; i++) _jointNames[i] = parms._axisName[i];
2122 
2123  _txQueueSize=parms._txQueueSize;
2124  _rxQueueSize=parms._rxQueueSize;
2125  _filter = -1;
2126 
2127  for(int id=0;id<CAN_MAX_CARDS;id++)
2128  _destInv[id]=-1;
2129 
2130  //now fill inverted map
2131  printf("printing destinations and inverted map\n");
2132  for(int jj=0;jj<_njoints;jj+=2)
2133  {
2134  _destInv[_destinations[jj/2]]=jj;
2135  printf("%d %d\n",jj,_destinations[jj/2]);
2136  }
2137 
2138  _bcastRecvBuffer = allocAndCheck<BCastBufferElement> (_njoints);
2139  for (int j=0; j<_njoints ;j++)
2140  {
2141  _bcastRecvBuffer[j]._update_e=Time::now();
2142  _bcastRecvBuffer[j]._position_joint.resetStats();
2143  _bcastRecvBuffer[j]._position_rotor.resetStats();
2144  _bcastRecvBuffer[j]._speed_rotor.resetStats();
2145  _bcastRecvBuffer[j]._accel_rotor.resetStats();
2146  }
2147 
2148  //previously initialized
2149  iCanBus->canSetBaudRate(_speed);
2150  unsigned int i=0;
2151 
2152 #if ICUB_CANMASKS_STRICT_FILTER
2153  yInfo("using ICUB_CANMASKS_STRICT_FILTER option\n");
2154  // sets all message ID's for class 0
2155  for (int j=0; j<CAN_MAX_CARDS; j++)
2156  {
2157  unsigned int sent_addr = 0x000 + ( 0x000<<4) + _destinations[j];
2158  unsigned int recv_addr = 0x000 + (_destinations[j]<<4) + 0x000;
2159  iCanBus->canIdAdd(sent_addr);
2160  iCanBus->canIdAdd(recv_addr);
2161  }
2162  yDebug("class 0 set\n");
2163 
2164  // set all message ID's for class 1
2165  for (int j=0; j<CAN_MAX_CARDS; j++)
2166  {
2167  unsigned int start_addr = 0x100 + (_destinations[j]<<4) + 0x000;
2168  unsigned int end_addr = 0x100 + (_destinations[j]<<4) + 0x00F;
2169  for (i = start_addr; i < end_addr; i++)
2170  iCanBus->canIdAdd(i);
2171  }
2172  yDebug("class 1 set\n");
2173 
2174 #else
2175  yInfo("opening all CAN masks\n");
2176  // sets all message ID's for class 0
2177  for (i = 0x000; i < 0x0ff; i++)
2178  iCanBus->canIdAdd(i);
2179  yDebug("class 0 set\n");
2180 
2181  // set all message ID's for class 1
2182  for (i = 0x100; i < 0x1ff; i++)
2183  iCanBus->canIdAdd(i);
2184  yDebug("class 1 set\n");
2185 #endif
2186 
2187  // set all message ID's for class 2
2188  for (i = 0x200; i < 0x2ff; i++)
2189  iCanBus->canIdAdd(i);
2190  yDebug("class 2 set\n");
2191 
2192  // set all message ID's for class 3
2193  for (i = 0x300; i < 0x3ff; i++)
2194  iCanBus->canIdAdd(i);
2195  yDebug("class 3 set\n");
2196 
2197  _readBuffer=iBufferFactory->createBuffer(BUF_SIZE);
2198  _writeBuffer=iBufferFactory->createBuffer(BUF_SIZE);
2199  _replyBuffer=iBufferFactory->createBuffer(BUF_SIZE);
2200  _echoBuffer=iBufferFactory->createBuffer(BUF_SIZE);
2201  yDebug("Can read/write buffers created, buffer size: %d\n", BUF_SIZE);
2202 
2203  requestsQueue = new RequestsQueue(_njoints, ICUBCANPROTO_POL_MC_CMD_MAXNUM);
2204 
2205  _initialized=true;
2206 
2207  yInfo("CanBusResources::initialized correctly\n");
2208  return true;
2209 }
2210 
2211 
2213 {
2214  // remember uninitialize might be called more than once
2215  // need to check for already destroyed pointers.
2216  if (!_initialized)
2217  return true;
2218 
2219  //yTrace("CanBusResources::uninitialize\n");
2220  checkAndDestroy<BCastBufferElement> (_bcastRecvBuffer);
2221 
2222  if (_initialized)
2223  {
2224  iBufferFactory->destroyBuffer(_readBuffer);
2225  iBufferFactory->destroyBuffer(_writeBuffer);
2226  iBufferFactory->destroyBuffer(_replyBuffer);
2227  iBufferFactory->destroyBuffer(_echoBuffer);
2228  _initialized=false;
2229  }
2230 
2231  if (requestsQueue!=0)
2232  {
2233  delete requestsQueue;
2234  requestsQueue=0;
2235  }
2236 
2237  if (_destInv!=0)
2238  {
2239  delete [] _destInv;
2240  _destInv=0;
2241  }
2242 
2243  if (_jointNames != 0)
2244  {
2245  delete[] _jointNames;
2246  _jointNames = 0;
2247  }
2248 
2249  _initialized=false;
2250  return true;
2251 }
2252 
2253 
2255 {
2256  unsigned int messages=BUF_SIZE;
2257  bool res=false;
2258 
2259  _readMessages=0;
2260  res=iCanBus->canRead(_readBuffer, messages, &_readMessages);
2261  return res;
2262 }
2263 
2265 {
2266  _writeMessages = 0;
2267  return true;
2268 }
2269 
2270 bool CanBusResources::addMessage (int msg_id, int joint)
2271 {
2272  unsigned int id = _destinations[joint/2] & 0x0f;
2273  unsigned char data;
2274  data=msg_id;
2275  if ((joint % 2) == 1)
2276  data |= 0x80;
2277 
2278  _writeBuffer[_writeMessages].setId(id);
2279  _writeBuffer[_writeMessages].setLen(1);
2280  _writeBuffer[_writeMessages].getData()[0]=data;
2281  _writeMessages ++;
2282 
2283  return true;
2284 }
2285 
2286 bool CanBusResources::addMessage (int id, int joint, int msg_id)
2287 {
2288  unsigned char *data=_writeBuffer[_writeMessages].getData();
2289  unsigned int destId= _destinations[joint/2] & 0x0f;
2290 
2291  data[0] = msg_id;
2292  if ((joint % 2) == 1)
2293  data[0] |= 0x80;
2294 
2295  _writeBuffer[_writeMessages].setId(destId);
2296  _writeBuffer[_writeMessages].setLen(1);
2297  _writeMessages ++;
2298 
2299  CanRequest rq;
2300  rq.threadId=id;
2301  rq.joint=joint;
2302  rq.msg=msg_id;
2303  requestsQueue->append(rq);
2304 
2305  return true;
2306 }
2307 
2309 {
2310  if (_writeMessages < 1)
2311  return false;
2312 
2313  unsigned int sent=0;
2314 
2315  DEBUG_FUNC("Sending buffer:\n");
2316  for(unsigned int k=0; k<_writeMessages;k++)
2317  {
2318  PRINT_CAN_MESSAGE("El:", _writeBuffer[k]);
2319  }
2320 
2321  bool res;
2322  res=iCanBus->canWrite(_writeBuffer, _writeMessages, &sent);
2323  if (!res)
2324  {
2325  return false;
2326  }
2327 
2328  return true;
2329 }
2330 
2331 bool CanBusResources::printMessage (const CanMessage& m)
2332 {
2333  unsigned int id;
2334  unsigned int len;
2335  const unsigned char *data=m.getData();
2336 
2337  id=m.getId();
2338  len=m.getLen();
2339 
2340 
2341  int ret = sprintf (_printBuffer, "class: %2d s: %2x d: %2x c: %1d msg: %3d (%x) ",
2342  (id & 0x700) >> 8,
2343  (id & 0xf0) >> 4,
2344  (id & 0x0f),
2345  ((data[0] & 0x80)==0)?0:1,
2346  (data[0] & 0x7f),
2347  (data[0] & 0x7f));
2348 
2349  if (len > 1)
2350  {
2351  ret += sprintf (_printBuffer+ret, "x: ");
2352  }
2353 
2354  unsigned int j;
2355  for (j = 1; j < len; j++)
2356  {
2357  ret += sprintf (_printBuffer+ret, "%x ", data[j]);
2358  }
2359 
2360  printf("%s", _printBuffer);
2361 
2362  return true;
2363 }
2364 
2366 {
2367  unsigned int j;
2368 
2370  yDebug("CAN: write buffer\n");
2371  for (j = 0; j < _writeMessages; j++)
2372  printMessage (_writeBuffer[j]);
2373 
2374  yDebug("CAN: reply buffer\n");
2375  for (j = 0; j < _writeMessages; j++)
2376  printMessage (_replyBuffer[j]);
2377 
2378  yDebug("CAN: echo buffer\n");
2379  for (j = 0; j < _echoMessages; j++)
2380  printMessage (_echoBuffer[j]);
2381 
2382  yDebug("CAN: read buffer\n");
2383  for (j = 0; j < _readMessages; j++)
2384  printMessage (_readBuffer[j]);
2385  yDebug("CAN: -------------\n");
2386 
2387  return true;
2388 }
2389 
2390 
2392 PeriodicThread(0.01),
2393 ImplementPositionControl(this),
2394 ImplementVelocityControl(this),
2395 ImplementPidControl(this),
2396 ImplementEncodersTimed(this),
2397 ImplementControlCalibration(this),
2398 ImplementAmplifierControl(this),
2399 ImplementControlLimits(this),
2400 ImplementTorqueControl(this),
2401 ImplementImpedanceControl(this),
2402 ImplementControlMode(this),
2403 ImplementPositionDirect(this),
2404 ImplementInteractionMode(this),
2405 ImplementMotorEncoders(this),
2406 ImplementMotor(this),
2407 ImplementRemoteVariables(this),
2408 ImplementAxisInfo(this),
2409 ImplementPWMControl(this),
2410 ImplementCurrentControl(this)
2411 {
2412  system_resources = (void *) new CanBusResources;
2413  ACE_ASSERT (system_resources != NULL);
2414  _opened = false;
2415  _axisTorqueHelper = 0;
2418  _MCtorqueControlEnabled = false;
2420  _ref_positions = 0;
2421  _max_vel_jnt_cmd = 0;
2422  _ref_command_speeds = 0;
2423  _ref_speeds = 0;
2424  _ref_accs=0;
2425  _ref_torques=0;
2427  mServerLogger = NULL;
2428 }
2429 
2430 
2432 {
2433  checkAndDestroy<double>(_ref_command_positions);
2434  checkAndDestroy<double>(_ref_command_speeds);
2435  checkAndDestroy<double>(_ref_positions);
2436  checkAndDestroy<double>(_max_vel_jnt_cmd);
2437  checkAndDestroy<double>(_ref_speeds);
2438  checkAndDestroy<double>(_ref_accs);
2439  checkAndDestroy<double>(_ref_torques);
2440  checkAndDestroy<double>(_last_position_move_time);
2441 
2442  if (system_resources != NULL)
2444  system_resources = NULL;
2445 }
2446 
2447 bool CanBusMotionControl::open (Searchable &config)
2448 {
2449  yTrace("Opening CanBusMotionControl Control\n");
2450  std::string dbg_string = config.toString().c_str();
2451 
2454  bool ret=false;
2455  _mutex.lock();
2456 
2457  if(!p.fromConfig(config))
2458  {
2459  _mutex.unlock();
2460  yError() << "One or more errors found while parsing device configuration";
2461  return false;
2462  }
2463 
2464  std::string str=config.toString().c_str();
2465  Property prop;
2466  prop.fromString(str.c_str());
2467  networkName=config.find("NetworkId").asString();
2468  canDevName=config.find("canbusdevice").asString(); //for backward compatibility
2469  if (canDevName=="") canDevName=config.findGroup("CAN").find("canbusdevice").asString();
2470  if(canDevName=="") { yError() << "cannot find parameter 'canbusdevice'\n"; return false;}
2471  prop.unput("device");
2472  prop.unput("subdevice");
2473  prop.put("device", canDevName.c_str());
2474  std::string canPhysDevName = config.find("physDevice").asString(); //for backward compatibility
2475  if (canPhysDevName=="") canPhysDevName = config.findGroup("CAN").find("physDevice").asString();
2476  prop.put("physDevice",canPhysDevName.c_str());
2477  prop.put("canDeviceNum", p._networkN);
2478  prop.put("canTxTimeout", p._txTimeout);
2479  prop.put("canRxTimeout", p._rxTimeout);
2480  if (p._txQueueSize!=-1)
2481  prop.put("canTxQueueSize", p._txQueueSize);
2482  if (p._rxQueueSize!=-1)
2483  prop.put("canRxQueueSize", p._rxQueueSize);
2484 
2485  ret=res.initialize(prop);
2486 
2487  if (!ret)
2488  {
2489  _mutex.unlock();
2490  yError() << "Unable to uninitialize CAN driver";
2491  return false;
2492  }
2493 
2494  // used for printing debug messages.
2495  _filter = -1;
2496  _writerequested = false;
2497  _noreply = false;
2498 
2499  double *tmpZeros = new double [p._njoints];
2500  double *tmpOnes = new double [p._njoints];
2501  for (int i=0; i< p._njoints; i++)
2502  {
2503  tmpZeros[i]=0.0;
2504  tmpOnes[i]=1.0;
2505  }
2506 
2507  ImplementPositionControl::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2508  ImplementVelocityControl::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2509 
2510 // ImplementPositionControl<CanBusMotionControl, IPositionControl>::
2511 // initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2512 //
2513 // ImplementVelocityControl<CanBusMotionControl, IVelocityControl>::
2514 // initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2515 
2516  ImplementPidControl::initialize(p._njoints, p._axisMap, p._angleToEncoder, NULL, p._newtonsToSensor, p._ampsToSensor, p._dutycycleToPwm);
2517 
2518  ImplementEncodersTimed::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2519 
2520  ImplementMotorEncoders::initialize(p._njoints, p._axisMap, p._rotToEncoder, tmpZeros);
2521 
2522  ImplementMotor::initialize(p._njoints, p._axisMap);
2523 
2524  ImplementControlCalibration::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2525 
2526  ImplementAmplifierControl::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2527 
2528  ImplementControlLimits::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2529 
2530  ImplementControlMode::initialize(p._njoints, p._axisMap);
2531  ImplementTorqueControl::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros, p._newtonsToSensor, p._ampsToSensor, nullptr,nullptr,nullptr);
2532  _axisTorqueHelper = new axisTorqueHelper(p._njoints,p._torqueSensorId,p._torqueSensorChan, p._maxTorque, p._newtonsToSensor);
2533 
2534  if (p._torqueControlUnits==CanBusMotionControlParameters::MACHINE_UNITS) {}
2535  else if (p._torqueControlUnits==CanBusMotionControlParameters::METRIC_UNITS) {}
2536  else {yError() << "Invalid _torqueControlUnits value: %d" << p._torqueControlUnits; return false;}
2537 
2538  _axisImpedanceHelper = new axisImpedanceHelper(p._njoints, p._impedance_limits);
2539  ImplementImpedanceControl::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros, p._newtonsToSensor);
2540  ImplementPositionDirect::initialize(p._njoints, p._axisMap, p._angleToEncoder, p._zeros);
2541  ImplementInteractionMode::initialize(p._njoints, p._axisMap);
2542  _axisPositionDirectHelper = new axisPositionDirectHelper(p._njoints, p._axisMap, p._angleToEncoder, p._maxStep);
2543  ImplementAxisInfo::initialize(p._njoints, p._axisMap);
2544  ImplementRemoteVariables::initialize(p._njoints, p._axisMap);
2545  ImplementCurrentControl::initialize(p._njoints, p._axisMap, p._ampsToSensor);
2546  ImplementPWMControl::initialize(p._njoints, p._axisMap, p._dutycycleToPwm);
2547 
2548  delete [] tmpZeros; tmpZeros=0;
2549  delete [] tmpOnes; tmpOnes=0;
2550 
2551  // temporary variables used by the ddriver.
2552  _ref_command_positions = allocAndCheck<double>(p._njoints);
2553  _ref_positions = allocAndCheck<double>(p._njoints);
2554  _max_vel_jnt_cmd = allocAndCheck<double>(p._njoints);
2555  _ref_command_speeds = allocAndCheck<double>(p._njoints);
2556  _ref_speeds = allocAndCheck<double>(p._njoints);
2557  _ref_accs = allocAndCheck<double>(p._njoints);
2558  _ref_torques = allocAndCheck<double>(p._njoints);
2559  _last_position_move_time=allocAndCheck<double>(p._njoints);
2560  memset(_last_position_move_time, 0, sizeof(double)*p._njoints);
2561 
2562  for (int i = 0; i<p._njoints; i++)
2563  {
2564  _ref_command_positions[i] = NAN;
2565  _ref_speeds[i] = 0.0;
2566  _ref_command_speeds[i] = 0.0;
2567  }
2568 
2569  _mutex.unlock();
2570 
2571  // default initialization for this device driver.
2572  yarp::os::Time::delay(0.005);
2573  setPids(VOCAB_PIDTYPE_POSITION, p._pids);
2574 
2575  if (p._torqueControlEnabled==true)
2576  {
2577  _MCtorqueControlEnabled = true;
2578  yarp::os::Time::delay(0.005);
2579  setPids(VOCAB_PIDTYPE_TORQUE, p._tpids);
2580 
2581  for (int i=0; i<p._njoints; i++)
2582  {
2583  MotorTorqueParameters ps;
2584  ps.bemf = p._bemfGain[i];
2585  this->setMotorTorqueParams(i,ps);
2586 
2587  yarp::os::Time::delay(0.002);
2588  this->setFilterTypeRaw(i,p._filterType[i]);
2589  yarp::os::Time::delay(0.002);
2590  }
2591  }
2592 
2593  //set the source of the torque measurments to the boards
2594  #if 0
2595  for (int j=0; j<p._njoints; j++)
2596  {
2597  yarp::os::Time::delay(0.001);
2598  this->setTorqueSource(j,p._torqueSensorId[j],p._torqueSensorChan[j]);
2599  }
2600  #endif
2601 
2602  // intial value !=0 for velocity reference during trajectory generation
2603  for (int j = 0; j<p._njoints; j++)
2604  {
2605  yarp::os::Time::delay(0.002);
2606  setRefSpeed(j, 10.0);
2607  }
2608 
2609  // impedance parameters
2610  for (int j=0; j<p._njoints; j++)
2611  {
2612  yarp::os::Time::delay(0.001);
2613  setImpedance(j,p._impedance_params[j].stiffness,p._impedance_params[j].damping);
2614  }
2615 
2616  int i;
2617  for(i = 0; i < p._njoints; i++)
2618  {
2619  yarp::os::Time::delay(0.001);
2620  setBCastMessages(i, p._broadcast_mask[i]);
2621  }
2622 
2623  // set limits, on encoders and max current
2624  for(i = 0; i < p._njoints; i++)
2625  {
2626  yarp::os::Time::delay(0.001);
2627  setLimits(i, p._limitsMin[i], p._limitsMax[i]);
2628  yarp::os::Time::delay(0.001);
2629  setVelLimits(i, 0, p._maxJntCmdVelocity[i]);
2630  yarp::os::Time::delay(0.001);
2631  setMaxCurrent(i, p._currentLimits[i]);
2632  }
2633 
2634  // set limits, on encoders and max current
2635  for(i = 0; i < p._njoints; i++)
2636  {
2637  yarp::os::Time::delay(0.001);
2638  setVelocityShiftRaw(i, p._velocityShifts[i]);
2639  yarp::os::Time::delay(0.001);
2640  setVelocityTimeoutRaw(i, p._velocityTimeout[i]);
2641  yarp::os::Time::delay(0.001);
2642  setPWMLimit(i, p._motorPwmLimits[i]);
2643  }
2644 
2645  // set parameters for speed/acceleration estimation
2646  for(i = 0; i < p._njoints; i++)
2647  {
2648  yarp::os::Time::delay(0.001);
2649  setSpeedEstimatorShiftRaw(i,p._estim_params[i].jnt_Vel_estimator_shift,
2650  p._estim_params[i].jnt_Acc_estimator_shift,
2651  p._estim_params[i].mot_Vel_estimator_shift,
2652  p._estim_params[i].mot_Acc_estimator_shift);
2653  }
2654  _speedEstimationHelper = new speedEstimationHelper(p._njoints, p._estim_params);
2655 
2656  // disable the controller, cards will start with the pid controller & pwm off
2657  for (i = 0; i < p._njoints; i++)
2658  {
2659  yarp::os::Time::delay(0.001);
2660  setControlMode(i, VOCAB_CM_IDLE);
2661  }
2662  const Bottle &analogList=config.findGroup("analog").tail();
2663  // if (analogList!=0)
2664  if (analogList.size()>0)
2665  {
2666  for(int k=0;k<analogList.size();k++)
2667  {
2668  std::string analogId=analogList.get(k).asString().c_str();
2669 
2670  TBR_AnalogSensor *as=instantiateAnalog(config, analogId);
2671  if (as!=0)
2672  {
2673  analogSensors.push_back(as);
2674  }
2675  }
2676 
2677  }
2678 
2680 
2681  PeriodicThread::setPeriod((double)p._polling_interval/1000.0);
2682  PeriodicThread::start();
2683 
2684  //get firmware versions
2685  firmware_info* info = new firmware_info[p._njoints];
2686  can_protocol_info icub_interface_protocol;
2687  icub_interface_protocol.major=CAN_PROTOCOL_MAJOR;
2688  icub_interface_protocol.minor=CAN_PROTOCOL_MINOR;
2689  for (int j=0; j<p._njoints; j++)
2690  {
2691  yarp::os::Time::delay(0.001);
2692  bool b=getFirmwareVersionRaw(j,icub_interface_protocol,&(info[j]));
2693  if (b==false) yError() << "Unable to read firmware version";
2694  }
2695  _firmwareVersionHelper = new firmwareVersionHelper(p._njoints, info, icub_interface_protocol);
2697 
2698 #ifdef ICUB_CANPROTOCOL_STRICT
2701  {
2702  PeriodicThread::stop();
2703  _opened = false;
2704  yError() << "checkFirmwareVersions() failed. CanBusMotionControl::open returning false,";
2705  return false;
2706  }
2708 #else
2709  yWarning("*********************************************************************************\n");
2710  yWarning("**** WARNING: ICUB_CANPROTOCOL_STRICT not defined, skipping firmware check! *****\n");
2711  yWarning("*********************************************************************************\n");
2712 #endif
2713 
2714  _opened = true;
2715  yDebug() << "CanBusMotionControl::open returned true\n";
2716  return true;
2717 }
2718 
2719 bool CanBusMotionControl::readFullScaleAnalog(int analog_address, int ch, double* fullScale)
2720 {
2722  int destId=0x0200|analog_address;
2723  *fullScale=1e-20;
2724  unsigned int i=0;
2725  res.startPacket();
2726  res._writeBuffer[0].setId(destId);
2727  res._writeBuffer[0].getData()[0]=0x18;
2728  res._writeBuffer[0].getData()[1]=ch;
2729  res._writeBuffer[0].setLen(2);
2730  res._writeMessages++;
2731  res.writePacket();
2732 
2733  long int timeout=0;
2734  bool full_scale_read=false;
2735  do
2736  {
2737  res.read();
2738  for (i=0; i<res._readMessages; i++)
2739  {
2740  CanMessage& m = res._readBuffer[i];
2741  unsigned int currId=m.getId();
2742  if (currId==(0x0200|(analog_address<<4)))
2743  if (m.getLen()==4 &&
2744  m.getData()[0]==0x18 &&
2745  m.getData()[1]==ch)
2746  {
2747  *fullScale=m.getData()[2]<<8 | m.getData()[3];
2748  full_scale_read=true;
2749  break;
2750  }
2751  }
2752  yarp::os::Time::delay(0.002);
2753  timeout++;
2754  }
2755  while(timeout<32 && full_scale_read==false);
2756 
2757  if (full_scale_read==false)
2758  {
2759  yError() << "Trying to get fullscale data from sensor: no answer received or message lost (ch:" << ch <<")";
2760  return false;
2761  }
2762 
2763  return true;
2764 }
2765 
2766 TBR_AnalogSensor *CanBusMotionControl::instantiateAnalog(yarp::os::Searchable& config, std::string deviceid)
2767 {
2769  TBR_AnalogSensor *analogSensor=0;
2770 
2771  //std::string groupName=std::string("ANALOG-");
2772  //groupName+=deviceid;
2773  //Bottle analogConfig=config.findGroup(groupName.c_str());
2774  Bottle &analogConfig=config.findGroup(deviceid.c_str());
2775  if (analogConfig.size()>0)
2776  {
2777  yDebug("Initializing analog device %s\n", deviceid.c_str());
2778 
2779  analogSensor=new TBR_AnalogSensor;
2780  analogSensor->setDeviceId(deviceid);
2781 
2782  bool isVirtualSensor=false;
2783  char analogId=analogConfig.find("CanAddress").asInt32();
2784  char analogFormat=analogConfig.find("Format").asInt32();
2785  int analogChannels=analogConfig.find("Channels").asInt32();
2786  int analogCalibration=analogConfig.find("UseCalibration").asInt32();
2787  //int SensorFullScale=analogConfig.find("FullScale").asInt32();
2788 
2789  if (analogConfig.check("PortName"))
2790  {
2791  isVirtualSensor = true;
2792  std::string virtualPortName = analogConfig.find("PortName").asString();
2793  bool canEchoEnabled = analogConfig.find("CanEcho").asInt32();
2794  analogSensor->backDoor = new TBR_CanBackDoor();
2795  analogSensor->backDoor->setUp(&res, &_mutex, canEchoEnabled, analogSensor);
2796  std::string rn("/");
2797  rn += config.find("robotName").asString().c_str();
2798  //rn+=String("/");
2799  rn+=virtualPortName;
2800  analogSensor->backDoor->open(rn.c_str());
2801  //RANDAZ_TODO if needed set other parameters to backDoor
2802  }
2803 
2804  switch (analogFormat)
2805  {
2806  case 8:
2807  analogSensor->open(analogChannels, TBR_AnalogSensor::ANALOG_FORMAT_8, analogId, analogCalibration, isVirtualSensor);
2808  break;
2809  case 16:
2810  analogSensor->open(analogChannels, TBR_AnalogSensor::ANALOG_FORMAT_16, analogId, analogCalibration, isVirtualSensor);
2811  break;
2812  }
2813 
2814  int destId=0x0200|analogSensor->getId();
2815 
2816  if (analogConfig.check("Period"))
2817  {
2818  int period=analogConfig.find("Period").asInt32();
2819  res.startPacket();
2820 
2821  res.startPacket();
2822  res._writeBuffer[0].setId(destId);
2823  res._writeBuffer[0].getData()[0]=0x08;
2824  res._writeBuffer[0].getData()[1]=period;
2825  res._writeBuffer[0].setLen(2);
2826  res._writeMessages++;
2827  res.writePacket();
2828 
2829  }
2830 
2831  //init message for mais board
2832  if (analogChannels==16 && analogFormat==8)
2833  {
2834  res.startPacket();
2835  res._writeBuffer[0].setId(destId);
2836  res._writeBuffer[0].getData()[0]=0x07;
2837  res._writeBuffer[0].getData()[1]=0x00;
2838  res._writeBuffer[0].setLen(2);
2839  res._writeMessages++;
2840  res.writePacket();
2841  }
2842  //init message for strain board
2843  else if (analogChannels==6 && analogFormat==16 && isVirtualSensor==false)
2844  {
2845  //calibrated astrain board
2846  if (analogCalibration==1)
2847  {
2848  //get the full scale values from the strain board
2849  for (int ch=0; ch<6; ch++)
2850  {
2851  bool b=false;
2852  int attempts = 0;
2853  while(attempts<15)
2854  {
2855  b = readFullScaleAnalog(analogSensor->getId(), ch, &analogSensor->getScaleFactor()[ch]);
2856  if (b==true)
2857  {
2858  if (attempts>0) yWarning("Trying to get fullscale data from sensor: channel recovered (ch:%d)\n", ch);
2859  break;
2860  }
2861  attempts++;
2862  yarp::os::Time::delay(0.020);
2863  }
2864  if (attempts>=15)
2865  {
2866  yError("Trying to get fullscale data from sensor %s: all attempts failed (ch:%d)\n", deviceid.c_str(), ch);
2867  yError("Device %s cannot be opened.\n", deviceid.c_str());
2868  return 0; //@@@delete missing, but TBR_AnalogSensor will be deprecated soon
2869  }
2870  }
2871 
2872  // debug messages
2873  #if 1
2874  yDebug("Sensor Fullscale Id %#4X: %f %f %f %f %f %f ",analogId,
2875  analogSensor->getScaleFactor()[0],
2876  analogSensor->getScaleFactor()[1],
2877  analogSensor->getScaleFactor()[2],
2878  analogSensor->getScaleFactor()[3],
2879  analogSensor->getScaleFactor()[4],
2880  analogSensor->getScaleFactor()[5]);
2881  #endif
2882 
2883  // start the board
2884  res.startPacket();
2885  res._writeBuffer[0].setId(destId);
2886  res._writeBuffer[0].getData()[0]=0x07;
2887  res._writeBuffer[0].getData()[1]=0x00;
2888  res._writeBuffer[0].setLen(2);
2889  res._writeMessages++;
2890  res.writePacket();
2891  }
2892  //not calibrated strain board
2893  else
2894  {
2895  res.startPacket();
2896  res._writeBuffer[0].setId(destId);
2897  res._writeBuffer[0].getData()[0]=0x07;
2898  res._writeBuffer[0].getData()[1]=0x03;
2899  res._writeBuffer[0].setLen(2);
2900  res._writeMessages++;
2901  res.writePacket();
2902  }
2903  }
2904  else if (analogChannels==6 && analogFormat==16 && isVirtualSensor==true)
2905  {
2906  //set the full scale values for a VIRTUAL sensor
2907  for (int jnt=0; jnt<_axisTorqueHelper->getNumberOfJoints(); jnt++)
2908  {
2909  int cfgId = _axisTorqueHelper->getTorqueSensorId(jnt);
2910  int cfgChan = _axisTorqueHelper->getTorqueSensorChan(jnt);
2911  if (cfgId == analogId)
2912  {
2913  for (int ch=0; ch<6; ch++)
2914  {
2915  if (cfgChan == ch)
2916  {
2917  double maxTrq = _axisTorqueHelper->getMaximumTorque(jnt);
2918  analogSensor->getScaleFactor()[ch]=maxTrq;
2919  break;
2920  }
2921  }
2922  }
2923  }
2924 
2925  // debug messages
2926  #if 1
2927  yDebug("Sensor Fullscale Id %#4X: %f %f %f %f %f %f",analogId,
2928  analogSensor->getScaleFactor()[0],
2929  analogSensor->getScaleFactor()[1],
2930  analogSensor->getScaleFactor()[2],
2931  analogSensor->getScaleFactor()[3],
2932  analogSensor->getScaleFactor()[4],
2933  analogSensor->getScaleFactor()[5]);
2934  #endif
2935  }
2936  }
2937  return analogSensor;
2938 }
2939 
2940 void CanBusMotionControl::finiAnalog(TBR_AnalogSensor *analogSensor)
2941 {
2943 
2944  if (analogSensor->isOpen())
2945  {
2946  res.startPacket();
2947  int destId=0x0200|analogSensor->getId();
2948 
2949  res.startPacket();
2950  res._writeBuffer[0].setId(destId);
2951  res._writeBuffer[0].getData()[0]=0x07;
2952  res._writeBuffer[0].getData()[1]=0x01;
2953  res._writeBuffer[0].setLen(2);
2954  res._writeMessages++;
2955 
2956  //debug
2957 #if 0
2958  yDebug("---> Len:%d %x %x %x\n",
2959  res._writeBuffer[0].getLen(),
2960  res._writeBuffer[0].getId(),
2961  res._writeBuffer[0].getData()[0],
2962  res._writeBuffer[0].getData()[1]);
2963 #endif
2964  res.writePacket();
2965 
2966  //shut down backdoor
2967  if (analogSensor->backDoor)
2968  analogSensor->backDoor->close();
2969  }
2970 }
2971 
2973 {
2975 
2976  //yDebug("CanBusMotionControl::close\n");
2977 
2978  if (_opened) {
2979  // disable the controller, pid controller & pwm off
2980  int i;
2981  for (i = 0; i < res._njoints; i++)
2982  {
2983  setControlMode(i, VOCAB_CM_IDLE);
2984  }
2985 
2986  if (isRunning())
2987  {
2989  int i;
2990  for(i = 0; i < res.getJoints(); i++)
2991  setBCastMessages(i, 0x00);
2992  }
2993 
2994  PeriodicThread::stop ();
2995 
2996  ImplementPositionControl::uninitialize();
2997  ImplementVelocityControl::uninitialize();
2998 
2999 // ImplementPositionControl<CanBusMotionControl, IPositionControl>::uninitialize ();
3000 // ImplementVelocityControl<CanBusMotionControl, IVelocityControl>::uninitialize();
3001 
3002  ImplementPidControl::uninitialize();
3003  ImplementEncodersTimed::uninitialize();
3004  ImplementMotorEncoders::uninitialize();
3005  ImplementControlCalibration::uninitialize();
3006  ImplementAmplifierControl::uninitialize();
3007  ImplementControlLimits::uninitialize();
3008 
3009  ImplementControlMode::uninitialize();
3010  ImplementTorqueControl::uninitialize();
3011  ImplementImpedanceControl::uninitialize();
3012  ImplementPositionDirect::uninitialize();
3013  ImplementInteractionMode::uninitialize();
3014  ImplementRemoteVariables::uninitialize();
3015  ImplementAxisInfo::uninitialize();
3016  ImplementCurrentControl::uninitialize();
3017  ImplementPWMControl::uninitialize();
3018 
3019  //stop analog sensors
3020  std::list<TBR_AnalogSensor *>::iterator it=analogSensors.begin();
3021  while(it!=analogSensors.end())
3022  {
3023  if ((*it))
3024  {
3025  finiAnalog(*it);
3026  delete (*it);
3027  (*it)=0;
3028  }
3029  it++;
3030  }
3031 
3032  }
3033 
3034  if (threadPool != 0)
3035  {delete threadPool; threadPool = 0;}
3036  if (_axisTorqueHelper != 0)
3037  {delete _axisTorqueHelper; _axisTorqueHelper = 0;}
3038  if (_firmwareVersionHelper != 0)
3040 
3041  checkAndDestroy<double> (_ref_command_positions);
3042  checkAndDestroy<double> (_ref_command_speeds);
3043  checkAndDestroy<double> (_ref_positions);
3044  checkAndDestroy<double> (_ref_speeds);
3045  checkAndDestroy<double> (_ref_accs);
3046  checkAndDestroy<double> (_ref_torques);
3047  checkAndDestroy<double> (_max_vel_jnt_cmd);
3048 
3049  int ret = res.uninitialize ();
3050  _opened = false;
3051 
3052  return ret;
3053 }
3054 
3055 void CanBusMotionControl::handleBroadcasts()
3056 {
3058 
3059  double before=Time::now();
3060  unsigned int i=0;
3061  const int _networkN=r._networkN;
3062 
3063  for (unsigned int buff_num=0; buff_num<2; buff_num++)
3064  {
3065  unsigned int size = 0;
3066  CanBuffer* buffer_pointer=0;
3067  if (buff_num==0)
3068  {
3069  size = r._readMessages;
3070  buffer_pointer = &r._readBuffer;
3071  }
3072  else
3073  {
3074  size = r._echoMessages;
3075  buffer_pointer = &r._echoBuffer;
3076  }
3077 
3078  for (i = 0; i < size; i++)
3079  {
3080  unsigned int len=0;
3081  unsigned int id=0;
3082  unsigned char *data=0;
3083  CanMessage& m = (*buffer_pointer)[i];
3084  data=m.getData();
3085  id=m.getId();
3086  len=m.getLen();
3087 
3088  if ((id & 0x700) == 0x300) // class = 3 These messages come from analog sensors
3089  {
3090  // 4 next bits = source address, next 4 bits = msg type
3091  const unsigned int addr = ((id & 0x0f0) >> 4);
3092  const unsigned int type = id & 0x00f;
3093  if (type == 0x0A || type == 0x0B) //strain messages
3094  {
3095  int off = (type-0x0A)*3;
3096  for (int axis=0; axis<r.getJoints(); axis++)
3097  {
3098  int attached_board = _axisTorqueHelper->getTorqueSensorId(axis);
3099  if ( attached_board == addr)
3100  {
3101  for(int chan=0;chan<3;chan++)
3102  {
3103  int attached_channel = _axisTorqueHelper->getTorqueSensorChan(axis);
3104  if ( attached_channel == chan+off)
3105  {
3106  double scaleFactor = 1/_axisTorqueHelper->getNewtonsToSensor(axis);
3107  r._bcastRecvBuffer[axis]._torque=(((unsigned short)(data[2*chan+1]))<<8)+data[2*chan]-0x8000;
3108  r._bcastRecvBuffer[axis]._torque=r._bcastRecvBuffer[axis]._torque*scaleFactor;
3109  //r._bcastRecvBuffer[axis]._torque=0;
3110  r._bcastRecvBuffer[axis]._update_t = before;
3111  }
3112  }
3113  }
3114  }
3115  }
3116  }
3117  else if ((id & 0x700) == 0x100) // class = 1 These messages come from the control boards.
3118  {
3119  // 4 next bits = source address, next 4 bits = msg type
3120  // this allows sending two 32-bit numbers is a single CAN message.
3121  //
3122  // need an array here for storing the messages on a per-joint basis.
3123  const int addr = ((id & 0x0f0) >> 4);
3124  int j;
3125  bool found=false;
3126  for (j = 0; j < CAN_MAX_CARDS; j++)
3127  {
3128  if (r._destinations[j] == addr)
3129  {
3130  found = true;
3131  break;
3132  }
3133  }
3134 
3135  if (!found)
3136  {
3137  static int count=0;
3138  if (count%5000==0)
3139  {
3140  yError("%s [%d] Warning, got unexpected broadcast msg(s), last one from address %d, (original) id 0x%x, len %d\n", canDevName.c_str(), _networkN, addr, id, len);
3141  char tmp1 [255]; tmp1[0]=0;
3142  char tmp2 [255]; tmp2[0]=0;
3143  for (j = 0; j < CAN_MAX_CARDS; j++)
3144  {
3145  sprintf (tmp1, "%d ", r._destinations[j]);
3146  strcat (tmp2,tmp1);
3147  }
3148  yError("%s [%d] valid addresses are (%s)\n",canDevName.c_str(), _networkN,tmp2);
3149  count++;
3150  j=-1; //error
3151  }
3152  }
3153  else
3154  {
3155  j *= 2;
3156 
3157  /* less sign nibble specifies msg type */
3158  switch (id & 0x00f)
3159  {
3160  case ICUBCANPROTO_PER_MC_MSG__OVERFLOW:
3161 
3162  yError ("CAN PACKET LOSS, board %d buffer full\r\n", (((id & 0x0f0) >> 4)-1));
3163 
3164  break;
3165 
3166  case ICUBCANPROTO_PER_MC_MSG__PRINT:
3167 
3168  if (data[0] == ICUBCANPROTO_PER_MC_MSG__PRINT ||
3169  data[0] == ICUBCANPROTO_PER_MC_MSG__PRINT + 128)
3170  {
3171  int addr = (((id & 0x0f0) >> 4)-1);
3172 
3173  int string_id = cstring[addr].add_string(&r._readBuffer[i]);
3174  if (string_id != -1)
3175  {
3176  cstring[addr].print(string_id, canDevName.c_str(), r._networkN);
3177  cstring[addr].clear_string(string_id);
3178  }
3179  }
3180  break;
3181 
3182  case ICUBCANPROTO_PER_MC_MSG__POSITION:
3183  {
3184  // r._bcastRecvBuffer[j]._position = *((int *)(data));
3185  // r._bcastRecvBuffer[j]._update_p = before;
3186  int tmp=*((int *)(data));
3188 
3189  j++;
3190  if (j < r.getJoints())
3191  {
3192  tmp =*((int *)(data+4));
3193  //r._bcastRecvBuffer[j]._position = *((int *)(data+4));
3194  //r._bcastRecvBuffer[j]._update_p = before;
3196  }
3197  }
3198  break;
3199 
3200  case ICUBCANPROTO_PER_MC_MSG__MOTOR_POSITION:
3201  {
3202  // r._bcastRecvBuffer[j]._position = *((int *)(data));
3203  // r._bcastRecvBuffer[j]._update_p = before;
3204  int tmp=*((int *)(data));
3206 
3207  j++;
3208  if (j < r.getJoints())
3209  {
3210  tmp =*((int *)(data+4));
3211  //r._bcastRecvBuffer[j]._position = *((int *)(data+4));
3212  //r._bcastRecvBuffer[j]._update_p = before;
3214  }
3215  }
3216  break;
3217 
3218  case ICUBCANPROTO_PER_MC_MSG__MOTOR_SPEED:
3219  {
3220  int tmp;
3221  tmp =*((short *)(data));
3222  r._bcastRecvBuffer[j]._speed_rotor.update(tmp, before);
3223  tmp =*((short *)(data+4));
3224  r._bcastRecvBuffer[j]._accel_rotor.update(tmp, before);
3225  r._bcastRecvBuffer[j]._update_s = before;
3226  j++;
3227  if (j < r.getJoints())
3228  {
3229  tmp =*((short *)(data+2));
3230  r._bcastRecvBuffer[j]._speed_rotor.update(tmp, before);
3231  tmp =*((short *)(data+6));
3232  r._bcastRecvBuffer[j]._accel_rotor.update(tmp, before);
3233  r._bcastRecvBuffer[j]._update_s = before;
3234  }
3235  break;
3236  }
3237  break;
3238  #if 0
3239  case ICUBCANPROTO_PER_MC_MSG__TORQUE:
3240  {
3241  int tmp=0; //*((int *)(data));
3243  r._bcastRecvBuffer[j]._update_t=before;
3244 
3245  j++;
3246  if (j < r.getJoints())
3247  {
3248  tmp = 0;//*((int *)(data+4));
3250  r._bcastRecvBuffer[j]._update_t=before;
3251  }
3252  }
3253  #endif
3254 
3255  case ICUBCANPROTO_PER_MC_MSG__PID_VAL:
3256  r._bcastRecvBuffer[j]._pid_value = *((short *)(data));
3257  r._bcastRecvBuffer[j]._update_v = before;
3258 
3259  j++;
3260  if (j < r.getJoints())
3261  {
3262  r._bcastRecvBuffer[j]._pid_value = *((short *)(data+2));
3263  r._bcastRecvBuffer[j]._update_v = before;
3264  }
3265  break;
3266 
3267  case ICUBCANPROTO_PER_MC_MSG__STATUS:
3268  // fault signals.
3269  r._bcastRecvBuffer[j]._axisStatus= *((short *)(data));
3270  r._bcastRecvBuffer[j]._canStatus= *((char *)(data+4));
3271  r._bcastRecvBuffer[j]._boardStatus= *((char *)(data+5));
3272  r._bcastRecvBuffer[j]._update_e = before;
3273  r._bcastRecvBuffer[j]._controlmodeStatus=*((char *)(data+1));
3274  r._bcastRecvBuffer[j]._address=addr;
3275  r._bcastRecvBuffer[j]._canTxError+=*((char *) (data+6));
3276  r._bcastRecvBuffer[j]._canRxError+=*((char *) (data+7));
3277  #if 0
3278  if (_networkN==1)
3279  {
3280  for(int m=0;m<8;m++)
3281  yDebug("%.2x ", data[m]);
3282  yDebug("\n");
3283  }
3284  #endif
3285 
3286  bool bFlag;
3287 
3288  if ((bFlag=r._bcastRecvBuffer[j].isOverCurrent())) yError ("%s [%d] board %d OVERCURRENT AXIS 0\n", canDevName.c_str(), _networkN, addr);
3289  logJointData(canDevName.c_str(),_networkN,j,9,yarp::os::Value((int)bFlag));
3290 
3291  //r._bcastRecvBuffer[j].ControlStatus(r._networkN, r._bcastRecvBuffer[j]._controlmodeStatus,addr);
3292  //if (r._bcastRecvBuffer[j].isFaultOk()) yInfo("Board %d OK\n", addr);
3293 
3294  logJointData(canDevName.c_str(),_networkN,j,21,yarp::os::Value((int)r._bcastRecvBuffer[j]._controlmodeStatus));
3295 
3296  if ((bFlag=r._bcastRecvBuffer[j].isFaultUndervoltage())) yError ("%s [%d] board %d FAULT UNDERVOLTAGE AXIS 0\n", canDevName.c_str(), _networkN, addr);
3297  logJointData(canDevName.c_str(),_networkN,j,7,yarp::os::Value((int)bFlag));
3298 
3299  if ((bFlag=r._bcastRecvBuffer[j].isFaultExternal())) yWarning ("%s [%d] board %d FAULT EXT AXIS 0\n", canDevName.c_str(), _networkN, addr);
3300  logJointData(canDevName.c_str(),_networkN,j,10,yarp::os::Value((int)bFlag));
3301 
3302  if ((bFlag=r._bcastRecvBuffer[j].isFaultOverload())) yError ("%s [%d] board %d FAULT OVERLOAD AXIS 0\n", canDevName.c_str(), _networkN, addr);
3303  logJointData(canDevName.c_str(),_networkN,j,8,yarp::os::Value((int)bFlag));
3304 
3305  if ((bFlag=r._bcastRecvBuffer[j].isHallSensorError())) yError ("%s [%d] board %d HALL SENSOR ERROR AXIS 0\n", canDevName.c_str(), _networkN, addr);
3306  logJointData(canDevName.c_str(),_networkN,j,11,yarp::os::Value((int)bFlag));
3307 
3308  if ((bFlag=r._bcastRecvBuffer[j].isAbsEncoderError())) yError ("%s [%d] board %d ABS ENCODER ERROR AXIS 0\n", canDevName.c_str(), _networkN, addr);
3309  logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((int)bFlag));
3310 
3311  if ((bFlag=r._bcastRecvBuffer[j].isOpticalEncoderError())) yError ("%s [%d] board %d OPTICAL ENCODER ERROR AXIS 0\n", canDevName.c_str(), _networkN, addr);
3312  logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((int)bFlag));
3313 
3314  if ((bFlag=r._bcastRecvBuffer[j].isCanTxOverflow())) yError ("%s [%d] board %d CAN TX OVERFLOW \n", canDevName.c_str(), _networkN, addr);
3315  logJointData(canDevName.c_str(),_networkN,j,16,yarp::os::Value((int)bFlag));
3316 
3317  if ((bFlag=r._bcastRecvBuffer[j].isCanBusOff())) yError ("%s [%d] board %d CAN BUS_OFF \n", canDevName.c_str(), _networkN, addr);
3318  logJointData(canDevName.c_str(),_networkN,j,13,yarp::os::Value((int)bFlag));
3319 
3320  if (r._bcastRecvBuffer[j].isCanTxError()) yError ("%s [%d] board %d CAN TX ERROR \n", canDevName.c_str(), _networkN, addr);
3321  logJointData(canDevName.c_str(),_networkN,j,14,yarp::os::Value((int)r._bcastRecvBuffer[j]._canTxError));
3322 
3323  if (r._bcastRecvBuffer[j].isCanRxError()) yError ("%s [%d] board %d CAN RX ERROR \n", canDevName.c_str(), _networkN, addr);
3324  logJointData(canDevName.c_str(),_networkN,j,15,yarp::os::Value((int)r._bcastRecvBuffer[j]._canRxError));
3325 
3326  if ((bFlag=r._bcastRecvBuffer[j].isCanTxOverrun())) yError ("%s [%d] board %d CAN TX OVERRUN \n", canDevName.c_str(), _networkN, addr);
3327  logJointData(canDevName.c_str(),_networkN,j,17,yarp::os::Value((int)bFlag));
3328 
3329  if (r._bcastRecvBuffer[j].isCanRxWarning()) yError ("%s [%d] board %d CAN RX WARNING \n", canDevName.c_str(), _networkN, addr);
3330 
3331  if ((bFlag=r._bcastRecvBuffer[j].isCanRxOverrun())) yError ("%s [%d] board %d CAN RX OVERRUN \n", canDevName.c_str(), _networkN, addr);
3332  logJointData(canDevName.c_str(),_networkN,j,17,yarp::os::Value((int)bFlag));
3333 
3334  if ((bFlag=r._bcastRecvBuffer[j].isMainLoopOverflow()))
3335  {
3337  //yWarning ("%s [%d] board %d MAIN LOOP TIME EXCEDEED \n", canDevName.c_str(), _networkN, addr);
3338  }
3339  logJointData(canDevName.c_str(),_networkN,j,18,yarp::os::Value((int)bFlag));
3340 
3341  if ((bFlag=r._bcastRecvBuffer[j].isOverTempCh1())) yError ("%s [%d] board %d OVER TEMPERATURE CH 1 \n", canDevName.c_str(), _networkN, addr);
3342  logJointData(canDevName.c_str(),_networkN,j,19,yarp::os::Value((int)bFlag));
3343 
3344  if ((bFlag=r._bcastRecvBuffer[j].isOverTempCh2())) yError ("%s [%d] board %d OVER TEMPERATURE CH 2 \n", canDevName.c_str(), _networkN, addr);
3345  logJointData(canDevName.c_str(),_networkN,j+1,19,yarp::os::Value((int)bFlag));
3346 
3347  if ((bFlag=r._bcastRecvBuffer[j].isTempErrorCh1())) yError ("%s [%d] board %d ERROR TEMPERATURE CH 1\n", canDevName.c_str(), _networkN, addr);
3348  logJointData(canDevName.c_str(),_networkN,j,20,yarp::os::Value((int)bFlag));
3349 
3350  if ((bFlag=r._bcastRecvBuffer[j].isTempErrorCh2())) yError ("%s [%d] board %d ERROR TEMPERATURE CH 2\n", canDevName.c_str(), _networkN, addr);
3351  logJointData(canDevName.c_str(),_networkN,j+1,20,yarp::os::Value((int)bFlag));
3352 
3353  j++;
3354 
3355  if (j < r.getJoints())
3356  {
3357  r._bcastRecvBuffer[j]._address=addr;
3358  r._bcastRecvBuffer[j]._axisStatus= *((short *)(data+2));
3359  r._bcastRecvBuffer[j]._update_e = before;
3360  r._bcastRecvBuffer[j]._controlmodeStatus=*((char *)(data+3));
3361  // r._bcastRecvBuffer[j].ControlStatus(r._networkN, r._bcastRecvBuffer[j]._controlmodeStatus,addr);
3362 
3363  logJointData(canDevName.c_str(),_networkN,j,21,yarp::os::Value((int)r._bcastRecvBuffer[j]._controlmodeStatus));
3364 
3365  if ((bFlag=r._bcastRecvBuffer[j].isOverCurrent())) yError ("%s [%d] board %d OVERCURRENT AXIS 1\n", canDevName.c_str(), _networkN, addr);
3366  logJointData(canDevName.c_str(),_networkN,j,9,yarp::os::Value((int)bFlag));
3367 
3368  if ((bFlag=r._bcastRecvBuffer[j].isFaultUndervoltage())) yError ("%s [%d] board %d FAULT UNDERVOLTAGE AXIS 1\n", canDevName.c_str(), _networkN, addr);
3369  logJointData(canDevName.c_str(),_networkN,j,7,yarp::os::Value((int)bFlag));
3370 
3371  if ((bFlag=r._bcastRecvBuffer[j].isFaultExternal())) yWarning ("%s [%d] board %d FAULT EXT AXIS 1\n", canDevName.c_str(), _networkN, addr);
3372  logJointData(canDevName.c_str(),_networkN,j,10,yarp::os::Value((int)bFlag));
3373 
3374  if ((bFlag=r._bcastRecvBuffer[j].isFaultOverload())) yError ("%s [%d] board %d FAULT OVERLOAD AXIS 1\n", canDevName.c_str(), _networkN, addr);
3375  logJointData(canDevName.c_str(),_networkN,j,8,yarp::os::Value((int)bFlag));
3376 
3377  if ((bFlag=r._bcastRecvBuffer[j].isHallSensorError())) yError ("%s [%d] board %d HALL SENSOR ERROR AXIS 1\n", canDevName.c_str(), _networkN, addr);
3378  logJointData(canDevName.c_str(),_networkN,j,11,yarp::os::Value((int)bFlag));
3379 
3380  if ((bFlag=r._bcastRecvBuffer[j].isAbsEncoderError())) yError ("%s [%d] board %d ABS ENCODER ERROR AXIS 1\n", canDevName.c_str(), _networkN, addr);
3381  logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((int)bFlag));
3382 
3383  if ((bFlag=r._bcastRecvBuffer[j].isOpticalEncoderError())) yError ("%s [%d] board %d OPTICAL ENCODER ERROR AXIS 0\n", canDevName.c_str(), _networkN, addr);
3384  logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((int)bFlag));
3385  }
3386 
3387  break;
3388 
3389  case ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS:
3390  r._bcastRecvBuffer[j]._interactionmodeStatus=*((char *)(data)) & 0x0F;
3391  r._bcastRecvBuffer[j]._update_e2 = before;
3392  j++;
3393  if (j < r.getJoints())
3394  {
3395  r._bcastRecvBuffer[j]._interactionmodeStatus=(*((char *)(data)) >> 4) & 0x0F;
3396  r._bcastRecvBuffer[j]._update_e2 = before;
3397  }
3398  break;
3399 
3400  case ICUBCANPROTO_PER_MC_MSG__CURRENT:
3401  r._bcastRecvBuffer[j]._current = *((short *)(data));
3402  r._bcastRecvBuffer[j]._update_c = before;
3403  j++;
3404  if (j < r.getJoints())
3405  {
3406  r._bcastRecvBuffer[j]._current = *((short *)(data+2));
3407  r._bcastRecvBuffer[j]._update_c = before;
3408  }
3409  break;
3410 
3411  case ICUBCANPROTO_PER_MC_MSG__PID_ERROR:
3412  r._bcastRecvBuffer[j]._position_error = *((short *)(data));
3413  r._bcastRecvBuffer[j]._torque_error = *((short *)(data+4));
3414  r._bcastRecvBuffer[j]._update_r = before;
3415  j++;
3416  if (j < r.getJoints())
3417  {
3418  r._bcastRecvBuffer[j]._position_error = *((short *)(data+2));
3419  r._bcastRecvBuffer[j]._torque_error = *((short *)(data+6));
3420  r._bcastRecvBuffer[j]._update_r = before;
3421  }
3422  break;
3423 
3424  case ICUBCANPROTO_PER_MC_MSG__VELOCITY:
3425  // also receives the acceleration values.
3426  r._bcastRecvBuffer[j]._speed_joint = *((short *)(data));
3427  r._bcastRecvBuffer[j]._accel_joint = *((short *)(data+4));
3428  r._bcastRecvBuffer[j]._update_s = before;
3429  j++;
3430  if (j < r.getJoints())
3431  {
3432  r._bcastRecvBuffer[j]._speed_joint = *((short *)(data+2));
3433  r._bcastRecvBuffer[j]._accel_joint = *((short *)(data+6));
3434  r._bcastRecvBuffer[j]._update_s = before;
3435  }
3436  break;
3437 
3438  default:
3439  break;
3440  }
3441  }
3442  }
3443  }
3444  }
3445 }
3446 
3451 {
3453 
3454  _writerequested = false;
3455  _noreply = false;
3456 
3457  r._error_status = true;
3458 
3459  errorstring.reserve(errorstringsize);
3460  previousRun=0;
3461  averagePeriod=0;
3462  averageThreadTime=0;
3463  currentRun=0;
3464  myCount=-1;
3465  lastReportTime=Time::now();
3466 
3467  return true;
3468 }
3469 
3471 {
3472  //doing nothing
3473 }
3474 
3476 {
3478  unsigned int i = 0;
3479 
3480  myCount++;
3481  double before = Time::now();
3482  currentRun=before;
3483  if (myCount>0)
3484  averagePeriod+=(currentRun-previousRun)*1000;
3485 
3487  // check timeout on messages
3488  std::list<CanRequest> timedout;
3489  for(int j=0;j<r.requestsQueue->getNJoints();j++)
3490  {
3491  for(int m=0;m<r.requestsQueue->getNMessages();m++)
3492  {
3493  ThreadFifo *fifo=r.requestsQueue->getFifo(j,m);
3494  ACE_ASSERT (fifo!=0);
3495  std::list<ThreadId>::iterator it=fifo->begin();
3496  std::list<ThreadId>::iterator end=fifo->end();
3497  while(it!=end)
3498  {
3499  //increase wait time
3500  (*it).waitTime+=r._polling_interval;
3501  if ((*it).waitTime>=r._timeout)
3502  {
3503  // ok we have it... this is a timed out message
3504  CanRequest rq;
3505  rq.joint=j;
3506  rq.msg=m;
3507  rq.threadId=(*it).id;
3508  timedout.push_back(rq); //store this request, so we can wake up the waiting thread
3509  it=fifo->erase(it); //it now points to the next element
3510  yError("%s [%d] thread:%d msg:%d joint:%d timed out\n",
3511  canDevName.c_str(),
3512  r._networkN,
3513  rq.threadId, rq.msg, rq.joint);
3514 
3515  // ???
3516  //logJointData(canDevName.c_str(),r._networkN,j,3,yarp::os::Value(1));
3517  }
3518  else
3519  ++it;
3520  }
3521  }
3522  }
3523 
3524  //now go through the list of timedout requests and wake up waiting threads
3525  std::list<CanRequest>::iterator it=timedout.begin();
3526  std::list<CanRequest>::iterator end=timedout.end();
3527  while(it!=end)
3528  {
3529  int tid=(*it).threadId;
3531  t->timeout(); //notify one message timedout
3532  ++it;
3533  }
3535  // report error LOOP
3536  if ((currentRun-lastReportTime)>REPORT_PERIOD)
3537  {
3538  double avPeriod=1000.0*getEstimatedPeriod();
3539  double avThTime=1000.0*getEstimatedUsed();//averageThreadTime/myCount;
3540  unsigned int it=getIterations();
3541  resetStat();
3542  yDebug("%s [%d] thread ran %d times, req.dT:%d[ms], av.dT:%.2lf[ms] av.loopT :%.2lf[ms]\n",
3543  canDevName.c_str(),
3544  r._networkN,
3545  it,
3547  avPeriod,
3548  avThTime);
3549 
3550  const char *can=canDevName.c_str();
3551  logNetworkData(can,r._networkN,10,yarp::os::Value(r._polling_interval));
3552  logNetworkData(can,r._networkN,11,yarp::os::Value((double)avPeriod));
3553  logNetworkData(can,r._networkN,12,yarp::os::Value((double)avThTime));
3554 
3555  if (r.iCanErrors)
3556  {
3557  CanErrors errors;
3558  r.iCanErrors->canGetErrors(errors);
3559  yDebug(" Can Errors -- Device Rx:%u, Device Tx:%u, RxOvf: %u, TxOvf: %u, BusOff: %d -- Driver Fifo Rx:%u, Driver Fifo Tx:%u\n",
3560  errors.rxCanErrors,
3561  errors.txCanErrors,
3562  errors.rxCanFifoOvr,
3563  errors.txCanFifoOvr,
3564  errors.busoff,
3565  errors.rxBufferOvr,
3566  errors.txBufferOvr);
3567 
3568  logNetworkData(can,r._networkN,5,yarp::os::Value(errors.rxCanErrors));
3569  logNetworkData(can,r._networkN,6,yarp::os::Value(errors.txCanErrors));
3570 
3571  logNetworkData(can,r._networkN,7,yarp::os::Value((int)errors.rxCanFifoOvr));
3572  logNetworkData(can,r._networkN,8,yarp::os::Value((int)errors.txCanFifoOvr));
3573 
3574  logNetworkData(can,r._networkN,9,yarp::os::Value((int)errors.busoff));
3575 
3576  logNetworkData(can,r._networkN,3,yarp::os::Value((int)errors.rxBufferOvr));
3577  logNetworkData(can,r._networkN,4,yarp::os::Value((int)errors.txBufferOvr));
3578  }
3579  else
3580  {
3581  yDebug(" Device has no ICanBusErr interface\n");
3582  }
3583 
3584 
3585  int j=0;
3587  char tmp[255] = {0};
3588  errorstring.clear();
3589 
3590  snprintf(tmp, sizeof(tmp), "%s [%d] printing boards infos:\n", canDevName.c_str(), r._networkN);
3591  errorstring.append(tmp);
3592 
3593  bool errorF=false;
3594  for (j=0; j<r._njoints ;j++)
3595  {
3597  {
3598  errorF=true;
3599  int addr=r._destinations[j/2];
3600  yWarning("%s [%d] board %d MAIN LOOP TIME EXCEDEED %d TIMES!\n", canDevName.c_str(), r._networkN, addr,r._bcastRecvBuffer[j]._mainLoopOverflowCounter);
3601  logJointData(canDevName.c_str(),r._networkN,j,18,yarp::os::Value((int)r._bcastRecvBuffer[j]._mainLoopOverflowCounter));
3603  }
3604  else
3605  {
3606  logJointData(canDevName.c_str(),r._networkN,j,18,yarp::os::Value(0));
3607  }
3608  }
3609 
3610  for (j=0; j<r._njoints ;j+=2)
3611  {
3612  int addr=r._destinations[j/2];
3613  if ( (r._bcastRecvBuffer[j]._canTxError>0)||(r._bcastRecvBuffer[j]._canRxError>0))
3614  {
3615  errorF=true;
3616  snprintf(tmp, sizeof(tmp), "Id:%d T:%u R:%u ", addr, r._bcastRecvBuffer[j]._canTxError, r._bcastRecvBuffer[j]._canRxError);
3617  errorstring.append(tmp);
3618 
3619 
3620  logJointData(canDevName.c_str(),r._networkN,j,14,yarp::os::Value((int)r._bcastRecvBuffer[j]._canTxError));
3621  logJointData(canDevName.c_str(),r._networkN,j,15,yarp::os::Value((int)r._bcastRecvBuffer[j]._canRxError));
3622  }
3623  else
3624  {
3625  logJointData(canDevName.c_str(),r._networkN,j,14,yarp::os::Value(0));
3626  logJointData(canDevName.c_str(),r._networkN,j,15,yarp::os::Value(0));
3627  }
3628  }
3629  if (!errorF)
3630  {
3631  snprintf(tmp, sizeof(tmp), "None");
3632  errorstring.append(tmp);
3633  }
3634 
3635  yDebug("%s\n", errorstring.c_str());
3636 
3637  //Check statistics on boards
3638  for (j=0; j<r._njoints ;j++)
3639  {
3640  double lastRecv = r._bcastRecvBuffer[j]._update_e;
3641 
3642  logJointData(canDevName.c_str(),r._networkN,j,2,yarp::os::Value((int)(currentRun-lastRecv)));
3643 
3644  if ( (currentRun-lastRecv)>BCAST_STATUS_TIMEOUT)
3645  {
3646  int ch=j%2;
3647  int addr=r._destinations[j/2];
3648  yWarning("%s [%d] have not heard from board %d (channel %d) since %.2lf seconds\n",
3649  canDevName.c_str(),
3650  r._networkN,
3651  addr, ch, currentRun-lastRecv);
3652 
3653  logJointData(canDevName.c_str(),r._networkN,j,3,yarp::os::Value(1));
3654  }
3655  else
3656  {
3657  logJointData(canDevName.c_str(),r._networkN,j,3,yarp::os::Value(0));
3658  }
3659  }
3660 
3661  //Check position update frequency
3662  for(j=0; j<r._njoints; j++)
3663  {
3664  double dT;
3665  double max;
3666  double min;
3667  int it;
3670 
3671  logJointData(canDevName.c_str(),r._networkN,j,5,yarp::os::Value(max));
3672 
3673  double POS_LATENCY_WARN_THR=averagePeriod*1.5;
3674  if (max>POS_LATENCY_WARN_THR)
3675  {
3676  yWarning("%s [%d] jnt %d, warning encoder latency above threshold (lat: %.2lf [ms], received %d msgs)\n",
3677  canDevName.c_str(),
3678  r._networkN,
3679  j,
3680  max,
3681  it);
3682  }
3683 
3684  if (it<1)
3685  {
3686  yWarning("%s [%d] joint %d, warning not enough encoder messages (received %d msgs)\n",
3687  canDevName.c_str(),
3688  r._networkN,
3689  j,
3690  it);
3691 
3692  logJointData(canDevName.c_str(),r._networkN,j,4,yarp::os::Value(1));
3693  }
3694  else
3695  {
3696  logJointData(canDevName.c_str(),r._networkN,j,4,yarp::os::Value(0));
3697  }
3698  }
3699 
3701  std::list<TBR_AnalogSensor *>::iterator analogIt=analogSensors.begin();
3702  while(analogIt!=analogSensors.end())
3703  {
3704  TBR_AnalogSensor *pAnalog=(*analogIt);
3705  if (pAnalog)
3706  {
3707  unsigned int sat;
3708  unsigned int err;
3709  unsigned int tout;
3710  pAnalog->getCounters(sat, err, tout);
3711 
3712  const char* devName=canDevName.c_str();
3713  int id=pAnalog->getId();
3714 
3715  #ifdef _USE_INTERFACEGUI
3716  logAnalogData(devName,r._networkN,id,3,yarp::os::Value((int)sat));
3717  logAnalogData(devName,r._networkN,id,4,yarp::os::Value((int)err));
3718  logAnalogData(devName,r._networkN,id,5,yarp::os::Value((int)tout));
3719  #endif
3720 
3721  if (sat+err+tout!=0)
3722  {
3723  yWarning("%s [%d] analog %s saturated:%u errors: %u timeout:%u\n",
3724  devName,
3725  r._networkN,
3726  pAnalog->getDeviceId().c_str(),
3727  sat, err, tout);
3728  }
3729  pAnalog->resetCounters();
3730  }
3731  analogIt++;
3732  }
3733 
3734  myCount=0;
3735  lastReportTime=currentRun;
3736  averagePeriod=0;
3737  averageThreadTime=0;
3738  }
3739 
3740  //DEBUG_FUNC("CanBusMotionControl::thread running [%d]: wait\n", mycount);
3741  _mutex.lock();
3742  //DEBUG_FUNC("posted\n");
3743 
3744  if (r.read () != true)
3745  r.printMessage("%s [%d] CAN: read failed\n", canDevName.c_str(), r._networkN);
3746 
3747  // handle all broadcast messages.
3748  // (class 1, 8 bits of the ID used to define the message type and source address).
3749 
3750  handleBroadcasts();
3751 
3752  std::list<TBR_AnalogSensor *>::iterator analogIt=analogSensors.begin();
3753  while(analogIt!=analogSensors.end())
3754  {
3755  TBR_AnalogSensor *pAnalog=(*analogIt);
3756  if (pAnalog)
3757  {
3758  if (!pAnalog->handleAnalog(system_resources))
3759  {
3760  yWarning("%s [%d] analog sensor received unexpected class 0x03 messages\n", canDevName.c_str(), r._networkN);
3761  }
3762  }
3763  else
3764  yWarning("Got null pointer this is unusual\n");
3765 
3766  analogIt++;
3767  }
3768 
3769  //
3770  // handle class 0 messages - polling messages.
3771  // (class 0, 8 bits of the ID used to represent the source and destination).
3772  // the first byte of the message is the message type and motor number (0 or 1).
3773  //
3774  if (r.requestsQueue->getPending()>0)
3775  {
3776  DEBUG_FUNC("There are %d pending messages, read msgs: %d\n",
3778  for (i = 0; i < r._readMessages; i++)
3779  {
3780  unsigned char *msgData;
3781 
3782  CanMessage& m = r._readBuffer[i];
3783  msgData=m.getData();
3784 
3785  if (getClass(m) == 0)
3786  {
3787  PRINT_CAN_MESSAGE("Received \n", m);
3789  int j=getJoint(m,r._destInv); //get joint from message
3790  int id=r.requestsQueue->pop(j, msgData[0]);
3791  if(id==-1)
3792  {
3793  yWarning("%s [%d] Received message but no threads waiting for it. (id: 0x%x, Class:%d MsgData[0]:%d)\n ", canDevName.c_str(), r._networkN, m.getId(), getClass(m), msgData[0]);
3794  continue;
3795  }
3797  if (t==0)
3798  {
3799  yWarning("Asked a bad thread id, this is probably a bug, check threadPool\n");
3800  continue;
3801  }
3802  DEBUG_FUNC("Pushing reply\n");
3803  //push reply to thread's list of replies
3804  if (!t->push(m))
3805  yError("error while pushing a reply, this is probably an error\n");
3806  }
3807  }
3808  }
3809  else
3810  {
3811  //DEBUG_FUNC("Thread loop: no pending messages\n");
3812  }
3813 
3814  // counter ++;
3815  /*if (counter > r._timeout)
3816  {
3818  r.printMessage("CAN: timeout - still %d messages unacknowledged\n", remainingMsgs);
3819  r._error_status = false;
3820  }*/
3821 
3822  r._echoMessages = 0; //echo buffer cleanup
3823 
3824  _mutex.unlock();
3825 
3826  double now = Time::now();
3827  averageThreadTime+=(now-before)*1000;
3828  previousRun=before; //save last run time
3829 }
3830 
3831 
3832  // ControlMode
3834 {
3835  DEBUG_FUNC("Calling GET_CONTROL_MODES\n");
3837  int i;
3838  int temp;
3839  std::lock_guard<std::recursive_mutex> lck(_mutex);
3840  for (i = 0; i < r.getJoints(); i++)
3841  {
3842  temp = int(r._bcastRecvBuffer[i]._controlmodeStatus);
3843  v[i]=from_modeint_to_modevocab(temp);
3844  }
3845  return true;
3846 }
3847 /*
3848 //@@@ TO BE REMOVED LATER (AFTER INCLUDING FIRMWARE_SHARED)
3849 #ifndef icubCanProto_controlmode_calibration
3850 #define icubCanProto_controlmode_calibration 0x060
3851 #endif
3852 
3853 #ifndef icubCanProto_controlmode_forceIdle
3854 #define icubCanProto_controlmode_forceIdle 0x09
3855 #endif
3856 
3857 #ifndef icubCanProto_interactionmode_stiff
3858 #define icubCanProto_interactionmode_stiff 0x00
3859 #endif
3860 
3861 #ifndef icubCanProto_interactionmode_compliant
3862 #define icubCanProto_interactionmode_compliant 0x01
3863 #endif
3864 
3865 #ifndef icubCanProto_interactionmode_unknownError
3866 #define icubCanProto_interactionmode_unknownError 0xFF
3867 #endif
3868 */
3869 //---------------------------------------------------------
3870 
3872 {
3873  switch (interactionvocab)
3874  {
3875  case VOCAB_IM_STIFF:
3876  return icubCanProto_interactionmode_stiff;
3877  break;
3878  case VOCAB_IM_COMPLIANT:
3879  return icubCanProto_interactionmode_compliant;
3880  break;
3881  case VOCAB_IM_UNKNOWN:
3882  default:
3883  return icubCanProto_interactionmode_unknownError;
3884  break;
3885  }
3886 }
3887 
3889 {
3890  switch (interactionint)
3891  {
3892  case icubCanProto_interactionmode_stiff:
3893  return VOCAB_IM_STIFF;
3894  break;
3895  case icubCanProto_interactionmode_compliant:
3896  return VOCAB_IM_COMPLIANT;
3897  break;
3898  case icubCanProto_interactionmode_unknownError:
3899  default:
3900  return icubCanProto_interactionmode_unknownError;
3901  break;
3902  }
3903 }
3904 
3905 
3906 icubCanProto_controlmode_t CanBusMotionControl::from_modevocab_to_modeint (int modevocab)
3907 {
3908  switch (modevocab)
3909  {
3910  case VOCAB_CM_IDLE:
3911  return icubCanProto_controlmode_idle;
3912  break;
3913  case VOCAB_CM_POSITION:
3914  return icubCanProto_controlmode_position;
3915  break;
3916  case VOCAB_CM_MIXED:
3917  return icubCanProto_controlmode_mixed;
3918  break;
3919  case VOCAB_CM_POSITION_DIRECT:
3920  return icubCanProto_controlmode_direct;
3921  break;
3922  case VOCAB_CM_VELOCITY:
3923  return icubCanProto_controlmode_velocity;
3924  break;
3925  case VOCAB_CM_TORQUE:
3926  return icubCanProto_controlmode_torque;
3927  break;
3928  case VOCAB_CM_IMPEDANCE_POS:
3929  return icubCanProto_controlmode_impedance_pos;
3930  break;
3931  case VOCAB_CM_IMPEDANCE_VEL:
3932  return icubCanProto_controlmode_impedance_vel;
3933  break;
3934  case VOCAB_CM_PWM:
3935  return icubCanProto_controlmode_openloop;
3936  break;
3937  case VOCAB_CM_CURRENT:
3938  return icubCanProto_controlmode_unknownError;
3939  yError("'VOCAB_CM_CURRENT' error condition detected");
3940  break;
3941 
3942  case VOCAB_CM_FORCE_IDLE:
3943  return icubCanProto_controlmode_forceIdle;
3944  break;
3945 
3946  default:
3947  return icubCanProto_controlmode_unknownError;
3948  yError ("'VOCAB_CM_UNKNOWN' error condition detected");
3949  break;
3950  }
3951 }
3952 
3954 {
3955  switch (modeint)
3956  {
3957  case icubCanProto_controlmode_idle:
3958  return VOCAB_CM_IDLE;
3959  break;
3960  case icubCanProto_controlmode_position:
3961  return VOCAB_CM_POSITION;
3962  break;
3963  case icubCanProto_controlmode_mixed:
3964  return VOCAB_CM_MIXED;
3965  break;
3966  case icubCanProto_controlmode_direct:
3967  return VOCAB_CM_POSITION_DIRECT;
3968  break;
3969  case icubCanProto_controlmode_velocity:
3970  return VOCAB_CM_VELOCITY;
3971  break;
3972  case icubCanProto_controlmode_torque:
3973  return VOCAB_CM_TORQUE;
3974  break;
3975  case icubCanProto_controlmode_impedance_pos:
3976  return VOCAB_CM_IMPEDANCE_POS;
3977  break;
3978  case icubCanProto_controlmode_impedance_vel:
3979  return VOCAB_CM_IMPEDANCE_VEL;
3980  break;
3981  case icubCanProto_controlmode_openloop:
3982  return VOCAB_CM_PWM;
3983  break;
3984 
3985  //internal status
3986  case icubCanProto_controlmode_hwFault:
3987  return VOCAB_CM_HW_FAULT;
3988  break;
3989  case icubCanProto_controlmode_notConfigured:
3990  return VOCAB_CM_NOT_CONFIGURED;
3991  break;
3992  case icubCanProto_controlmode_configured:
3993  return VOCAB_CM_CONFIGURED;
3994  break;
3995  case icubCanProto_controlmode_unknownError:
3996  yError ("'icubCanProto_controlmode_unknownError' error condition detected");
3997  return VOCAB_CM_UNKNOWN;
3998  break;
3999 
4000  case icubCanProto_controlmode_calibration:
4001 /* Commented out because generate duplicate case in switch, the previous one should be enough
4002  * with this new version of protocol... ask RANDAZZ
4003  case icubCanProto_calibration_type0_hard_stops:
4004  case icubCanProto_calibration_type1_abs_sens_analog:
4005  case icubCanProto_calibration_type2_hard_stops_diff:
4006  case icubCanProto_calibration_type3_abs_sens_digital:
4007  case icubCanProto_calibration_type4_abs_and_incremental:
4008 */
4009  return VOCAB_CM_CALIBRATING;
4010  break;
4011 
4012  default:
4013  yError ("'VOCAB_CM_UNKNOWN' error condition detected");
4014  return VOCAB_CM_UNKNOWN;
4015  break;
4016  }
4017 }
4018 
4020 {
4022  if (!(j>= 0 && j <= r.getJoints()))
4023  {
4024  *v=VOCAB_CM_UNKNOWN;
4025  return false;
4026  }
4027 
4028  short s;
4029 
4030  DEBUG_FUNC("Calling GET_CONTROL_MODE\n");
4031  //_readWord16 (CAN_GET_CONTROL_MODE, j, s);
4032 
4033  std::lock_guard<std::recursive_mutex> lck(_mutex);
4035 
4037  return true;
4038 }
4039 
4040 // IControl Mode 2
4041 bool CanBusMotionControl::getControlModesRaw(const int n_joints, const int *joints, int *modes)
4042 {
4043  DEBUG_FUNC("Calling GET_CONTROL_MODE MULTIPLE JOINTS \n");
4044  if (joints==0) return false;
4045  if (modes==0) return false;
4046 
4048  int i;
4049  std::lock_guard<std::recursive_mutex> lck(_mutex);
4050  for (i = 0; i < n_joints; i++)
4051  {
4052  getControlModeRaw(joints[i], &modes[i]);
4053  }
4054  return true;
4055 }
4056 
4057 bool CanBusMotionControl::setControlModeRaw(const int j, const int mode)
4058 {
4059  if (!(j >= 0 && j <= (CAN_MAX_CARDS-1)*2))
4060  return false;
4061 
4062  if (mode == VOCAB_CM_TORQUE && _MCtorqueControlEnabled == false) {yError()<<"Torque control is disabled. Check your configuration parameters"; return false;}
4063 
4064  DEBUG_FUNC("Calling SET_CONTROL_MODE_RAW SINGLE JOINT\n");
4065  bool ret = true;
4066 
4067  icubCanProto_controlmode_t v = from_modevocab_to_modeint(mode);
4068  if (v==icubCanProto_controlmode_unknownError) return false;
4069  _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,j,v);
4070 
4071  int current_mode = VOCAB_CM_UNKNOWN;
4072  int timeout = 0;
4073 
4074  do
4075  {
4076  getControlModeRaw(j,&current_mode);
4077  if (current_mode==mode) {ret = true; break;}
4078  if (current_mode==VOCAB_CM_IDLE && mode==VOCAB_CM_FORCE_IDLE) {ret = true; break;}
4079  if (current_mode==VOCAB_CM_HW_FAULT)
4080  {
4081  if (mode!=VOCAB_CM_FORCE_IDLE) {yError ("Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), j);}
4082  ret = true; break;
4083  }
4084  yarp::os::Time::delay(0.010);
4085  if (timeout >0) yWarning ("setControlModeRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), j, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(mode).c_str());
4086  timeout++;
4087  }
4088  while (timeout < 10);
4089  if (timeout>=10)
4090  {
4091  ret = false;
4092  yError ("100ms Timeout occured in setControlModeRaw (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), j, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(mode).c_str());
4093  }
4094 
4095  return ret;
4096 }
4097 
4098 bool CanBusMotionControl::setControlModesRaw(const int n_joints, const int *joints, int *modes)
4099 {
4100  DEBUG_FUNC("Calling SET_CONTROL_MODE_RAW MULTIPLE JOINTS\n");
4101  if (n_joints==0) return false;
4102  if (joints==0) return false;
4103  bool ret = true;
4104  for (int i=0;i<n_joints; i++)
4105  {
4106  if (modes[i] == VOCAB_CM_TORQUE && _MCtorqueControlEnabled == false) {yError()<<"Torque control is disabled. Check your configuration parameters"; continue;}
4107 
4108  icubCanProto_controlmode_t v = from_modevocab_to_modeint(modes[i]);
4109  if (v==icubCanProto_controlmode_unknownError) ret = false;
4110  _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,joints[i],v);
4111 
4112  int current_mode = VOCAB_CM_UNKNOWN;
4113  int timeout = 0;
4114  do
4115  {
4116  getControlModeRaw(joints[i],&current_mode);
4117  if (current_mode==modes[i]) {ret = true; break;}
4118  if (current_mode==VOCAB_CM_IDLE)
4119  {
4120  if (modes[i]!=VOCAB_CM_FORCE_IDLE) {yError ("Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), joints[i]);}
4121  ret = true; break;
4122  }
4123  yarp::os::Time::delay(0.010);
4124  if (timeout >0) yWarning ("setControlModesRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), joints[i], yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4125  timeout++;
4126  }
4127  while (timeout < 10);
4128  if (timeout>=10)
4129  {
4130  ret = false;
4131  yError ("100ms Timeout occured in setControlModesRaw(M) (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), joints[i], yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4132  }
4133  }
4134 
4135  return ret;
4136 }
4137 
4138 bool CanBusMotionControl::getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys)
4139 {
4140  listOfKeys->clear();
4141  listOfKeys->addString("filterType");
4142  listOfKeys->addString("PWMLimit");
4143  return true;
4144 }
4145 
4146 bool CanBusMotionControl::getRemoteVariableRaw(std::string key, yarp::os::Bottle& val)
4147 {
4148  val.clear();
4150  if (key == "filterType")
4151  {
4152  Bottle& r = val.addList(); for (int i = 0; i< res.getJoints(); i++) { int tmp = 0; getFilterTypeRaw(i, &tmp); r.addInt32(tmp); }
4153  return true;
4154  }
4155  if (key == "PWMLimit")
4156  {
4157  Bottle& r = val.addList(); for (int i = 0; i< res.getJoints(); i++) { double tmp = 0; getPWMLimitRaw(i, &tmp); r.addFloat64(tmp); }
4158  return true;
4159  }
4160  yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
4161  return false;
4162 }
4163 
4164 bool CanBusMotionControl::setRemoteVariableRaw(std::string key, const yarp::os::Bottle& val)
4165 {
4167  size_t _njoints = r.getJoints();
4168 
4169  std::string s1 = val.toString();
4170  if (val.size() != _njoints)
4171  {
4172  yWarning("setRemoteVariable(): Protocol error %s", s1.c_str());
4173  return false;
4174  }
4175 
4176  if (key == "filterType")
4177  {
4178  for (int i = 0; i < r.getJoints(); i++)
4179  {
4180  int filter_type = val.get(i).asInt32();
4181  this->setFilterTypeRaw(i, filter_type);
4182  }
4183  return true;
4184  }
4185  if (key == "PWMLimit")
4186  {
4187  for (int i = 0; i < r.getJoints(); i++)
4188  {
4189  double limit = val.get(i).asFloat64();
4190  this->setPWMLimitRaw(i, (int)(limit));
4191  }
4192  return true;
4193  }
4194  yWarning("setRemoteVariable(): Unknown variable %s", key.c_str());
4195  return false;
4196 }
4197 
4198 bool CanBusMotionControl::getAxisNameRaw(int axis, std::string& name)
4199 {
4201  if (axis >= 0 && axis < r.getJoints())
4202  {
4203  name = r._jointNames[axis];
4204  return true;
4205  }
4206  else
4207  {
4208  name = "ERROR";
4209  return false;
4210  }
4211 }
4212 
4213 bool CanBusMotionControl::getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type)
4214 {
4216  if (axis >= 0 && axis < r.getJoints())
4217  {
4218  //type = joint_type[axis];
4219  type = VOCAB_JOINTTYPE_REVOLUTE;
4220  return true;
4221  }
4222  else
4223  {
4224  return false;
4225  }
4226 }
4227 
4229 {
4230  DEBUG_FUNC("Calling SET_CONTROL_MODE_RAW ALL JOINT\n");
4232  bool ret = true;
4233 
4234  for (int i = 0; i < r.getJoints(); i++)
4235  {
4236  if (modes[i] == VOCAB_CM_TORQUE && _MCtorqueControlEnabled == false) {yError()<<"Torque control is disabled. Check your configuration parameters"; continue;}
4237 
4238  icubCanProto_controlmode_t v = from_modevocab_to_modeint(modes[i]);
4239  if (v==icubCanProto_controlmode_unknownError) return false;
4240  _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,i,v);
4241 
4242  int current_mode = VOCAB_CM_UNKNOWN;
4243  int timeout = 0;
4244  do
4245  {
4246  getControlModeRaw(i,&current_mode);
4247  if (current_mode==modes[i]) {ret = true; break;}
4248  if (current_mode==VOCAB_CM_IDLE && modes[i]==VOCAB_CM_FORCE_IDLE) {ret = true; break;}
4249  if (current_mode==VOCAB_CM_HW_FAULT)
4250  {
4251  if (modes[i]!=VOCAB_CM_FORCE_IDLE) {yError ("Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), i);}
4252  ret = true; break;
4253  }
4254  yarp::os::Time::delay(0.010);
4255  if (timeout >0) yWarning ("setControlModesRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), i, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4256  timeout++;
4257  }
4258  while (timeout < 10);
4259  if (timeout>=10)
4260  {
4261  ret = false;
4262  yError ("100ms Timeout occured in setControlModesRaw (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), i, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4263  }
4264  }
4265 
4266  return ret;
4267 }
4268 
4269 // return the number of controlled axes.
4271 {
4273  *ax = r.getJoints();
4274 
4275  return true;
4276 }
4277 
4278 bool CanBusMotionControl::helper_setPosPidRaw (int axis, const Pid &pid)
4279 {
4280  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4281  return false;
4282  _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_P_GAIN, axis, S_16(pid.kp));
4283  _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_D_GAIN, axis, S_16(pid.kd));
4284  _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_I_GAIN, axis, S_16(pid.ki));
4285  _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_ILIM_GAIN, axis, S_16(pid.max_int));
4286  _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_OFFSET, axis, S_16(pid.offset));
4287  _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_SCALE, axis, S_16(pid.scale));
4288  _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_TLIM, axis, S_16(pid.max_output));
4289  _writeWord16Ex (ICUBCANPROTO_POL_MC_CMD__SET_POS_STICTION_PARAMS, axis, S_16(pid.stiction_up_val), S_16(pid.stiction_down_val), false);
4290  return true;
4291 }
4292 
4293 bool CanBusMotionControl::setPidRaw (const PidControlTypeEnum& pidtype, int axis, const Pid &pid)
4294 {
4295  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4296  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4297  return false;
4298 
4299  switch (pidtype)
4300  {
4301  case VOCAB_PIDTYPE_POSITION:
4302  helper_setPosPidRaw(axis,pid);
4303  break;
4304  case VOCAB_PIDTYPE_VELOCITY:
4305  helper_setVelPidRaw(axis,pid);
4306  break;
4307  case VOCAB_PIDTYPE_CURRENT:
4308  helper_setCurPidRaw(axis,pid);
4309  break;
4310  case VOCAB_PIDTYPE_TORQUE:
4311  helper_setTrqPidRaw(axis,pid);
4312  break;
4313  default:
4314  yError()<<"Invalid pidtype:"<<pidtype;
4315  break;
4316  }
4317  return true;
4318 }
4319 
4320 bool CanBusMotionControl::getImpedanceRaw (int axis, double *stiff, double *damp)
4321 {
4322  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4323  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4324  return false;
4325 
4326  if (!ENABLED(axis))
4327  {
4328  //@@@ TODO: check here
4329  //*stiff = 0;
4330  //*damp = 0;
4331  return true;
4332  }
4333 
4335  _mutex.lock();
4336  int id;
4337  if (!threadPool->getId(id))
4338  {
4339  yError("More than %d threads, cannot allow more\n", CANCONTROL_MAX_THREADS);
4340  _mutex.unlock();
4341  return false;
4342  }
4343 
4344  r.startPacket();
4345  r.addMessage (id, axis, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS);
4346  r.writePacket();
4347 
4349  t->setPending(r._writeMessages);
4350  _mutex.unlock();
4351  t->synch();
4352 
4353  if (!r.getErrorStatus() || (t->timedOut()))
4354  {
4355  yError("getImpedanceRaw: message timed out\n");
4356  //@@@ TODO: check here
4357  //*stiff = 0;
4358  //*damp = 0;
4359  return false;
4360  }
4361 
4362  CanMessage *m=t->get(0);
4363  if (m==0)
4364  {
4365  //@@@ TODO: check here
4366  //*stiff = 0;
4367  //*damp = 0;
4368  return false;
4369  }
4370 
4371  unsigned char *data;
4372  data=m->getData()+1;
4373  *stiff= *((short *)(data));
4374  data+=2;
4375  *damp= *((short *)(data));
4376  *damp/= 1000;
4377 
4378  t->clear();
4379 
4380  return true;
4381 }
4382 
4383 bool CanBusMotionControl::getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
4384 {
4386  const int axis = j;
4387  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4388  return false;
4389 
4390  std::lock_guard<std::recursive_mutex> lck(_mutex);
4391  // *** This method is implementented reading data without sending/receiving data from the Canbus ***
4396  int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4397  return true;
4398 }
4399 
4401 {
4402  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4403  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4404  return false;
4405 
4406  if (!ENABLED(axis))
4407  {
4408  //@@@ TODO: check here
4409  //*off = 0;
4410  return true;
4411  }
4412 
4414  _mutex.lock();
4415  int id;
4416  if (!threadPool->getId(id))
4417  {
4418  yError("More than %d threads, cannot allow more\n", CANCONTROL_MAX_THREADS);
4419  _mutex.unlock();
4420  return false;
4421  }
4422 
4423  r.startPacket();
4424  r.addMessage (id, axis, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET);
4425  r.writePacket();
4426 
4428  t->setPending(r._writeMessages);
4429  _mutex.unlock();
4430  t->synch();
4431 
4432  if (!r.getErrorStatus() || (t->timedOut()))
4433  {
4434  yError("getImpedanceOffset: message timed out\n");
4435  //@@@ TODO: check here
4436  //*off=0;
4437  return false;
4438  }
4439 
4440  CanMessage *m=t->get(0);
4441  if (m==0)
4442  {
4443  //@@@ TODO: check here
4444  //*off=0;
4445  return false;
4446  }
4447 
4448  unsigned char *data;
4449  data=m->getData()+1;
4450  *off= *((short *)(data));
4451 
4452  t->clear();
4453 
4454  return true;
4455 }
4456 
4457 bool CanBusMotionControl::setImpedanceRaw (int axis, double stiff, double damp)
4458 {
4459  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4460  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4461  return false;
4462 
4463  DEBUG_FUNC("setImpedanceRaw\n");
4464 
4465  if (!ENABLED(axis))
4466  return true;
4467 
4469  std::lock_guard<std::recursive_mutex> lck(_mutex);
4470  r.startPacket();
4471  r.addMessage (ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_PARAMS, axis);
4472  *((short *)(r._writeBuffer[0].getData()+1)) = S_16(stiff);
4473  *((short *)(r._writeBuffer[0].getData()+3)) = S_16(damp*1000);
4474  *((short *)(r._writeBuffer[0].getData()+5)) = S_16(0);
4475  *((char *)(r._writeBuffer[0].getData()+7)) = 0;
4476  r._writeBuffer[0].setLen(8);
4477  r.writePacket();
4478 
4479  //yDebug("stiffness is: %d \n", S_16(stiff));
4480  return true;
4481 }
4482 
4483 bool CanBusMotionControl::setTorqueSource (int axis, char board_id, char board_chan )
4484 {
4485  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4486  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4487  return false;
4488 
4489  DEBUG_FUNC("setTorqueSource\n");
4490 
4491  if (!ENABLED(axis))
4492  return true;
4493 
4495  std::lock_guard<std::recursive_mutex> lck(_mutex);
4496  r.startPacket();
4497  r.addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_SOURCE, axis);
4498  *((char *)(r._writeBuffer[0].getData()+1)) = board_id;
4499  *((char *)(r._writeBuffer[0].getData()+2)) = board_chan;
4500  *((char *)(r._writeBuffer[0].getData()+3)) = 0;
4501  *((char *)(r._writeBuffer[0].getData()+4)) = 0;
4502  *((char *)(r._writeBuffer[0].getData()+5)) = 0;
4503  *((char *)(r._writeBuffer[0].getData()+6)) = 0;
4504  *((char *)(r._writeBuffer[0].getData()+7)) = 0;
4505  r._writeBuffer[0].setLen(8);
4506  r.writePacket();
4507  return true;
4508 }
4509 
4511 {
4512  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4513  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4514  return false;
4515 
4516  DEBUG_FUNC("setImpedanceOffsetRaw\n");
4517 
4518  if (!ENABLED(axis))
4519  return true;
4520 
4522  std::lock_guard<std::recursive_mutex> lck(_mutex);
4523  r.startPacket();
4524  r.addMessage (ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_OFFSET, axis);
4525  *((short *)(r._writeBuffer[0].getData()+1)) = S_16(off);
4526  r._writeBuffer[0].setLen(3);
4527  r.writePacket();
4528  return true;
4529 }
4530 
4532 {
4533  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4534  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4535  return false;
4536 
4537  short s;
4538  short s2;
4539 
4540  DEBUG_FUNC("Calling GET_P_GAIN\n");
4541  _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_P_GAIN, axis, s); out->kp = double(s);
4542  DEBUG_FUNC("Calling CAN_GET_D_GAIN\n");
4543  _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_D_GAIN, axis, s); out->kd = double(s);
4544  DEBUG_FUNC("Calling CAN_GET_I_GAIN\n");
4545  _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_I_GAIN, axis, s); out->ki = double(s);
4546  DEBUG_FUNC("Calling CAN_GET_ILIM_GAIN\n");
4547  _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_ILIM_GAIN, axis, s); out->max_int = double(s);
4548  DEBUG_FUNC("Calling CAN_GET_OFFSET\n");
4549  _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_OFFSET, axis, s); out->offset= double(s);
4550  DEBUG_FUNC("Calling CAN_GET_SCALE\n");
4551  _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_SCALE, axis, s); out->scale = double(s);
4552  DEBUG_FUNC("Calling CAN_GET_TLIM\n");
4553  _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_TLIM, axis, s); out->max_output = double(s);
4554  DEBUG_FUNC("Calling CAN_GET_POS_STICTION_PARAMS\n");
4555  _readWord16Ex (ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS, axis, s, s2 ); out->stiction_up_val = double(s); out->stiction_down_val = double(s2);
4556  DEBUG_FUNC("Get PID done!\n");
4557 
4558  return true;
4559 }
4560 
4561 bool CanBusMotionControl::getPidRaw (const PidControlTypeEnum& pidtype, int axis, Pid *pid)
4562 {
4563  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
4564  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4565  return false;
4566 
4567  switch (pidtype)
4568  {
4569  case VOCAB_PIDTYPE_POSITION:
4570  helper_getPosPidRaw(axis,pid);
4571  break;
4572  case VOCAB_PIDTYPE_VELOCITY:
4573  helper_getVelPidRaw(axis,pid);
4574  break;
4575  case VOCAB_PIDTYPE_CURRENT:
4576  helper_getCurPidRaw(axis,pid);
4577  break;
4578  case VOCAB_PIDTYPE_TORQUE:
4579  helper_getTrqPidRaw(axis,pid);
4580  break;
4581  default:
4582  yError()<<"Invalid pidtype:"<<pidtype;
4583  break;
4584  }
4585  return true;
4586 }
4587 
4588 bool CanBusMotionControl::getPidsRaw (const PidControlTypeEnum& pidtype, Pid *pids)
4589 {
4591 
4592  int i;
4593  for (i = 0; i < r.getJoints(); i++)
4594  {
4595  getPidRaw(pidtype,i,&pids[i]);
4596  }
4597 
4598  return true;
4599 }
4600 
4601 bool CanBusMotionControl::helper_setTrqPidRaw(int axis, const Pid &pid)
4602 {
4605 
4606  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4607  return false;
4608 
4609  DEBUG_FUNC("setTorquePidRaw\n");
4610 
4611  if (!ENABLED(axis))
4612  return true;
4613 
4614  _mutex.lock();
4615  r.startPacket();
4616  r.addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PID, axis);
4617  *((short *)(r._writeBuffer[0].getData()+1)) = S_16(pid.kp);
4618  *((short *)(r._writeBuffer[0].getData()+3)) = S_16(pid.ki);
4619  *((short *)(r._writeBuffer[0].getData()+5)) = S_16(pid.kd);
4620  *((short *)(r._writeBuffer[0].getData()+7)) = S_16(pid.scale);
4621  r._writeBuffer[0].setLen(8);
4622  r.writePacket();
4623  r.startPacket();
4624  r.addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PIDLIMITS, axis);
4625  *((short *)(r._writeBuffer[0].getData()+1)) = S_16(pid.offset);
4626  *((short *)(r._writeBuffer[0].getData()+3)) = S_16(pid.max_output);
4627  *((short *)(r._writeBuffer[0].getData()+5)) = S_16(pid.max_int);
4628  *((short *)(r._writeBuffer[0].getData()+7)) = S_16(0);
4629  r._writeBuffer[0].setLen(8);
4630  r.writePacket();
4631  _mutex.unlock();
4632  _writeWord16Ex (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS, axis, S_16(pid.stiction_up_val), S_16(pid.stiction_down_val), false);
4633  _mutex.lock();
4634  r.startPacket();
4635  r.addMessage (ICUBCANPROTO_POL_MC_CMD__SET_MODEL_PARAMS, axis);
4636  *((short *)(r._writeBuffer[0].getData()+1)) = S_16(pid.kff);
4637  *((short *)(r._writeBuffer[0].getData()+3)) = S_16(0);
4638  *((short *)(r._writeBuffer[0].getData()+5)) = S_16(0);
4639  *((short *)(r._writeBuffer[0].getData()+7)) = S_16(0);
4640  r._writeBuffer[0].setLen(8);
4641  r.writePacket();
4642  _mutex.unlock();
4643  return true;
4644 }
4645 
4647 {
4648  DEBUG_FUNC("Calling CAN_GET_TORQUE_PID \n");
4649 
4651  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4652  return false;
4653 
4654 
4655 
4656  if (!ENABLED(axis))
4657  {
4658  //@@@ TODO: check here
4659  // value = 0;
4660  return true;
4661  }
4662 
4663  _mutex.lock();
4664  int id;
4665  if (!threadPool->getId(id))
4666  {
4667  yError("More than %d threads, cannot allow more\n", CANCONTROL_MAX_THREADS);
4668  _mutex.unlock();
4669  return false;
4670  }
4671 
4672  r.startPacket();
4673  r.addMessage (id, axis, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID);
4674  r.writePacket();
4675 
4677  t->setPending(r._writeMessages);
4678  _mutex.unlock();
4679  t->synch();
4680 
4681  if (!r.getErrorStatus() || (t->timedOut()))
4682  {
4683  yError("getTorquePid: message timed out\n");
4684  //@@@ TODO: check here
4685  // value=0;
4686  return false;
4687  }
4688 
4689  CanMessage *m=t->get(0);
4690  if (m==0)
4691  {
4692  //@@@ TODO: check here
4693  // value=0;
4694  return false;
4695  }
4696 
4697  unsigned char *data;
4698  data=m->getData()+1;
4699  out->kp= *((short *)(data));
4700  data+=2;
4701  out->ki= *((short *)(data));
4702  data+=2;
4703  out->kd= *((short *)(data));
4704  data+=2;
4705  out->scale= *((char *)(data));
4706 
4707  t->clear();
4708 
4709  _mutex.lock();
4710 
4711  DEBUG_FUNC("Calling CAN_GET_TORQUE_PIDLIMITS\n");
4712 
4713  r.startPacket();
4714  r.addMessage (id, axis, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS);
4715  r.writePacket();
4716 
4717  // ThreadTable2 *t=threadPool->getThreadTable(id);
4718  t->setPending(r._writeMessages);
4719  _mutex.unlock();
4720  t->synch();
4721 
4722  if (!r.getErrorStatus() || (t->timedOut()))
4723  {
4724  yError("getTorquePidLimits: message timed out\n");
4725  //@@@ TODO: check here
4726  // value=0;
4727  return false;
4728  }
4729 
4730  m=t->get(0);
4731  if (m==0)
4732  {
4733  //@@@ TODO: check here
4734  // value=0;
4735  return false;
4736  }
4737 
4738  data=m->getData()+1;
4739  out->offset= *((short *)(data));
4740  data+=2;
4741  out->max_output= *((short *)(data));
4742  data+=2;
4743  out->max_int= *((short *)(data));
4744 
4745  t->clear();
4746 
4747  _mutex.lock();
4748 
4749  DEBUG_FUNC("Calling CAN_GET_MODEL_PARAMS\n");
4750 
4751  r.startPacket();
4752  r.addMessage (id, axis, ICUBCANPROTO_POL_MC_CMD__GET_MODEL_PARAMS);
4753  r.writePacket();
4754 
4755  // ThreadTable2 *t=threadPool->getThreadTable(id);
4756  t->setPending(r._writeMessages);
4757  _mutex.unlock();
4758  t->synch();
4759 
4760  if (!r.getErrorStatus() || (t->timedOut()))
4761  {
4762  yError("CAN_GET_MODEL_PARAMS: message timed out\n");
4763  //@@@ TODO: check here
4764  // value=0;
4765  return false;
4766  }
4767 
4768  m=t->get(0);
4769  if (m==0)
4770  {
4771  //@@@ TODO: check here
4772  // value=0;
4773  return false;
4774  }
4775 
4776  data=m->getData()+1;
4777  out->kff= *((short *)(data));
4778 
4779  t->clear();
4780 
4781  DEBUG_FUNC("Calling CAN_GET_TORQUE_STICTION_PARAMS\n");
4782  short s1;
4783  short s2;
4784  _readWord16Ex (ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS, axis, s1, s2 );
4785  out->stiction_up_val = double(s1);
4786  out->stiction_down_val = double(s2);
4787 
4788  return true;
4789 }
4790 
4791 bool CanBusMotionControl::setPidsRaw(const PidControlTypeEnum& pidtype, const Pid *pids)
4792 {
4794  if (pids==0) return false;
4795 
4796  int i;
4797  for (i = 0; i < r.getJoints(); i++)
4798  {
4799  setPidRaw(pidtype, i, pids[i]);
4800  }
4801 
4802  return true;
4803 }
4804 
4806 bool CanBusMotionControl::setPidReferenceRaw (const PidControlTypeEnum& pidtype, int j, double ref)
4807 {
4808  return NOT_YET_IMPLEMENTED("setPidReferenceRaw");
4809 }
4810 
4812 bool CanBusMotionControl::setPidReferencesRaw (const PidControlTypeEnum& pidtype, const double *refs)
4813 {
4815  if (refs==0) return false;
4816 
4817  int i=0;
4818  bool ret = true;
4819  for (i = 0; i < r.getJoints(); i++)
4820  {
4821  ret = ret & setPidReferenceRaw(pidtype,i,refs[i]);
4822  }
4823 
4824  return ret;
4825 }
4826 
4828 bool CanBusMotionControl::setRefTorqueRaw (int j, double ref_trq)
4829 {
4830  const int axis = j;
4831  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4832  return false;
4833 
4834  //I'm sending a DWORD but the value MUST be clamped to S_16. Do not change.
4835  return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE, axis, S_16(ref_trq));
4836 }
4837 
4838 bool CanBusMotionControl::setRefTorquesRaw(const int n_joint, const int *joints, const double *t)
4839 {
4840  bool ret = true;
4841  for(int j=0; j< n_joint; j++)
4842  {
4843  ret &= setRefTorqueRaw(joints[j], t[j]);
4844  }
4845  return ret;
4846 }
4847 
4849 bool CanBusMotionControl::getTorqueRaw (int j, double *trq)
4850 {
4852  const int axis = j;
4853  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4854  return false;
4855 
4856  int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4857  std::lock_guard<std::recursive_mutex> lck(_mutex);
4858  *trq = double(r._bcastRecvBuffer[k]._torque);
4859  return true;
4860 }
4861 
4863 bool CanBusMotionControl::setRefTorquesRaw (const double *ref_trqs)
4864 {
4866 
4867  int i;
4868  for (i = 0; i < r.getJoints(); i++)
4869  {
4870  //I'm sending a DWORD but the value MUST be clamped to S_16. Do not change.
4871  if (_writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE, i, S_16(ref_trqs[i])) != true)
4872  return false;
4873  }
4874 
4875  return true;
4876 }
4877 
4880 {
4882  bool ret = true;
4883  for (int j = 0; j < r.getJoints() && ret; j++)
4884  {
4885  ret &= getTorqueRaw(j, &trqs[j]);
4886  }
4887  return ret;
4888 }
4889 
4890 bool CanBusMotionControl::getTorqueRangeRaw (int j, double *min, double *max)
4891 {
4893  const int axis = j;
4894  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
4895  return false;
4896 
4897  std::lock_guard<std::recursive_mutex> lck(_mutex);
4898  // *** This method is implementented reading data without sending/receiving data from the Canbus ***
4899  *min=0; //set output to zero (default value)
4900  *max=0; //set output to zero (default value)
4901  int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4902 
4903  std::list<TBR_AnalogSensor *>::iterator it=analogSensors.begin();
4904  while(it!=analogSensors.end())
4905  {
4906  if (*it)
4907  {
4908  int id = (*it)->getId();
4909  int cfgId = _axisTorqueHelper->getTorqueSensorId(k);
4910  if (cfgId == 0)
4911  {
4912  *min=0;
4913  *max=0;
4914  break;
4915  }
4916  else if (cfgId==id)
4917  {
4920  break;
4921  }
4922  }
4923  it++;
4924  }
4925  return true;
4926 }
4927 
4929 {
4930  return NOT_YET_IMPLEMENTED("getTorqueRangesRaw");
4931 }
4932 
4933 bool CanBusMotionControl::getPidErrorRaw(const PidControlTypeEnum& pidtype, int axis, double *err)
4934 {
4936  if (!(axis >= 0 && axis <= r.getJoints()))
4937  return false;
4938 
4939  std::lock_guard<std::recursive_mutex> lck(_mutex);
4940  switch (pidtype)
4941  {
4942  case VOCAB_PIDTYPE_POSITION:
4943  *err = double(r._bcastRecvBuffer[axis]._position_error);
4944  break;
4945  case VOCAB_PIDTYPE_TORQUE:
4946  *err = double(r._bcastRecvBuffer[axis]._torque_error);
4947  break;
4948  case VOCAB_PIDTYPE_VELOCITY:
4949  *err = 0; //not yet implemented
4950  NOT_YET_IMPLEMENTED("getPidErrorRaw VOCAB_PIDTYPE_VELOCITY");
4951  break;
4952  case VOCAB_PIDTYPE_CURRENT:
4953  *err = 0; //not yet implemented
4954  NOT_YET_IMPLEMENTED("getPidErrorRaw VOCAB_PIDTYPE_CURRENT");
4955  break;
4956  }
4957  return true;
4958 }
4959 
4961 {
4963  if (!(axis >= 0 && axis <= r.getJoints()))
4964  return false;
4965  std::lock_guard<std::recursive_mutex> lck(_mutex);
4966  *(ref) = this->_ref_command_positions[axis];
4967  return true;
4968 }
4969 
4971 {
4973  bool ret = true;
4974  for (int j = 0; j < r.getJoints() && ret; j++)
4975  {
4976  ret &= getTargetPositionRaw(j, &ref[j]);
4977  }
4978  return ret;
4979 }
4980 
4981 bool CanBusMotionControl::getTargetPositionsRaw(int nj, const int * jnts, double *refs)
4982 {
4983  bool ret = true;
4984  for (int j = 0; j < nj && ret; j++)
4985  {
4986  ret &= getTargetPositionRaw(jnts[j], &refs[j]);
4987  }
4988  return ret;
4989 }
4990 
4991 bool CanBusMotionControl::getRefVelocityRaw(int axis, double *ref)
4992 {
4994  if (!(axis >= 0 && axis <= r.getJoints()))
4995  return false;
4996  std::lock_guard<std::recursive_mutex> lck(_mutex);
4997  *(ref) = this->_ref_command_speeds[axis];
4998  return true;
4999 }
5000 
5002 {
5004  bool ret = true;
5005  for (int j = 0; j < r.getJoints() && ret; j++)
5006  {
5007  ret &= getRefVelocityRaw(j, &ref[j]);
5008  }
5009  return ret;
5010 }
5011 
5012 bool CanBusMotionControl::getRefVelocitiesRaw(int nj, const int * jnts, double *refs)
5013 {
5014  bool ret = true;
5015  for (int j = 0; j < nj && ret; j++)
5016  {
5017  ret &= getRefVelocityRaw(jnts[j], &refs[j]);
5018  }
5019  return ret;
5020 }
5021 
5022 bool CanBusMotionControl::getRefPositionRaw(int axis, double *ref)
5023 {
5025  if (!(axis >= 0 && axis <= r.getJoints()))
5026  return false;
5027  *ref = _ref_positions[axis];
5028  return true;
5029 }
5030 
5032 {
5034  bool ret = true;
5035  for (int j = 0; j < r.getJoints() && ret; j++)
5036  {
5037  ret &= getRefPositionRaw(j, &ref[j]);
5038  }
5039  return ret;
5040 }
5041 
5042 bool CanBusMotionControl::getRefPositionsRaw(int nj, const int * jnts, double *refs)
5043 {
5044  bool ret = true;
5045  for (int j = 0; j < nj && ret; j++)
5046  {
5047  ret &= getRefPositionRaw(jnts[j], &refs[j]);
5048  }
5049  return ret;
5050 }
5051 
5052 bool CanBusMotionControl::getPidErrorsRaw(const PidControlTypeEnum& pidtype, double *errs)
5053 {
5055  for (int i = 0; i < r.getJoints(); i++)
5056  {
5057  errs[i] = getPidErrorRaw(pidtype, i, &errs[i]);
5058  }
5059  return true;
5060 }
5061 
5062 bool CanBusMotionControl::getParameterRaw(int axis, unsigned int type, double* value)
5063 {
5064  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
5065  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5066  return false;
5067 
5068  if (!ENABLED(axis))
5069  {
5070  //@@@ TODO: check here
5071  // value = 0;
5072  return true;
5073  }
5074 
5076  _mutex.lock();
5077  int id;
5078  if (!threadPool->getId(id))
5079  {
5080  yError("More than %d threads, cannot allow more\n", CANCONTROL_MAX_THREADS);
5081  _mutex.unlock();
5082  return false;
5083  }
5084 
5085  r.startPacket();
5086  r.addMessage (id, axis, type);
5087  r.writePacket();
5088 
5090  t->setPending(r._writeMessages);
5091  _mutex.unlock();
5092  t->synch();
5093 
5094  if (!r.getErrorStatus() || (t->timedOut()))
5095  {
5096  yError("getParameterRaw: message timed out\n");
5097  //@@@ TODO: check here
5098  // value=0;
5099  return false;
5100  }
5101 
5102  CanMessage *m=t->get(0);
5103  if (m==0)
5104  {
5105  //@@@ TODO: check here
5106  // value=0;
5107  return false;
5108  }
5109 
5110  unsigned char *data;
5111  data=m->getData()+1;
5112  *value= *((short *)(data));
5113 
5114  t->clear();
5115 
5116  return true;
5117 }
5118 
5119 bool CanBusMotionControl::getDebugParameterRaw(int axis, unsigned int index, double* value)
5120 {
5121  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
5122  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5123  return false;
5124 
5125  if (!ENABLED(axis))
5126  {
5127  //@@@ TODO: check here
5128  // value = 0;
5129  return true;
5130  }
5131 
5133  _mutex.lock();
5134  int id;
5135  if (!threadPool->getId(id))
5136  {
5137  yError("More than %d threads, cannot allow more\n", CANCONTROL_MAX_THREADS);
5138  _mutex.unlock();
5139  return false;
5140  }
5141 
5142  r.startPacket();
5143  r.addMessage (id, axis, ICUBCANPROTO_POL_MC_CMD__GET_DEBUG_PARAM);
5144  *((unsigned char *)(r._writeBuffer[0].getData()+1)) = index;
5145  r._writeBuffer[0].setLen(2);
5146  r.writePacket();
5147 
5149  t->setPending(r._writeMessages);
5150  _mutex.unlock();
5151  t->synch();
5152 
5153  if (!r.getErrorStatus() || (t->timedOut()))
5154  {
5155  yError("getDebugParameterRaw: message timed out\n");
5156  //@@@ TODO: check here
5157  // value=0;
5158  return false;
5159  }
5160 
5161  CanMessage *m=t->get(0);
5162  if (m==0)
5163  {
5164  //@@@ TODO: check here
5165  // value=0;
5166  return false;
5167  }
5168 
5169  unsigned char *data;
5170  data=m->getData()+1;
5171  *value= *((short *)(data));
5172 
5173  t->clear();
5174 
5175  return true;
5176 }
5177 
5179 {
5180  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5181  return false;
5182 
5183  int val = 0;
5184  if (_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION, axis, val) == true)
5185  *value = double (val);
5186  else
5187  return false;
5188 
5189  return true;
5190 }
5191 
5192 
5193 bool CanBusMotionControl::getFirmwareVersionRaw (int axis, can_protocol_info const& icub_interface_protocol, firmware_info* fw_info)
5194 {
5195  // ACE_ASSERT (axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2);
5196  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5197  return false;
5198 
5199  if (!ENABLED(axis))
5200  {
5201  //@@@ TODO: check here
5202  // value = 0;
5203  return true;
5204  }
5205 
5207  _mutex.lock();
5208  int id;
5209  if (!threadPool->getId(id))
5210  {
5211  yError("More than %d threads, cannot allow more\n", CANCONTROL_MAX_THREADS);
5212  _mutex.unlock();
5213  return false;
5214  }
5215 
5216  fw_info->network_name=this->canDevName;
5217  fw_info->joint=axis;
5218  fw_info->board_can_id=r._destinations[axis/2] & 0x0f;
5219  fw_info->network_number=r._networkN;
5220 
5221  r.startPacket();
5222  r.addMessage (id, axis, ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION);
5223  *((unsigned char *)(r._writeBuffer[0].getData()+1)) = (unsigned char)(icub_interface_protocol.major & 0xFF);
5224  *((unsigned char *)(r._writeBuffer[0].getData()+2)) = (unsigned char)(icub_interface_protocol.minor & 0xFF);
5225  r._writeBuffer[0].setLen(3);
5226  r.writePacket();
5227 
5229  t->setPending(r._writeMessages);
5230  _mutex.unlock();
5231  t->synch();
5232 
5233  if (!r.getErrorStatus() || (t->timedOut()))
5234  {
5235  yError("getFirmwareVersion: message timed out\n");
5236  fw_info->board_type= 0;
5237  fw_info->fw_major= 0;
5238  fw_info->fw_version= 0;
5239  fw_info->fw_build= 0;
5240  return false;
5241  }
5242 
5243  CanMessage *m=t->get(0);
5244  if (m==0)
5245  {
5246  yError("getFirmwareVersion: message error\n");
5247  fw_info->board_type= 0;
5248  fw_info->fw_major= 0;
5249  fw_info->fw_version= 0;
5250  fw_info->fw_build= 0;
5251  return false;
5252  }
5253 
5254  unsigned char *data;
5255  data=m->getData()+1;
5256  fw_info->board_type= *((char *)(data));
5257  data+=1;
5258  fw_info->fw_major= *((char *)(data));
5259  data+=1;
5260  fw_info->fw_version= *((char *)(data));
5261  data+=1;
5262  fw_info->fw_build= *((char *)(data));
5263  data+=1;
5264  fw_info->can_protocol.major = *((char *)(data));
5265  data+=1;
5266  fw_info->can_protocol.minor = *((char *)(data));
5267  data+=1;
5268  fw_info->ack = *((char *)(data));
5269  t->clear();
5270 
5271  return true;
5272 }
5273 
5274 bool CanBusMotionControl::getPidReferenceRaw(const PidControlTypeEnum& pidtype, int j, double *ref)
5275 {
5276  const int axis = j;
5277  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5278  {
5279  return false;
5280  }
5281 
5282  switch (pidtype)
5283  {
5284  case VOCAB_PIDTYPE_POSITION:
5285  {
5286  int value = 0;
5287  if (_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION, axis, value) == true)
5288  { *ref = double (value);}
5289  else
5290  { *ref =0; yError() << "Invalid _readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION)"; return false; }
5291  }
5292  break;
5293  case VOCAB_PIDTYPE_VELOCITY:
5294  {
5295  *ref=0;
5296  NOT_YET_IMPLEMENTED("getPidReferenceRaw VOCAB_PIDTYPE_VELOCITY");
5297  }
5298  break;
5299  case VOCAB_PIDTYPE_CURRENT:
5300  {
5301  *ref=0;
5302  NOT_YET_IMPLEMENTED("getPidReferenceRaw VOCAB_PIDTYPE_CURRENT");
5303  }
5304  break;
5305  case VOCAB_PIDTYPE_TORQUE:
5306  {
5307  *ref=0;
5308  NOT_YET_IMPLEMENTED("getPidReferenceRaw VOCAB_PIDTYPE_TORQUE");
5309  }
5310  break;
5311  default:
5312  {
5313  *ref=0;
5314  yError()<<"Invalid pidtype:"<<pidtype;
5315  }
5316  break;
5317  }
5318  return true;
5319 }
5320 
5321 bool CanBusMotionControl::getPidReferencesRaw(const PidControlTypeEnum& pidtype, double *refs)
5322 {
5323  bool ret = true;
5325  if (refs==0) return false;
5326 
5327  for (int i = 0; i < r.getJoints(); i++)
5328  {
5329  ret &= getPidReferenceRaw(pidtype, i, &refs[i]);
5330  }
5331  return ret;
5332 }
5333 
5334 bool CanBusMotionControl::getPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double *err)
5335 {
5336  return NOT_YET_IMPLEMENTED("getErrorLimit");
5337 }
5338 
5339 bool CanBusMotionControl::getPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, double *errs)
5340 {
5341  return NOT_YET_IMPLEMENTED("getErrorLimits");
5342 }
5343 
5344 bool CanBusMotionControl::setPidErrorLimitRaw(const PidControlTypeEnum& pidtype, int j, double limit)
5345 {
5346  return NOT_YET_IMPLEMENTED("setErrorLimit");
5347 }
5348 
5349 bool CanBusMotionControl::setPidErrorLimitsRaw(const PidControlTypeEnum& pidtype, const double *limits)
5350 {
5351  return NOT_YET_IMPLEMENTED("setErrorLimits");
5352 }
5353 
5354 bool CanBusMotionControl::resetPidRaw(const PidControlTypeEnum& pidtype, int j)
5355 {
5356  return NOT_YET_IMPLEMENTED("resetPid");
5357 }
5358 
5359 bool CanBusMotionControl::enablePidRaw(const PidControlTypeEnum& pidtype, int axis)
5360 {
5361  return NOT_YET_IMPLEMENTED("enablePidRaw");
5362 }
5363 
5364 bool CanBusMotionControl::setPidOffsetRaw(const PidControlTypeEnum& pidtype, int axis, double v)
5365 {
5366  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5367  return false;
5368 
5369  return _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_OFFSET, axis, S_16(v));
5370 
5371 }
5372 
5373 bool CanBusMotionControl::setParameterRaw(int axis, unsigned int type, double value)
5374 {
5375  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5376  return false;
5377 
5378  return _writeWord16 (type, axis, S_16(value));
5379 
5380 }
5381 
5382 bool CanBusMotionControl::setDebugParameterRaw(int axis, unsigned int index, double value)
5383 {
5384  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5385  return false;
5386 
5387 
5389  std::lock_guard<std::recursive_mutex> lck(_mutex);
5390  r.startPacket();
5391  r.addMessage (ICUBCANPROTO_POL_MC_CMD__SET_DEBUG_PARAM, axis);
5392  *((unsigned char *)(r._writeBuffer[0].getData()+1)) = (unsigned char)(index & 0xFF);
5393  *((short *)(r._writeBuffer[0].getData()+2)) = S_16(value);
5394  *((short *)(r._writeBuffer[0].getData()+4)) = 0;
5395  *((short *)(r._writeBuffer[0].getData()+6)) = 0;
5396  r._writeBuffer[0].setLen(8);
5397  r.writePacket();
5398  return true;
5399 }
5400 
5402 {
5403  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5404  return false;
5405 
5406  //return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_POSITION, axis, S_32(value));
5407  return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_COMMAND_POSITION, axis, S_32(value));
5408 }
5409 
5410 bool CanBusMotionControl::getPidOutputRaw(const PidControlTypeEnum& pidtype, int axis, double *out)
5411 {
5413  if (!(axis >= 0 && axis <= r.getJoints()))
5414  return false;
5415 
5416  std::lock_guard<std::recursive_mutex> lck(_mutex);
5417  switch (pidtype)
5418  {
5419  case VOCAB_PIDTYPE_POSITION:
5420  *(out) = double(r._bcastRecvBuffer[axis]._pid_value);
5421  break;
5422  case VOCAB_PIDTYPE_VELOCITY:
5423  *(out) = double(r._bcastRecvBuffer[axis]._pid_value);
5424  break;
5425  case VOCAB_PIDTYPE_CURRENT:
5426  *(out) = double(r._bcastRecvBuffer[axis]._pid_value);
5427  break;
5428  case VOCAB_PIDTYPE_TORQUE:
5429  *(out) = double(r._bcastRecvBuffer[axis]._pid_value);
5430  break;
5431  default:
5432  yError()<<"Invalid pidtype:"<<pidtype;
5433  break;
5434  }
5435  return true;
5436 }
5437 
5438 bool CanBusMotionControl::getPidOutputsRaw(const PidControlTypeEnum& pidtype, double *outs)
5439 {
5441  for (int i = 0; i < r.getJoints(); i++)
5442  {
5443  getPidOutput(pidtype, i, &outs[i]);
5444  }
5445  return true;
5446 }
5447 
5448 bool CanBusMotionControl::disablePidRaw(const PidControlTypeEnum& pidtype, int axis)
5449 {
5450  return NOT_YET_IMPLEMENTED("disablePidRaw");
5451 }
5452 
5453 bool CanBusMotionControl::positionMoveRaw(int axis, double ref)
5454 {
5457  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5458  return false;
5459 
5460  if (!ENABLED (axis))
5461  {
5462  // still fills the _ref_position structure.
5463  _ref_command_positions[axis] = ref;
5464  return true;
5465  }
5466 
5467  if (yarp::os::Time::now()-_last_position_move_time[axis]<MAX_POSITION_MOVE_INTERVAL)
5468  {
5469  yWarning() << "Performance warning: You are using positionMove commands at high rate (<"<< MAX_POSITION_MOVE_INTERVAL*1000.0 <<" ms). Probably position control mode is not the right control mode to use.";
5470  }
5471  _last_position_move_time[axis] = yarp::os::Time::now();
5472 
5473  int mode = 0;
5474  getControlModeRaw(axis, &mode);
5475  if (mode != VOCAB_CM_POSITION &&
5476  mode != VOCAB_CM_MIXED &&
5477  mode != VOCAB_CM_IMPEDANCE_POS &&
5478  mode != VOCAB_CM_IDLE)
5479  {
5480  yError() << "positionMoveRaw: skipping command because " << networkName.c_str() << " joint " << axis << "is not in VOCAB_CM_POSITION mode";
5481  return true;
5482  }
5483 
5484  std::lock_guard<std::recursive_mutex> lck(_mutex);
5485 
5486  r.startPacket();
5487  r.addMessage (ICUBCANPROTO_POL_MC_CMD__POSITION_MOVE, axis);
5488 
5489  _ref_command_positions[axis] = ref;
5490  *((int*)(r._writeBuffer[0].getData() + 1)) = S_32(_ref_command_positions[axis]);
5491  *((short*)(r._writeBuffer[0].getData() + 5)) = S_16(_ref_speeds[axis]);
5492  r._writeBuffer[0].setLen(7);
5493 
5494  r.writePacket();
5495  return true;
5496 }
5497 
5499 {
5501  int i = 0;
5502  if (refs == 0) return false;
5503  bool ret = true;
5504  for (i = 0; i < r.getJoints (); i++)
5505  {
5506  ret = ret & positionMoveRaw(i, refs[i]);
5507  }
5508 
5509  return ret;
5510 }
5511 
5512 bool CanBusMotionControl::relativeMoveRaw(int j, double delta)
5513 {
5514  return NOT_YET_IMPLEMENTED("positionRelative");
5515 }
5516 
5517 bool CanBusMotionControl::relativeMoveRaw(const double *deltas)
5518 {
5519  return NOT_YET_IMPLEMENTED("positionRelative");
5520 }
5521 
5524 {
5525  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5526  return false;
5527 
5528  short value;
5529 
5530  if (!_readWord16 (ICUBCANPROTO_POL_MC_CMD__MOTION_DONE, axis, value))
5531  {
5532  *ret=false;
5533  return false;
5534  }
5535 
5536  *ret= (value!=0);
5537 
5538  return true;
5539 }
5540 
5543 {
5545  int i;
5546  short value;
5547 
5548  _mutex.lock();
5549 
5550  int id;
5551  if (!threadPool->getId(id))
5552  {
5553  yError("More than %d threads, cannot allow more\n", CANCONTROL_MAX_THREADS);
5554  _mutex.unlock();
5555  return false;
5556  }
5557 
5558  r.startPacket();
5559  for (i = 0; i < r.getJoints(); i++)
5560  {
5561  if (ENABLED(i))
5562  {
5563  r.addMessage (id, i, ICUBCANPROTO_POL_MC_CMD__MOTION_DONE);
5564  }
5565  }
5566 
5567  if (r._writeMessages < 1)
5568  {
5569  _mutex.unlock();
5570  return false;
5571  }
5572 
5573  r.writePacket(); //write immediatly
5574 
5576  t->setPending(r._writeMessages);
5577  _mutex.unlock();
5578  t->synch();
5579 
5580  if (!r.getErrorStatus())
5581  return false;
5582 
5583  int j;
5584  for (i = 0, j = 0; i < r.getJoints(); i++)
5585  {
5586  if (ENABLED(i))
5587  {
5588  CanMessage *m = t->getByJoint(i, r._destInv);
5589  if ( (m!=0) && (m->getId() != 0xffff) )
5590  {
5591  value = *((short *)(m->getData()+1));
5592  if (!value)
5593  {
5594  *val=false;
5595  return true;
5596  }
5597  }
5598  j++;
5599  }
5600  }
5601 
5602  t->clear();
5603 
5604  *val=true;
5605  return true;
5606 }
5607 
5608 bool CanBusMotionControl::calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3)
5609 {
5610  bool b = _writeByteWords16(ICUBCANPROTO_POL_MC_CMD__CALIBRATE_ENCODER, axis, type, S_16(p1), S_16(p2), S_16(p3));
5611  return b;
5612 }
5613 
5614 bool CanBusMotionControl::setCalibrationParametersRaw(int j, const CalibrationParameters& params)
5615 {
5616  return calibrateAxisWithParamsRaw(j, params.type, params.param1, params.param2, params.param3);
5617 }
5618 
5619 bool CanBusMotionControl::setRefSpeedRaw(int axis, double sp)
5620 {
5621  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5622  return false;
5623 
5624  sp /= 10.0; // encoder ticks per ms
5625  _ref_speeds[axis] = sp;
5626  return true;
5627 }
5628 
5630 {
5632  memcpy(_ref_speeds, spds, sizeof(double)* r.getJoints());
5633  int i;
5634  for (i = 0; i < r.getJoints(); i++)
5635  _ref_speeds[i] /= 10.0;
5636 
5637  return true;
5638 }
5639 
5641 {
5642  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5643  return false;
5644 
5645  /*
5646  *Acceleration is expressed in imp/s^2.
5647  *We divided it by 1000^2 to express
5648  *it in imp/ms^2
5649  */
5650  acc /= 1000.0;
5651  acc /= 1000.0;
5652  _ref_accs[axis] = acc;
5653  const short s = S_16(_ref_accs[axis]);
5654 
5655  return _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_ACCELER, axis, s);
5656 }
5657 
5659 {
5661 
5662  int i;
5663  for (i = 0; i < r.getJoints(); i++)
5664  {
5665  /*
5666  *Acceleration is expressed in imp/s^2.
5667  *We divided it by 1000^2 to express
5668  *it in imp/ms^2
5669  */
5670  double acc;
5671  acc = accs[i]/1000.0;
5672  acc /= 1000.0;
5673  _ref_accs[i] = acc;
5674 
5675  if (!_writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_ACCELER, i, S_16(_ref_accs[i])))
5676  return false;
5677  }
5678 
5679  return true;
5680 }
5681 
5684 {
5686 
5687  memcpy(spds, _ref_speeds, sizeof(double)* r.getJoints());
5688  int i;
5689  for (i = 0; i < r.getJoints(); i++)
5690  spds[i] *= 10.0;
5691 
5692  return true;
5693 }
5694 
5695 bool CanBusMotionControl::getRefSpeedRaw (int axis, double *spd)
5696 {
5697  if (!(axis >= 0 && axis <= (CAN_MAX_CARDS-1)*2))
5698  return false;
5699  *spd = _ref_speeds[axis] * 10.0;
5700 
5701  return true;
5702 }
5703 
5706 {
5707  CanBusResources& r = RES(