ElmoCtrl.cpp
Go to the documentation of this file.
1 /****************************************************************
2  *
3  * Copyright (c) 2010
4  *
5  * Fraunhofer Institute for Manufacturing Engineering
6  * and Automation (IPA)
7  *
8  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
9  *
10  * Project name: care-o-bot
11  * ROS stack name: cob3_driver
12  * ROS package name: cob_head_axis
13  *
14  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15  *
16  * Author: Ulrich Reiser, email:ulrich.reiser@ipa.fhg.de
17  *
18  * Date of creation: Jul 2010
19  * ToDo:
20  *
21  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22  *
23  * Redistribution and use in source and binary forms, with or without
24  * modification, are permitted provided that the following conditions are met:
25  *
26  * * Redistributions of source code must retain the above copyright
27  * notice, this list of conditions and the following disclaimer.
28  * * Redistributions in binary form must reproduce the above copyright
29  * notice, this list of conditions and the following disclaimer in the
30  * documentation and/or other materials provided with the distribution.
31  * * Neither the name of the Fraunhofer Institute for Manufacturing
32  * Engineering and Automation (IPA) nor the names of its
33  * contributors may be used to endorse or promote products derived from
34  * this software without specific prior written permission.
35  *
36  * This program is free software: you can redistribute it and/or modify
37  * it under the terms of the GNU Lesser General Public License LGPL as
38  * published by the Free Software Foundation, either version 3 of the
39  * License, or (at your option) any later version.
40  *
41  * This program is distributed in the hope that it will be useful,
42  * but WITHOUT ANY WARRANTY; without even the implied warranty of
43  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
44  * GNU Lesser General Public License LGPL for more details.
45  *
46  * You should have received a copy of the GNU Lesser General Public
47  * License LGPL along with this program.
48  * If not, see <http://www.gnu.org/licenses/>.
49  *
50  ****************************************************************/
51 
52 
53 #include <cob_head_axis/ElmoCtrl.h>
54 #include <unistd.h>
55 #include <iostream>
56 #include <unistd.h>
57 
58 using namespace std;
59 
60 void Sleep(int msecs){usleep(1000*msecs);}
61 
62 bool ElmoCtrl::sendNetStartCanOpen(CanItf* canCtrl) {
63  bool ret = false;
64 
65  CanMsg msg;
66 
67  msg.m_iID = 0;
68  msg.m_iLen = 2;
69  msg.set(1,0,0,0,0,0,0,0);
70  ret = canCtrl->transmitMsg(msg, false);
71 
72  usleep(100000);
73 
74  return ret;
75 }
76 
77 
78 ElmoCtrl::ElmoCtrl() {
79  m_Joint = NULL;
80  m_JointParams = NULL;
81  m_CanCtrl = NULL;
82  m_CanBaseAddress = 0;
83  m_MotionCtrlType = POS_CTRL;
84  m_MaxVel = 2.0;
85  m_Params = NULL;
86 }
87 
88 ElmoCtrl::~ElmoCtrl() {
89  if (m_Joint)
90  m_Joint->shutdown();
91  delete m_Joint;
92  if (m_JointParams)
93  delete m_JointParams;
94  if (m_CanCtrl)
95  delete m_CanCtrl;
96 }
97 
98 bool ElmoCtrl::Home()
99 {
100  bool success = false;
101  if (m_Joint != NULL) {
102  //THIS is needed for head_axis on cob3-2!
103  //set input logic to 'general purpose'
104  m_Joint->IntprtSetInt(8, 'I', 'L', 2, 7);
105  usleep(20000);
106 
107  //THIS is needed for cob3-3 (it's using DIN1):
108  //set input logic to 'general purpose'
109  m_Joint->IntprtSetInt(8, 'I', 'L', 1, 6);
110  usleep(20000);
111 
112  m_Joint->initHoming();
113  }
114 
115  //ToDo: UHR: necessary?
116  Sleep(10);
117  printf("ElmoCtrl: Home(): Homing Vel = %f\n",m_HomingDir*0.3);
118  m_Joint->setGearVelRadS(m_HomingDir*0.3);
119  //cpc-pk: removed this sleep, it's not necessary to leave home-sensor during homing
120  //Sleep(750);
121  success = m_Joint->execHoming();
122  m_Joint->setGearVelRadS(0.0);
123 
124  return success;
125 }
126 
127 
128 int ElmoCtrl::evalCanBuffer() {
129  bool bRet;
130 
131  //pthread_mutex_lock(&(m_Mutex));
132 
133  // as long as there is something in the can buffer -> read out next message
134  while(m_CanCtrl->receiveMsg(&m_CanMsgRec) == true) {
135  bRet = false;
136  // check if the message belongs to head_axis motor
137  bRet |= m_Joint->evalReceivedMsg(m_CanMsgRec);
138 
139  if (bRet == true) {
140  } else std::cout << "cob_head_axis: Unknown CAN-msg: " << m_CanMsgRec.m_iID << " " << (int)m_CanMsgRec.getAt(0) << " " << (int)m_CanMsgRec.getAt(1) << std::endl;
141  }
142 
143  //pthread_mutex_unlock(&(m_Mutex));
144 
145  return 0;
146 }
147 
148 bool ElmoCtrl::RecoverAfterEmergencyStop() {
149 
150  bool success = false;
151  printf("Resetting motor ...\n");
152  success = m_Joint->start();
153  if (!success) {
154  printf("failed!\n");
155  } else {
156  printf("successful\n");
157  m_Joint->setGearVelRadS(0);
158  }
159  Sleep(1000);
160  return success;
161 }
162 
163 
164 bool ElmoCtrl::Init(ElmoCtrlParams * params, bool home) { //home = true by default
165  bool success = false;
166  string CanIniFile;
167  string CanDevice;
168  int baudrate = 0;
169 
170  m_Params = params;
171 
172  if (params == NULL) {
173  printf("ElmoCtrlParams:Error:%s:%d:, invalid parameters!\n",__FILE__,__LINE__);
174  success = false;
175  } else {
176  success = true;
177  }
178 
179  if (success) {
180  printf( "------------ ElmoCtrl Init ---------------\n");
181 
182  //Allocate memory and read params e.g. gotten from ROS parameter server
183  m_Joint = new CanDriveHarmonica();
184  m_JointParams = new DriveParam();
185  m_CanBaseAddress = params->GetModuleID();
186  CanIniFile = params->GetCanIniFile();
187  m_MaxVel = params->GetMaxVel();
188  m_HomingDir = params->GetHomingDir();
189  m_HomingDigIn = params->GetHomingDigIn();
190 
191  m_EncIncrPerRevMot = params->GetEncoderIncrements();
192  m_MotorDirection = params->GetMotorDirection();
193  m_GearRatio = params->GetGearRatio();
194 
195  if (CanIniFile.length() == 0) {
196  printf("%s,%d:Error: Parameter 'CanIniFile' not given!\n",__FILE__,__LINE__);
197  success = false;
198  }
199 
200  CanDevice = params->GetCanDevice();
201  if (CanDevice.length() == 0) {
202  printf("%s,%d:Error: Parameter 'Can-Device' not given!\n",__FILE__,__LINE__);
203  success = false;
204  }
205 
206  baudrate = params->GetBaudRate();
207  if (baudrate == 0) {
208  printf("%s,%d:Error: Parameter 'Baud-Rate' not given!\n",__FILE__,__LINE__);
209  success = false;
210  }
211 
212  //Setting motor model data
213  if (success) {
214  m_JointOffset = params->GetAngleOffset();
215  m_UpperLimit = params->GetUpperLimit();
216  m_LowerLimit = params->GetLowerLimit();
217  }
218 
219  if (success) {
220  printf("The following parameters were successfully read from the parameter server (given through *params): \n");
221  printf("CanIniFile: %s\n",CanIniFile.c_str());
222  printf("CanDieODvice: %s\n",CanDevice.c_str());
223  printf("Baudrate: %d\n",baudrate);
224  printf("Module ID: %d\n",m_CanBaseAddress);
225  printf("Max Vel: %f\n",m_MaxVel);
226  printf("Homing Dir: %d\n",m_HomingDir);
227  printf("HomingDigIn: %d\n",m_HomingDigIn);
228  printf("Offset/Limit(min/max) %f/(%f,%f)\n",m_JointOffset,m_LowerLimit,m_UpperLimit);
229  }
230  }
231 
232  //Setting up CAN interface
233 
234  if (success) {
235  //m_CanCtrl = new CanESD(CanIniFile.c_str(), false);
236  m_CanCtrl = new CANPeakSysUSB(CanIniFile.c_str());
237  if (m_CanCtrl == NULL) {
238  printf("%s,%d:Error: Could not open Can Device!\n",__FILE__,__LINE__);
239  success = false;
240  }
241  }
242 
243  if (success) {
244  /* WRONG CAN-identifiers
245  //m_CanBaseAddress = params->GetModulID(i);
246  m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1;
247  m_CanAddress.TxPDO2 = 0x285 + m_CanBaseAddress -1;
248  m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1;
249  m_CanAddress.TxSDO = 0x491 + m_CanBaseAddress -1;
250  m_CanAddress.RxSDO = 0x511 + m_CanBaseAddress -1;
251  */
252  m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1;
253  m_CanAddress.TxPDO2 = 0x281 + m_CanBaseAddress -1;
254  m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1;
255  m_CanAddress.TxSDO = 0x581 + m_CanBaseAddress -1;
256  m_CanAddress.RxSDO = 0x601 + m_CanBaseAddress -1;
257  m_Joint->setCanItf(m_CanCtrl);
258  m_Joint->setCanOpenParam(m_CanAddress.TxPDO1,
259  m_CanAddress.TxPDO2,
260  m_CanAddress.RxPDO2,
261  m_CanAddress.TxSDO,
262  m_CanAddress.RxSDO );
263 
264  printf("CanAdresses set to %d (Base), %x, %x, %x, %x, %x...\n", m_CanBaseAddress,
265  m_CanAddress.TxPDO1,
266  m_CanAddress.TxPDO2,
267  m_CanAddress.RxPDO2,
268  m_CanAddress.TxSDO,
269  m_CanAddress.RxSDO);
270 
271  }
272 
273  if (success) {
274  success = sendNetStartCanOpen(m_CanCtrl);
275  }
276 
277  if (success) {
278  //ToDo: Read from File!
279  /*
280  int iDriveIdent,
281  int iEncIncrPerRevMot,
282  double dVelMeasFrqHz,
283  double dBeltRatio,
284  double dGearRatio,
285  int iSign,
286  double dVelMaxEncIncrS,
287  double dAccIncrS2,
288  double dDecIncrS2,
289  int iEncOffsetIncr,
290  bool bIsSteer,
291  double dCurrToTorque,
292  double dCurrMax,
293  int iHomingDigIn
294  */
295 
296 /*
297  m_JointParams->setParam( //parameters taken from CanCtrl.ini
298  0, //int iDriveIdent,
299  4096,//int iEncIncrPerRevMot,
300  1,//double dVelMeasFrqHz,
301  1,//double dBeltRatio,
302  47.77,//double dGearRatio,
303  -1.0,//int iSign,
304  740000,//double dVelMaxEncIncrS,
305  1000000,//80000,//double dAccIncrS2,
306  1000000,//80000//double dDecIncrS2),
307  0, //int iEncOffsetIncr
308  true, // bool bIsSteer
309  0, // double dCurrToTorque
310  0, // double dCurrMax
311  m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9
312  );
313 */
314 
315  m_JointParams->setParam( //parameters taken from CanCtrl.ini
316  0, //int iDriveIdent,
317  m_EncIncrPerRevMot,//int iEncIncrPerRevMot,
318  1,//double dVelMeasFrqHz,
319  1,//double dBeltRatio,
320  m_GearRatio,//double dGearRatio,
321  m_MotorDirection,//int iSign,
322  74000000,//double dVelMaxEncIncrS,
323  1000000,//80000,//double dAccIncrS2,
324  1000000,//80000//double dDecIncrS2),
325  0, //int iEncOffsetIncr
326  true, // bool bIsSteer
327  0, // double dCurrToTorque
328  0, // double dm_HomingDigInCurrMax
329  m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9
330  );
331 
332 
333  m_Joint->setDriveParam(*m_JointParams);
334  }
335 
336  if (success)
337  {
338  printf("Init motor ...\n");
339  success = m_Joint->init();
340  if (!success)
341  {
342  printf("failed!\n");
343  }
344  else
345  {
346  printf("successful\n");
347  }
348  }
349 
350  if (success)
351  success = SetMotionCtrlType(m_MotionCtrlType);
352  if(!success) std::cout << "Failed to SetMotionCtrlType to " << m_MotionCtrlType << std::endl;
353 
354  if (success)
355  {
356  printf("Starting motor ..\n");
357  success = m_Joint->start();
358  if (!success)
359  {
360  printf("failed!\n");
361  }
362  else
363  {
364  printf("successful\n");
365  m_Joint->setGearVelRadS(0);
366  }
367  }
368  //ToDo: UHR: necessary?
369  Sleep(1000);
370 
371  if (success && home)
372  {
373  std::cout << "Start homing procedure now.." << std::endl;
374  success = Home();
375  }
376  //Thread init
377  if (success)
378  {
379  pthread_mutex_init(&m_Mutex,NULL);
380  }
381 
382 
383  return success;
384 }
385 
386 bool ElmoCtrl::SetMotionCtrlType(int type)
387 {
388  m_MotionCtrlType = type;
389  bool success = false;
390  if (type == POS_CTRL)
391  {
392  success = m_Joint->shutdown();
393  if (success)
394  success = m_Joint->setTypeMotion(CanDriveHarmonica::MOTIONTYPE_POSCTRL);
395 
396  //ToDo: necessary?
397  Sleep(100);
398  success = m_Joint->start();
399 
400  }
401  else if (type == VEL_CTRL)
402  {
403  //UHR: ToDo
404  printf("%s%d:Error: Velocity control not implemented yet!\n",__FILE__,__LINE__);
405  success = false;
406  }
407  return success;
408 };
409 
410 
411 int ElmoCtrl::GetMotionCtrlType()
412 {
413  return m_MotionCtrlType;
414 }
415 
416 bool ElmoCtrl::isError()
417 {
418  return m_Joint->isError();
419 }
420 
421 int ElmoCtrl::getGearPosVelRadS( double* pdAngleGearRad, double* pdVelGearRadS)
422 {
423 
424  // init default outputs
425  *pdAngleGearRad = 0;
426  *pdVelGearRadS = 0;
427 
428  m_Joint->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS);
429  *pdAngleGearRad = *pdAngleGearRad - m_JointOffset;
430 
431  return 0;
432 }
433 
434 //-----------------------------------------------
435 
436 int ElmoCtrl:: setGearPosVelRadS(double dPosRad, double dVelRadS)
437 {
438 
439  if(dPosRad< m_LowerLimit) {
440  std::cout << "Position under LowerBound -> set up" << std::endl;
441  dPosRad = m_LowerLimit;
442  } else if(dPosRad > m_UpperLimit) {
443  std::cout << "Position over UpperBound -> set down" << std::endl;
444  dPosRad = m_UpperLimit;
445  }
446 
447  if(dVelRadS > m_MaxVel)
448  dVelRadS = m_MaxVel;
449  else if(dVelRadS < -m_MaxVel)
450  dVelRadS = -m_MaxVel;
451 
452  //COB3-2: m_Joint->setGearPosVelRadS(dPosRad + m_JointOffset, dVelRadS);
453 
454  //COB3-1: m_Joint->setGearPosVelRadS(-3.141592654 - dPosRad + m_JointOffset, dVelRadS);
455  printf("ElmoCtrl:setGearPosVelRadS: dPosRad %f with vel %f\n",dPosRad, dVelRadS);
456  m_Joint->setGearPosVelRadS(m_HomingDir * dPosRad + m_JointOffset, dVelRadS);
457  return 0;
458 }
459 
460 bool ElmoCtrl::Stop()
461 {
462  //UHR: ToDo: what happens exactly in this method? Sudden stop?
463  double pos = 0.0;
464  double vel = 0.0;
465  m_Joint->getGearPosVelRadS(&pos,&vel);
466  m_Joint->setGearPosVelRadS(pos,0);
467 
468  return true;
469  //m_Joint[i]->shutdown();
470 
471 }
ElmoCtrlParams::GetAngleOffset
double GetAngleOffset()
Definition: ElmoCtrl.h:118
ElmoCtrl::GetMotionCtrlType
int GetMotionCtrlType()
Definition: ElmoCtrl.cpp:411
ElmoCtrl::Init
bool Init(ElmoCtrlParams *params, bool home=true)
Definition: ElmoCtrl.cpp:164
ElmoCtrlParams::GetMaxVel
double GetMaxVel()
Definition: ElmoCtrl.h:122
ElmoCtrl::getGearPosVelRadS
int getGearPosVelRadS(double *pdAngleGearRad, double *pdVelGearRadS)
Definition: ElmoCtrl.cpp:421
ElmoCtrlParams::GetModuleID
int GetModuleID()
Definition: ElmoCtrl.h:109
ElmoCtrlParams::GetHomingDigIn
int GetHomingDigIn()
Definition: ElmoCtrl.h:130
ElmoCtrl::sendNetStartCanOpen
bool sendNetStartCanOpen(CanItf *canCtrl)
Definition: ElmoCtrl.cpp:62
ElmoCtrl::setGearPosVelRadS
int setGearPosVelRadS(double dPosRad, double dVelRadS)
Definition: ElmoCtrl.cpp:436
ElmoCtrl::Stop
bool Stop()
Definition: ElmoCtrl.cpp:460
CanMsg::m_iLen
int m_iLen
ElmoCtrl::~ElmoCtrl
~ElmoCtrl()
Definition: ElmoCtrl.cpp:88
ElmoCtrlParams::GetLowerLimit
double GetLowerLimit()
Definition: ElmoCtrl.h:117
Sleep
void Sleep(int msecs)
Definition: ElmoCtrl.cpp:60
ElmoCtrl::SetMotionCtrlType
bool SetMotionCtrlType(int type)
Definition: ElmoCtrl.cpp:386
ElmoCtrl::ElmoCtrl
ElmoCtrl()
Definition: ElmoCtrl.cpp:78
ElmoCtrlParams::GetEncoderIncrements
int GetEncoderIncrements()
Definition: ElmoCtrl.h:138
ElmoCtrlParams::GetCanIniFile
std::string GetCanIniFile()
Definition: ElmoCtrl.h:134
ElmoCtrlParams::GetCanDevice
std::string GetCanDevice()
Definition: ElmoCtrl.h:102
ElmoCtrlParams::GetMotorDirection
int GetMotorDirection()
Definition: ElmoCtrl.h:149
CanMsg::m_iID
int m_iID
CanMsg::set
void set(BYTE Data0=0, BYTE Data1=0, BYTE Data2=0, BYTE Data3=0, BYTE Data4=0, BYTE Data5=0, BYTE Data6=0, BYTE Data7=0)
ElmoCtrl::Home
bool Home()
Definition: ElmoCtrl.cpp:98
ElmoCtrlParams::GetBaudRate
int GetBaudRate()
Definition: ElmoCtrl.h:106
ElmoCtrl::evalCanBuffer
int evalCanBuffer()
Definition: ElmoCtrl.cpp:128
ElmoCtrl::RecoverAfterEmergencyStop
bool RecoverAfterEmergencyStop()
Definition: ElmoCtrl.cpp:148
ElmoCtrl::isError
bool isError()
Definition: ElmoCtrl.cpp:416
CanItf::transmitMsg
virtual bool transmitMsg(CanMsg CMsg, bool bBlocking=true)=0
ElmoCtrlParams::GetGearRatio
double GetGearRatio()
Definition: ElmoCtrl.h:142
ElmoCtrlParams::GetUpperLimit
double GetUpperLimit()
Definition: ElmoCtrl.h:116
ElmoCtrlParams::GetHomingDir
int GetHomingDir()
Definition: ElmoCtrl.h:126

cob_head_axis
Author(s): Ulrich Reiser
autogenerated on Tue Jul 25 2017 02:22:10