cob_head_axis: cob_head_axis: ElmoCtrl.cpp Source File

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob3_driver
00012  * ROS package name: cob_camera_axis
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Ulrich Reiser, email:ulrich.reiser@ipa.fhg.de
00017  *
00018  * Date of creation: Jul 2010
00019  * ToDo:
00020  *
00021  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00022  *
00023  * Redistribution and use in source and binary forms, with or without
00024  * modification, are permitted provided that the following conditions are met:
00025  *
00026  *     * Redistributions of source code must retain the above copyright
00027  *       notice, this list of conditions and the following disclaimer.
00028  *     * Redistributions in binary form must reproduce the above copyright
00029  *       notice, this list of conditions and the following disclaimer in the
00030  *       documentation and/or other materials provided with the distribution.
00031  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00032  *       Engineering and Automation (IPA) nor the names of its
00033  *       contributors may be used to endorse or promote products derived from
00034  *       this software without specific prior written permission.
00035  *
00036  * This program is free software: you can redistribute it and/or modify
00037  * it under the terms of the GNU Lesser General Public License LGPL as 
00038  * published by the Free Software Foundation, either version 3 of the 
00039  * License, or (at your option) any later version.
00040  * 
00041  * This program is distributed in the hope that it will be useful,
00042  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00043  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00044  * GNU Lesser General Public License LGPL for more details.
00045  * 
00046  * You should have received a copy of the GNU Lesser General Public 
00047  * License LGPL along with this program. 
00048  * If not, see <http://www.gnu.org/licenses/>.
00049  *
00050  ****************************************************************/
00051 
00052 
00053 #include <cob_camera_axis/ElmoCtrl.h>
00054 
00055 #include <iostream>
00056 
00057 using namespace std;
00058 
00059 void Sleep(int msecs){usleep(1000*msecs);}
00060 
00061 bool ElmoCtrl::sendNetStartCanOpen(CanItf* canCtrl) {
00062         bool ret = false;       
00063 
00064         CanMsg msg;
00065 
00066         msg.m_iID  = 0;
00067         msg.m_iLen = 2;
00068         msg.set(1,0,0,0,0,0,0,0);
00069         ret = canCtrl->transmitMsg(msg, false);
00070 
00071         usleep(100000);
00072 
00073         return ret;
00074 }
00075 
00076 
00077 ElmoCtrl::ElmoCtrl() {
00078         m_Joint = NULL;
00079         m_JointParams = NULL;
00080         m_CanCtrl = NULL;
00081         m_CanBaseAddress = 0;
00082         m_MotionCtrlType = POS_CTRL;
00083         m_MaxVel = 2.0;
00084         m_Params = NULL;
00085 }
00086 
00087 ElmoCtrl::~ElmoCtrl() {
00088         if (m_Joint)
00089                 m_Joint->shutdown();
00090                 delete m_Joint;
00091         if (m_JointParams)
00092                 delete m_JointParams;
00093         if (m_CanCtrl)
00094                 delete m_CanCtrl;
00095 }
00096 
00097 bool ElmoCtrl::Home()
00098 {
00099         bool success = false;
00100         if (m_Joint != NULL) {
00101                 //THIS is needed for head_axis on cob3-2!
00102                 //set input logic to 'general purpose'
00103                 m_Joint->IntprtSetInt(8, 'I', 'L', 2, 7);                      
00104                 usleep(20000);  
00105                 
00106                 //THIS is needed for cob3-3 (it's using DIN1):
00107                 //set input logic to 'general purpose'
00108                 m_Joint->IntprtSetInt(8, 'I', 'L', 1, 6);                       
00109                 usleep(20000);  
00110                 
00111                 m_Joint->initHoming();
00112         }
00113 
00114         //ToDo: UHR: necessary?
00115         Sleep(10);
00116         printf("ElmoCtrl: Home(): Homing Vel = %f\n",m_HomingDir*0.3);
00117         m_Joint->setGearVelRadS(m_HomingDir*0.3);
00118         //cpc-pk: removed this sleep, it's not necessary to leave home-sensor during homing
00119         //Sleep(750);
00120         success = m_Joint->execHoming();
00121         m_Joint->setGearVelRadS(0.0);
00122 
00123         return success;
00124 }
00125 
00126 
00127 int ElmoCtrl::evalCanBuffer() {
00128         bool bRet;
00129         
00130         //pthread_mutex_lock(&(m_Mutex));
00131 
00132         // as long as there is something in the can buffer -> read out next message
00133         while(m_CanCtrl->receiveMsg(&m_CanMsgRec) == true) {
00134                 bRet = false;
00135                 // check if the message belongs to camera_axis motor
00136                 bRet |= m_Joint->evalReceivedMsg(m_CanMsgRec);
00137 
00138                 if (bRet == true) {
00139                 } else std::cout << "cob_camera_axis: Unknown CAN-msg: " << m_CanMsgRec.m_iID << "  " << (int)m_CanMsgRec.getAt(0) << " " << (int)m_CanMsgRec.getAt(1) << std::endl;
00140         }
00141         
00142         //pthread_mutex_unlock(&(m_Mutex));
00143 
00144         return 0;
00145 }
00146 
00147 bool ElmoCtrl::RecoverAfterEmergencyStop() {
00148         
00149         bool success = false;
00150         printf("Resetting motor ...\n");
00151         success = m_Joint->start();
00152         if (!success) {
00153                 printf("failed!\n");
00154         } else {        
00155                 printf("successful\n");
00156                 m_Joint->setGearVelRadS(0);
00157         }
00158         Sleep(1000);
00159         return success;
00160 }
00161 
00162 
00163 bool ElmoCtrl::Init(ElmoCtrlParams * params, bool home) { //home = true by default
00164         bool success = false;
00165         string CanIniFile;
00166         string CanDevice;
00167         int baudrate = 0;
00168         
00169         m_Params = params;
00170 
00171         if (params == NULL) {
00172                 printf("ElmoCtrlParams:Error:%s:%d:, invalid parameters!\n",__FILE__,__LINE__);
00173                 success = false;
00174         } else {
00175                 success = true;
00176         }
00177 
00178         if (success) {
00179                 printf( "------------ ElmoCtrl Init ---------------\n");
00180                 
00181                 //Allocate memory and read params e.g. gotten from ROS parameter server
00182                 m_Joint = new CanDriveHarmonica();
00183                 m_JointParams = new DriveParam();
00184                 m_CanBaseAddress = params->GetModuleID();
00185                 CanIniFile = params->GetCanIniFile();   
00186                 m_MaxVel = params->GetMaxVel();
00187                 m_HomingDir = params->GetHomingDir();
00188                 m_HomingDigIn = params->GetHomingDigIn();
00189                 
00190                 m_EncIncrPerRevMot = params->GetEncoderIncrements();
00191                 m_MotorDirection = params->GetMotorDirection();
00192                 m_GearRatio = params->GetGearRatio();
00193                 
00194                 if (CanIniFile.length() == 0) { 
00195                         printf("%s,%d:Error: Parameter 'CanIniFile' not given!\n",__FILE__,__LINE__);
00196                         success = false;
00197                 }
00198                 
00199                 CanDevice = params->GetCanDevice();
00200                 if (CanDevice.length() == 0) {  
00201                         printf("%s,%d:Error: Parameter 'Can-Device' not given!\n",__FILE__,__LINE__);
00202                         success = false;
00203                 }
00204                 
00205                 baudrate = params->GetBaudRate();
00206                 if (baudrate == 0) {    
00207                         printf("%s,%d:Error: Parameter 'Baud-Rate' not given!\n",__FILE__,__LINE__);
00208                         success = false;
00209                 }
00210                 
00211                 //Setting motor model data              
00212                 if (success) {
00213                         m_JointOffset = params->GetAngleOffset();
00214                         m_UpperLimit = params->GetUpperLimit();
00215                         m_LowerLimit = params->GetLowerLimit();
00216                 }
00217 
00218                 if (success) {
00219                         printf("The following parameters were successfully read from the parameter server (given through *params): \n");
00220                         printf("CanIniFile:     %s\n",CanIniFile.c_str());
00221                         printf("CanDieODvice:   %s\n",CanDevice.c_str());
00222                         printf("Baudrate:       %d\n",baudrate);
00223                         printf("Module ID: %d\n",m_CanBaseAddress);
00224                         printf("Max Vel: %f\n",m_MaxVel);
00225                         printf("Homing Dir: %d\n",m_HomingDir);
00226                         printf("HomingDigIn: %d\n",m_HomingDigIn);
00227                         printf("Offset/Limit(min/max)  %f/(%f,%f)\n",m_JointOffset,m_LowerLimit,m_UpperLimit);
00228                 }
00229         }
00230         
00231         //Setting up CAN interface
00232         
00233         if (success) {
00234           //m_CanCtrl = new CanESD(CanIniFile.c_str(), false);
00235                 m_CanCtrl = new CANPeakSysUSB(CanIniFile.c_str());
00236                 if (m_CanCtrl == NULL) {
00237                         printf("%s,%d:Error: Could not open Can Device!\n",__FILE__,__LINE__);
00238                         success = false;
00239                 }
00240           }
00241         
00242         if (success) {
00243                         /* WRONG CAN-identifiers
00244                         //m_CanBaseAddress = params->GetModulID(i);
00245                         m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1;
00246                         m_CanAddress.TxPDO2 = 0x285 + m_CanBaseAddress -1;
00247                         m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1;
00248                         m_CanAddress.TxSDO = 0x491 + m_CanBaseAddress -1;
00249                         m_CanAddress.RxSDO = 0x511 + m_CanBaseAddress -1;
00250                         */
00251                         m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1;
00252                         m_CanAddress.TxPDO2 = 0x281 + m_CanBaseAddress -1;
00253                         m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1;
00254                         m_CanAddress.TxSDO = 0x581 + m_CanBaseAddress -1;
00255                         m_CanAddress.RxSDO = 0x601 + m_CanBaseAddress -1;
00256                         m_Joint->setCanItf(m_CanCtrl);
00257                         m_Joint->setCanOpenParam(m_CanAddress.TxPDO1, 
00258                                                         m_CanAddress.TxPDO2, 
00259                                                         m_CanAddress.RxPDO2, 
00260                                                         m_CanAddress.TxSDO, 
00261                                                         m_CanAddress.RxSDO );
00262                                                         
00263                         printf("CanAdresses set to %d (Base), %x, %x, %x, %x, %x...\n", m_CanBaseAddress,
00264                                                                                                                                                 m_CanAddress.TxPDO1,
00265                                                                                                                                                 m_CanAddress.TxPDO2,
00266                                                                                                                                                 m_CanAddress.RxPDO2,
00267                                                                                                                                                 m_CanAddress.TxSDO,
00268                                                                                                                                                 m_CanAddress.RxSDO);
00269                 
00270           }
00271           
00272           if (success) {
00273                 success = sendNetStartCanOpen(m_CanCtrl);
00274           }
00275           
00276           if (success) {
00277                 //ToDo: Read from File!
00278                 /*
00279                 int iDriveIdent,
00280                 int iEncIncrPerRevMot,
00281                 double dVelMeasFrqHz,
00282                 double dBeltRatio,
00283                 double dGearRatio,
00284                 int iSign,
00285                 double dVelMaxEncIncrS,
00286                 double dAccIncrS2,
00287                 double dDecIncrS2,
00288                 int iEncOffsetIncr,
00289                 bool bIsSteer,
00290         double dCurrToTorque,
00291                 double dCurrMax,
00292                 int iHomingDigIn
00293                 */
00294 
00295 /*
00296                 m_JointParams->setParam( //parameters taken from CanCtrl.ini
00297                                                         0, //int iDriveIdent,
00298                                                         4096,//int iEncIncrPerRevMot,
00299                                                         1,//double dVelMeasFrqHz,
00300                                                         1,//double dBeltRatio,
00301                                                         47.77,//double dGearRatio,
00302                                                         -1.0,//int iSign,
00303                                                         740000,//double dVelMaxEncIncrS,
00304                                                         1000000,//80000,//double dAccIncrS2,
00305                                                         1000000,//80000//double dDecIncrS2),
00306                                                         0, //int iEncOffsetIncr
00307                                                         true, // bool bIsSteer
00308                                                         0, // double dCurrToTorque
00309                                                         0, // double dCurrMax
00310                                                         m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9
00311                                           );
00312 */
00313 
00314                 m_JointParams->setParam( //parameters taken from CanCtrl.ini
00315                                                         0, //int iDriveIdent,
00316                                                         m_EncIncrPerRevMot,//int iEncIncrPerRevMot,
00317                                                         1,//double dVelMeasFrqHz,
00318                                                         1,//double dBeltRatio,
00319                                                         m_GearRatio,//double dGearRatio,
00320                                                         m_MotorDirection,//int iSign,
00321                                                         74000000,//double dVelMaxEncIncrS,
00322                                                         1000000,//80000,//double dAccIncrS2,
00323                                                         1000000,//80000//double dDecIncrS2),
00324                                                         0, //int iEncOffsetIncr
00325                                                         true, // bool bIsSteer
00326                                                         0, // double dCurrToTorque
00327                                                         0, // double dm_HomingDigInCurrMax
00328                                                         m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9
00329                                           );
00330                 
00331                           
00332                 m_Joint->setDriveParam(*m_JointParams);
00333                 }
00334 
00335           if (success)
00336           {
00337                           printf("Init motor ...\n");
00338                           success = m_Joint->init();
00339                           if (!success)
00340                           {
00341                                           printf("failed!\n");
00342                           }
00343                           else
00344                           {     
00345                                           printf("successful\n");
00346                           }
00347           }
00348           
00349           if (success)
00350                           success = SetMotionCtrlType(m_MotionCtrlType);
00351                           if(!success) std::cout << "Failed to SetMotionCtrlType to " << m_MotionCtrlType << std::endl;
00352 
00353           if (success)
00354           {
00355                           printf("Starting motor ..\n");
00356                           success = m_Joint->start();
00357                           if (!success)
00358                           {
00359                                           printf("failed!\n");
00360                           }
00361                           else
00362                           {     
00363                                           printf("successful\n");
00364                                           m_Joint->setGearVelRadS(0);
00365                           }
00366           }
00367           //ToDo: UHR: necessary?
00368           Sleep(1000);
00369 
00370           if (success && home)
00371           {
00372                 std::cout << "Start homing procedure now.." << std::endl;
00373                           success = Home();
00374           }
00375           //Thread init
00376           if (success)
00377           {
00378                           pthread_mutex_init(&m_Mutex,NULL);
00379           }
00380 
00381 
00382           return success;
00383 }
00384 
00385 bool ElmoCtrl::SetMotionCtrlType(int type)
00386 {
00387         m_MotionCtrlType = type;
00388         bool success = false; 
00389         if (type == POS_CTRL)
00390         {
00391                         success = m_Joint->shutdown();
00392                         if (success)
00393                                         success = m_Joint->setTypeMotion(CanDriveHarmonica::MOTIONTYPE_POSCTRL);
00394                                                 
00395                         //ToDo: necessary?
00396                         Sleep(100);
00397                         success = m_Joint->start();
00398         
00399         }
00400         else if (type == VEL_CTRL)
00401         {
00402                 //UHR: ToDo
00403                 printf("%s%d:Error: Velocity control not implemented yet!\n",__FILE__,__LINE__);
00404                 success = false;
00405         }
00406         return success;
00407 };
00408 
00409 
00410 int ElmoCtrl::GetMotionCtrlType() 
00411 {
00412         return m_MotionCtrlType;
00413 }
00414 
00415 int ElmoCtrl::getGearPosVelRadS( double* pdAngleGearRad, double* pdVelGearRadS)
00416 {
00417 
00418         // init default outputs
00419         *pdAngleGearRad = 0;
00420         *pdVelGearRadS = 0;
00421 
00422         m_Joint->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS);
00423         *pdAngleGearRad = *pdAngleGearRad - m_JointOffset;
00424         
00425         return 0;
00426 }
00427 
00428 //-----------------------------------------------
00429 
00430 int ElmoCtrl:: setGearPosVelRadS(double dPosRad, double dVelRadS)
00431 {
00432 
00433         if(dPosRad< m_LowerLimit) {
00434                 std::cout << "Position under LowerBound -> set up" << std::endl;
00435                 dPosRad = m_LowerLimit;
00436         } else if(dPosRad > m_UpperLimit) {
00437                 std::cout << "Position over UpperBound -> set down" << std::endl;
00438                 dPosRad = m_UpperLimit;
00439         }
00440                 
00441         if(dVelRadS > m_MaxVel)
00442                 dVelRadS = m_MaxVel;
00443         else if(dVelRadS < -m_MaxVel)
00444                 dVelRadS = -m_MaxVel;
00445 
00446         //COB3-2: m_Joint->setGearPosVelRadS(dPosRad + m_JointOffset, dVelRadS);
00447         
00448         //COB3-1: m_Joint->setGearPosVelRadS(-3.141592654 - dPosRad + m_JointOffset, dVelRadS);
00449         printf("ElmoCtrl:setGearPosVelRadS: dPosRad %f with vel %f\n",dPosRad, dVelRadS);
00450         m_Joint->setGearPosVelRadS(m_HomingDir * dPosRad + m_JointOffset, dVelRadS);
00451         return 0;
00452 }
00453 
00454 bool ElmoCtrl::Stop()
00455 {
00456                 //UHR: ToDo: what happens exactly in this method? Sudden stop?
00457                 double pos = 0.0;
00458                 double vel = 0.0;
00459                 m_Joint->getGearPosVelRadS(&pos,&vel);
00460                 m_Joint->setGearPosVelRadS(pos,0);
00461 
00462                 return true;
00463                 //m_Joint[i]->shutdown();
00464 
00465 }