ElmoCtrl.cpp
Go to the documentation of this 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_head_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_head_axis/ElmoCtrl.h>
00054 #include <unistd.h>
00055 #include <iostream>
00056 #include <unistd.h>
00057 
00058 using namespace std;
00059 
00060 void Sleep(int msecs){usleep(1000*msecs);}
00061 
00062 bool ElmoCtrl::sendNetStartCanOpen(CanItf* canCtrl) {
00063         bool ret = false;       
00064 
00065         CanMsg msg;
00066 
00067         msg.m_iID  = 0;
00068         msg.m_iLen = 2;
00069         msg.set(1,0,0,0,0,0,0,0);
00070         ret = canCtrl->transmitMsg(msg, false);
00071 
00072         usleep(100000);
00073 
00074         return ret;
00075 }
00076 
00077 
00078 ElmoCtrl::ElmoCtrl() {
00079         m_Joint = NULL;
00080         m_JointParams = NULL;
00081         m_CanCtrl = NULL;
00082         m_CanBaseAddress = 0;
00083         m_MotionCtrlType = POS_CTRL;
00084         m_MaxVel = 2.0;
00085         m_Params = NULL;
00086 }
00087 
00088 ElmoCtrl::~ElmoCtrl() {
00089         if (m_Joint)
00090                 m_Joint->shutdown();
00091                 delete m_Joint;
00092         if (m_JointParams)
00093                 delete m_JointParams;
00094         if (m_CanCtrl)
00095                 delete m_CanCtrl;
00096 }
00097 
00098 bool ElmoCtrl::Home()
00099 {
00100         bool success = false;
00101         if (m_Joint != NULL) {
00102                 //THIS is needed for head_axis on cob3-2!
00103                 //set input logic to 'general purpose'
00104                 m_Joint->IntprtSetInt(8, 'I', 'L', 2, 7);                      
00105                 usleep(20000);  
00106                 
00107                 //THIS is needed for cob3-3 (it's using DIN1):
00108                 //set input logic to 'general purpose'
00109                 m_Joint->IntprtSetInt(8, 'I', 'L', 1, 6);                       
00110                 usleep(20000);  
00111                 
00112                 m_Joint->initHoming();
00113         }
00114 
00115         //ToDo: UHR: necessary?
00116         Sleep(10);
00117         printf("ElmoCtrl: Home(): Homing Vel = %f\n",m_HomingDir*0.3);
00118         m_Joint->setGearVelRadS(m_HomingDir*0.3);
00119         //cpc-pk: removed this sleep, it's not necessary to leave home-sensor during homing
00120         //Sleep(750);
00121         success = m_Joint->execHoming();
00122         m_Joint->setGearVelRadS(0.0);
00123 
00124         return success;
00125 }
00126 
00127 
00128 int ElmoCtrl::evalCanBuffer() {
00129         bool bRet;
00130         
00131         //pthread_mutex_lock(&(m_Mutex));
00132 
00133         // as long as there is something in the can buffer -> read out next message
00134         while(m_CanCtrl->receiveMsg(&m_CanMsgRec) == true) {
00135                 bRet = false;
00136                 // check if the message belongs to head_axis motor
00137                 bRet |= m_Joint->evalReceivedMsg(m_CanMsgRec);
00138 
00139                 if (bRet == true) {
00140                 } 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;
00141         }
00142         
00143         //pthread_mutex_unlock(&(m_Mutex));
00144 
00145         return 0;
00146 }
00147 
00148 bool ElmoCtrl::RecoverAfterEmergencyStop() {
00149         
00150         bool success = false;
00151         printf("Resetting motor ...\n");
00152         success = m_Joint->start();
00153         if (!success) {
00154                 printf("failed!\n");
00155         } else {        
00156                 printf("successful\n");
00157                 m_Joint->setGearVelRadS(0);
00158         }
00159         Sleep(1000);
00160         return success;
00161 }
00162 
00163 
00164 bool ElmoCtrl::Init(ElmoCtrlParams * params, bool home) { //home = true by default
00165         bool success = false;
00166         string CanIniFile;
00167         string CanDevice;
00168         int baudrate = 0;
00169         
00170         m_Params = params;
00171 
00172         if (params == NULL) {
00173                 printf("ElmoCtrlParams:Error:%s:%d:, invalid parameters!\n",__FILE__,__LINE__);
00174                 success = false;
00175         } else {
00176                 success = true;
00177         }
00178 
00179         if (success) {
00180                 printf( "------------ ElmoCtrl Init ---------------\n");
00181                 
00182                 //Allocate memory and read params e.g. gotten from ROS parameter server
00183                 m_Joint = new CanDriveHarmonica();
00184                 m_JointParams = new DriveParam();
00185                 m_CanBaseAddress = params->GetModuleID();
00186                 CanIniFile = params->GetCanIniFile();   
00187                 m_MaxVel = params->GetMaxVel();
00188                 m_HomingDir = params->GetHomingDir();
00189                 m_HomingDigIn = params->GetHomingDigIn();
00190                 
00191                 m_EncIncrPerRevMot = params->GetEncoderIncrements();
00192                 m_MotorDirection = params->GetMotorDirection();
00193                 m_GearRatio = params->GetGearRatio();
00194                 
00195                 if (CanIniFile.length() == 0) { 
00196                         printf("%s,%d:Error: Parameter 'CanIniFile' not given!\n",__FILE__,__LINE__);
00197                         success = false;
00198                 }
00199                 
00200                 CanDevice = params->GetCanDevice();
00201                 if (CanDevice.length() == 0) {  
00202                         printf("%s,%d:Error: Parameter 'Can-Device' not given!\n",__FILE__,__LINE__);
00203                         success = false;
00204                 }
00205                 
00206                 baudrate = params->GetBaudRate();
00207                 if (baudrate == 0) {    
00208                         printf("%s,%d:Error: Parameter 'Baud-Rate' not given!\n",__FILE__,__LINE__);
00209                         success = false;
00210                 }
00211                 
00212                 //Setting motor model data              
00213                 if (success) {
00214                         m_JointOffset = params->GetAngleOffset();
00215                         m_UpperLimit = params->GetUpperLimit();
00216                         m_LowerLimit = params->GetLowerLimit();
00217                 }
00218 
00219                 if (success) {
00220                         printf("The following parameters were successfully read from the parameter server (given through *params): \n");
00221                         printf("CanIniFile:     %s\n",CanIniFile.c_str());
00222                         printf("CanDieODvice:   %s\n",CanDevice.c_str());
00223                         printf("Baudrate:       %d\n",baudrate);
00224                         printf("Module ID: %d\n",m_CanBaseAddress);
00225                         printf("Max Vel: %f\n",m_MaxVel);
00226                         printf("Homing Dir: %d\n",m_HomingDir);
00227                         printf("HomingDigIn: %d\n",m_HomingDigIn);
00228                         printf("Offset/Limit(min/max)  %f/(%f,%f)\n",m_JointOffset,m_LowerLimit,m_UpperLimit);
00229                 }
00230         }
00231         
00232         //Setting up CAN interface
00233         
00234         if (success) {
00235           //m_CanCtrl = new CanESD(CanIniFile.c_str(), false);
00236                 m_CanCtrl = new CANPeakSysUSB(CanIniFile.c_str());
00237                 if (m_CanCtrl == NULL) {
00238                         printf("%s,%d:Error: Could not open Can Device!\n",__FILE__,__LINE__);
00239                         success = false;
00240                 }
00241           }
00242         
00243         if (success) {
00244                         /* WRONG CAN-identifiers
00245                         //m_CanBaseAddress = params->GetModulID(i);
00246                         m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1;
00247                         m_CanAddress.TxPDO2 = 0x285 + m_CanBaseAddress -1;
00248                         m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1;
00249                         m_CanAddress.TxSDO = 0x491 + m_CanBaseAddress -1;
00250                         m_CanAddress.RxSDO = 0x511 + m_CanBaseAddress -1;
00251                         */
00252                         m_CanAddress.TxPDO1 = 0x181 + m_CanBaseAddress -1;
00253                         m_CanAddress.TxPDO2 = 0x281 + m_CanBaseAddress -1;
00254                         m_CanAddress.RxPDO2 = 0x301 + m_CanBaseAddress -1;
00255                         m_CanAddress.TxSDO = 0x581 + m_CanBaseAddress -1;
00256                         m_CanAddress.RxSDO = 0x601 + m_CanBaseAddress -1;
00257                         m_Joint->setCanItf(m_CanCtrl);
00258                         m_Joint->setCanOpenParam(m_CanAddress.TxPDO1, 
00259                                                         m_CanAddress.TxPDO2, 
00260                                                         m_CanAddress.RxPDO2, 
00261                                                         m_CanAddress.TxSDO, 
00262                                                         m_CanAddress.RxSDO );
00263                                                         
00264                         printf("CanAdresses set to %d (Base), %x, %x, %x, %x, %x...\n", m_CanBaseAddress,
00265                                                                                                                                                 m_CanAddress.TxPDO1,
00266                                                                                                                                                 m_CanAddress.TxPDO2,
00267                                                                                                                                                 m_CanAddress.RxPDO2,
00268                                                                                                                                                 m_CanAddress.TxSDO,
00269                                                                                                                                                 m_CanAddress.RxSDO);
00270                 
00271           }
00272           
00273           if (success) {
00274                 success = sendNetStartCanOpen(m_CanCtrl);
00275           }
00276           
00277           if (success) {
00278                 //ToDo: Read from File!
00279                 /*
00280                 int iDriveIdent,
00281                 int iEncIncrPerRevMot,
00282                 double dVelMeasFrqHz,
00283                 double dBeltRatio,
00284                 double dGearRatio,
00285                 int iSign,
00286                 double dVelMaxEncIncrS,
00287                 double dAccIncrS2,
00288                 double dDecIncrS2,
00289                 int iEncOffsetIncr,
00290                 bool bIsSteer,
00291         double dCurrToTorque,
00292                 double dCurrMax,
00293                 int iHomingDigIn
00294                 */
00295 
00296 /*
00297                 m_JointParams->setParam( //parameters taken from CanCtrl.ini
00298                                                         0, //int iDriveIdent,
00299                                                         4096,//int iEncIncrPerRevMot,
00300                                                         1,//double dVelMeasFrqHz,
00301                                                         1,//double dBeltRatio,
00302                                                         47.77,//double dGearRatio,
00303                                                         -1.0,//int iSign,
00304                                                         740000,//double dVelMaxEncIncrS,
00305                                                         1000000,//80000,//double dAccIncrS2,
00306                                                         1000000,//80000//double dDecIncrS2),
00307                                                         0, //int iEncOffsetIncr
00308                                                         true, // bool bIsSteer
00309                                                         0, // double dCurrToTorque
00310                                                         0, // double dCurrMax
00311                                                         m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9
00312                                           );
00313 */
00314 
00315                 m_JointParams->setParam( //parameters taken from CanCtrl.ini
00316                                                         0, //int iDriveIdent,
00317                                                         m_EncIncrPerRevMot,//int iEncIncrPerRevMot,
00318                                                         1,//double dVelMeasFrqHz,
00319                                                         1,//double dBeltRatio,
00320                                                         m_GearRatio,//double dGearRatio,
00321                                                         m_MotorDirection,//int iSign,
00322                                                         74000000,//double dVelMaxEncIncrS,
00323                                                         1000000,//80000,//double dAccIncrS2,
00324                                                         1000000,//80000//double dDecIncrS2),
00325                                                         0, //int iEncOffsetIncr
00326                                                         true, // bool bIsSteer
00327                                                         0, // double dCurrToTorque
00328                                                         0, // double dm_HomingDigInCurrMax
00329                                                         m_HomingDigIn // int iHomingDigIn //cob3-2->11, cob3-1->9
00330                                           );
00331                 
00332                           
00333                 m_Joint->setDriveParam(*m_JointParams);
00334                 }
00335 
00336           if (success)
00337           {
00338                           printf("Init motor ...\n");
00339                           success = m_Joint->init();
00340                           if (!success)
00341                           {
00342                                           printf("failed!\n");
00343                           }
00344                           else
00345                           {     
00346                                           printf("successful\n");
00347                           }
00348           }
00349           
00350           if (success)
00351                           success = SetMotionCtrlType(m_MotionCtrlType);
00352                           if(!success) std::cout << "Failed to SetMotionCtrlType to " << m_MotionCtrlType << std::endl;
00353 
00354           if (success)
00355           {
00356                           printf("Starting motor ..\n");
00357                           success = m_Joint->start();
00358                           if (!success)
00359                           {
00360                                           printf("failed!\n");
00361                           }
00362                           else
00363                           {     
00364                                           printf("successful\n");
00365                                           m_Joint->setGearVelRadS(0);
00366                           }
00367           }
00368           //ToDo: UHR: necessary?
00369           Sleep(1000);
00370 
00371           if (success && home)
00372           {
00373                 std::cout << "Start homing procedure now.." << std::endl;
00374                           success = Home();
00375           }
00376           //Thread init
00377           if (success)
00378           {
00379                           pthread_mutex_init(&m_Mutex,NULL);
00380           }
00381 
00382 
00383           return success;
00384 }
00385 
00386 bool ElmoCtrl::SetMotionCtrlType(int type)
00387 {
00388         m_MotionCtrlType = type;
00389         bool success = false; 
00390         if (type == POS_CTRL)
00391         {
00392                         success = m_Joint->shutdown();
00393                         if (success)
00394                                         success = m_Joint->setTypeMotion(CanDriveHarmonica::MOTIONTYPE_POSCTRL);
00395                                                 
00396                         //ToDo: necessary?
00397                         Sleep(100);
00398                         success = m_Joint->start();
00399         
00400         }
00401         else if (type == VEL_CTRL)
00402         {
00403                 //UHR: ToDo
00404                 printf("%s%d:Error: Velocity control not implemented yet!\n",__FILE__,__LINE__);
00405                 success = false;
00406         }
00407         return success;
00408 };
00409 
00410 
00411 int ElmoCtrl::GetMotionCtrlType() 
00412 {
00413         return m_MotionCtrlType;
00414 }
00415 
00416 bool ElmoCtrl::isError() 
00417 {
00418         return m_Joint->isError();
00419 }
00420 
00421 int ElmoCtrl::getGearPosVelRadS( double* pdAngleGearRad, double* pdVelGearRadS)
00422 {
00423 
00424         // init default outputs
00425         *pdAngleGearRad = 0;
00426         *pdVelGearRadS = 0;
00427 
00428         m_Joint->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS);
00429         *pdAngleGearRad = *pdAngleGearRad - m_JointOffset;
00430         
00431         return 0;
00432 }
00433 
00434 //-----------------------------------------------
00435 
00436 int ElmoCtrl:: setGearPosVelRadS(double dPosRad, double dVelRadS)
00437 {
00438 
00439         if(dPosRad< m_LowerLimit) {
00440                 std::cout << "Position under LowerBound -> set up" << std::endl;
00441                 dPosRad = m_LowerLimit;
00442         } else if(dPosRad > m_UpperLimit) {
00443                 std::cout << "Position over UpperBound -> set down" << std::endl;
00444                 dPosRad = m_UpperLimit;
00445         }
00446                 
00447         if(dVelRadS > m_MaxVel)
00448                 dVelRadS = m_MaxVel;
00449         else if(dVelRadS < -m_MaxVel)
00450                 dVelRadS = -m_MaxVel;
00451 
00452         //COB3-2: m_Joint->setGearPosVelRadS(dPosRad + m_JointOffset, dVelRadS);
00453         
00454         //COB3-1: m_Joint->setGearPosVelRadS(-3.141592654 - dPosRad + m_JointOffset, dVelRadS);
00455         printf("ElmoCtrl:setGearPosVelRadS: dPosRad %f with vel %f\n",dPosRad, dVelRadS);
00456         m_Joint->setGearPosVelRadS(m_HomingDir * dPosRad + m_JointOffset, dVelRadS);
00457         return 0;
00458 }
00459 
00460 bool ElmoCtrl::Stop()
00461 {
00462                 //UHR: ToDo: what happens exactly in this method? Sudden stop?
00463                 double pos = 0.0;
00464                 double vel = 0.0;
00465                 m_Joint->getGearPosVelRadS(&pos,&vel);
00466                 m_Joint->setGearPosVelRadS(pos,0);
00467 
00468                 return true;
00469                 //m_Joint[i]->shutdown();
00470 
00471 }


cob_head_axis
Author(s): Ulrich Reiser
autogenerated on Thu Aug 27 2015 12:45:55