CanDriveHarmonica.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: cob_driver
00012  * ROS package name: cob_canopen_motor
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Christian Connette, email:christian.connette@ipa.fhg.de
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: Feb 2009
00021  * ToDo: - Assign Adsress of digital input for homing switch "iHomeDigIn" via parameters (in evalReceived Message, Line 116).
00022  *         - Homing Event should be defined by a parameterfile and handed to CanDrive... e.g. via the DriveParam.h (in inithoming, Line 531).
00023  *               - Check whether "requestStatus" can/should be done in the class implementing the component
00024  *
00025  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00026  *
00027  * Redistribution and use in source and binary forms, with or without
00028  * modification, are permitted provided that the following conditions are met:
00029  *
00030  *       * Redistributions of source code must retain the above copyright
00031  *         notice, this list of conditions and the following disclaimer.
00032  *       * Redistributions in binary form must reproduce the above copyright
00033  *         notice, this list of conditions and the following disclaimer in the
00034  *         documentation and/or other materials provided with the distribution.
00035  *       * Neither the name of the Fraunhofer Institute for Manufacturing 
00036  *         Engineering and Automation (IPA) nor the names of its
00037  *         contributors may be used to endorse or promote products derived from
00038  *         this software without specific prior written permission.
00039  *
00040  * This program is free software: you can redistribute it and/or modify
00041  * it under the terms of the GNU Lesser General Public License LGPL as 
00042  * published by the Free Software Foundation, either version 3 of the 
00043  * License, or (at your option) any later version.
00044  * 
00045  * This program is distributed in the hope that it will be useful,
00046  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00047  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00048  * GNU Lesser General Public License LGPL for more details.
00049  * 
00050  * You should have received a copy of the GNU Lesser General Public 
00051  * License LGPL along with this program. 
00052  * If not, see <http://www.gnu.org/licenses/>.
00053  *
00054  ****************************************************************/
00055 
00056 
00057 #include <assert.h>
00058 #include <cob_canopen_motor/CanDriveHarmonica.h>
00059 #include <unistd.h> 
00060 
00061 //-----------------------------------------------
00062 CanDriveHarmonica::CanDriveHarmonica()
00063 {
00064         // Parameter
00065         m_Param.iDivForRequestStatus = 10;
00066         m_Param.dCanTimeout = 6;
00067 
00068         // Variables
00069         m_pCanCtrl = NULL;
00070 
00071         m_iStatusCtrl = 0;
00072         m_dPosGearMeasRad = 0;
00073         m_dAngleGearRadMem  = 0;
00074         m_dVelGearMeasRadS = 0;
00075 
00076         m_VelCalcTime.SetNow();
00077 
00078         m_bLimSwLeft = false;
00079         m_bLimSwRight = false;
00080 
00081         m_bLimitSwitchEnabled = false;
00082 
00083         m_iCountRequestDiv = 0;
00084 
00085         m_iMotorState = ST_PRE_INITIALIZED;
00086         m_bCurrentLimitOn = false;
00087 
00088         m_iNumAttempsRecFail = 0;
00089 
00090         m_SendTime.SetNow();
00091         m_StartTime.SetNow();
00092 
00093         m_bOutputOfFailure = false;
00094         
00095         m_bIsInitialized = false;
00096         
00097 
00098         ElmoRec = new ElmoRecorder(this);
00099 
00100 }
00101 
00102 //-----------------------------------------------
00103 void CanDriveHarmonica::setCanOpenParam( int iTxPDO1, int iTxPDO2, int iRxPDO2, int iTxSDO, int iRxSDO )
00104 {
00105         m_ParamCanOpen.iTxPDO1 = iTxPDO1;
00106         m_ParamCanOpen.iTxPDO2 = iTxPDO2;
00107         m_ParamCanOpen.iRxPDO2 = iRxPDO2;
00108         m_ParamCanOpen.iTxSDO = iTxSDO;
00109         m_ParamCanOpen.iRxSDO = iRxSDO;
00110 
00111 }
00112 
00113 //-----------------------------------------------
00114 bool CanDriveHarmonica::evalReceivedMsg(CanMsg& msg)
00115 {
00116         bool bRet = false;
00117         int iDigIn;
00118         int iFailure;
00119         int iPara;
00120 
00121         int iHomeDigIn = 0x0001; // 0x0001 for CoB3 steering drive homing input; 0x0400 for Scara
00122         int iTemp1, iTemp2;
00123         
00124         m_CanMsgLast = msg;
00125 
00126         //-----------------------
00127         // eval answers from PDO1 - transmitted on SYNC msg
00128         if (msg.m_iID == m_ParamCanOpen.iTxPDO1)
00129         {
00130                 iTemp1 = (msg.getAt(3) << 24) | (msg.getAt(2) << 16)
00131                                 | (msg.getAt(1) << 8) | (msg.getAt(0) );
00132 
00133                 m_dPosGearMeasRad = m_DriveParam.getSign() * m_DriveParam.
00134                         PosMotIncrToPosGearRad(iTemp1);
00135 
00136                 iTemp2 = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00137                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00138                 
00139                 m_dVelGearMeasRadS = m_DriveParam.getSign() * m_DriveParam.
00140                         VelMotIncrPeriodToVelGearRadS(iTemp2);
00141 
00142                 m_WatchdogTime.SetNow();
00143 
00144                 bRet = true;
00145         }       
00146         
00147         //-----------------------
00148         // eval answers from binary interpreter
00149         if (msg.m_iID == m_ParamCanOpen.iTxPDO2)
00150         {
00151                 if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'X') ) // current pos
00152                 {
00153                 }
00154 
00155                 else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'A') ) // position absolute
00156                 {
00157                 }
00158 
00159                 else if( (msg.getAt(0) == 'J') && (msg.getAt(1) == 'V') ) // current velocity
00160                 {
00161                 }
00162 
00163                 else if( (msg.getAt(0) == 'B') && (msg.getAt(1) == 'G') ) // begin motion
00164                 {
00165                 }
00166 
00167                 else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') ) // user mode
00168                 {
00169                         iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00170                                 | (msg.getAt(5) << 8) | (msg.getAt(4)) );
00171                 }
00172 
00173                 else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'P') ) // digital in == limit switches
00174                 {
00175                         iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00176                                 | (msg.getAt(5) << 8) | (msg.getAt(4)) );
00177                         iDigIn = 0x1FFFFF & ( (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00178                                 | (msg.getAt(5) << 8) | (msg.getAt(4)) );
00179 
00180                         if( (iDigIn & iHomeDigIn) != 0x0000 )
00181                         {
00182                                 m_bLimSwRight = true;
00183                         }                       
00184                 }
00185 
00186                 else if( (msg.getAt(0) == 'S') && (msg.getAt(1) == 'R') ) // status
00187                 {
00188                         m_iStatusCtrl = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00189                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00190 
00191                         evalStatusRegister(m_iStatusCtrl);
00192                         ElmoRec->readoutRecorderTryStatus(m_iStatusCtrl, seg_Data);
00193                         
00194                 }
00195 
00196                 else if( (msg.getAt(0) == 'M') && (msg.getAt(1) == 'F') ) // motor failure
00197                 {
00198                         iFailure = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00199                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00200 
00201                         evalMotorFailure(iFailure);
00202                 }
00203 
00204                 // debug eval
00205                 else if( (msg.getAt(0) == 'U') && (msg.getAt(1) == 'M') )
00206                 {
00207                         iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00208                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00209 
00210                         std::cout << "um " << iPara << std::endl;
00211                 }
00212                 
00213                 else if( (msg.getAt(0) == 'P') && (msg.getAt(1) == 'M') )
00214                 {
00215                         iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00216                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00217 
00218                         std::cout << "pm " << iPara << std::endl;
00219                 }
00220 
00221                 else if( (msg.getAt(0) == 'A') && (msg.getAt(1) == 'C') )
00222                 {
00223                         iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00224                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00225 
00226                         std::cout << "ac " << iPara << std::endl;
00227                 }
00228 
00229                 else if( (msg.getAt(0) == 'D') && (msg.getAt(1) == 'C') )
00230                 {
00231                         iPara = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00232                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00233 
00234                         std::cout << "dc " << iPara << std::endl;
00235                 }
00236                 else if( (msg.getAt(0) == 'H') && (msg.getAt(1) == 'M') )
00237                 {
00238                         // status message (homing armed = 1 / disarmed = 0) is encoded in 5th byte
00239                         if(msg.getAt(4) == 0)
00240                         {
00241                                 // if 0 received: elmo disarmed homing after receiving the defined event
00242                                 m_bLimSwRight = true;
00243                         }
00244                 }
00245                 else if( (msg.getAt(0) == 'I') && (msg.getAt(1) == 'Q') )
00246                 {
00247                         int iVal=0;
00248                         iVal = (msg.getAt(7) << 24) | (msg.getAt(6) << 16)
00249                                 | (msg.getAt(5) << 8) | (msg.getAt(4) );
00250                         float* pfVal;
00251                         pfVal=(float*)&iVal;
00252                         m_dMotorCurr = *pfVal;                  
00253                 }
00254 
00255                 else
00256                 {
00257                 }
00258 
00259                 m_WatchdogTime.SetNow();
00260 
00261                 bRet = true;
00262         }
00263         
00264         //-----------------------
00265         // eval answer from SDO
00266         if (msg.m_iID == m_ParamCanOpen.iTxSDO)
00267         {
00268                 m_WatchdogTime.SetNow();
00269 
00270                 if( (msg.getAt(0) >> 5) == 0) { //Received Upload SDO Segment (scs = 0)
00271                         //std::cout << "SDO Upload Segment received" << std::endl;
00272                         receivedSDODataSegment(msg);
00273                         
00274                 } else if( (msg.getAt(0) & 0xE2) == 0x40) { //Received Initiate SDO Upload, that is not expedited -> start segmented upload (scs = 2 AND expedited flag = 0)
00275                         //std::cout << "SDO Initiate Segmented Upload received, Object ID: " << (msg.getAt(1) | (msg.getAt(2) << 8) ) << std::endl;
00276                         receivedSDOSegmentedInitiation(msg);
00277                         
00278                 } else if( (msg.getAt(0) >> 5) == 4) { // Received an Abort SDO Transfer message, cs = 4
00279                         unsigned int iErrorNum = (msg.getAt(4) | msg.getAt(5) << 8 | msg.getAt(6) << 16 | msg.getAt(7) << 24);
00280                         receivedSDOTransferAbort(iErrorNum);
00281                 }
00282 
00283                 bRet = true;
00284         }
00285 
00286         return bRet;
00287 }
00288 
00289 //-----------------------------------------------
00290 bool CanDriveHarmonica::init()
00291 {
00292         int iCnt, iPosCnt;
00293         bool bRet = true;
00294         CanMsg Msg;
00295 
00296         m_iMotorState = ST_PRE_INITIALIZED;
00297 
00298 
00299         // Set Values for Modulo-Counting. Neccessary to preserve absolute position for homed motors (after encoder overflow)
00300         int iIncrRevWheel = int( (double)m_DriveParam.getGearRatio() * (double)m_DriveParam.getBeltRatio()
00301                                         * (double)m_DriveParam.getEncIncrPerRevMot() * 3 );
00302         IntprtSetInt(8, 'M', 'O', 0, 0);
00303         usleep(20000);
00304         IntprtSetInt(8, 'X', 'M', 2, iIncrRevWheel * 5000);
00305         usleep(20000);
00306         IntprtSetInt(8, 'X', 'M', 1, -iIncrRevWheel * 5000);
00307         usleep(20000);
00308         
00309 
00310         setTypeMotion(MOTIONTYPE_VELCTRL);
00311         // ---------- set position counter to zero
00312         IntprtSetInt(8, 'P', 'X', 0, 0);
00313 
00314         iCnt = 0;
00315         while(true)
00316         {
00317                 m_pCanCtrl->receiveMsg(&Msg);
00318         
00319                 if( (Msg.getAt(0) == 'P') && (Msg.getAt(1) == 'X') )
00320                 {
00321                         iPosCnt = (Msg.getAt(7) << 24) | (Msg.getAt(6) << 16)
00322                                 | (Msg.getAt(5) << 8) | (Msg.getAt(4) );
00323                         
00324                         m_dPosGearMeasRad = m_DriveParam.getSign() * m_DriveParam.PosMotIncrToPosGearRad(iPosCnt);
00325                         m_dAngleGearRadMem  = m_dPosGearMeasRad;
00326                         break;
00327                 }
00328 
00329                 if ( iCnt > 300 )
00330                 {
00331                         std::cout << "CanDriveHarmonica: initial position not set" << std::endl;
00332                         bRet = false;
00333                         break;
00334                 }
00335 
00336                 usleep(10000);
00337                 iCnt++;
00338         }
00339 
00340         // ---------- set PDO mapping
00341         // Mapping of TPDO1:
00342         // - position
00343         // - velocity
00344         
00345         // stop all emissions of TPDO1
00346         sendSDODownload(0x1A00, 0, 0);
00347         
00348         // position 4 byte of TPDO1
00349         sendSDODownload(0x1A00, 1, 0x60640020);
00350 
00351         // velocity 4 byte of TPDO1
00352         sendSDODownload(0x1A00, 2, 0x60690020);
00353         
00354         // transmission type "synch"
00355         sendSDODownload(0x1800, 2, 1);
00356         
00357         // activate mapped objects
00358         sendSDODownload(0x1A00, 0, 2);
00359 
00360         m_bWatchdogActive = false;
00361         
00362         if( bRet )
00363                 m_bIsInitialized = true;
00364          
00365         return bRet;    
00366 }
00367 //-----------------------------------------------
00368 bool CanDriveHarmonica::stop()
00369 {       
00370         bool bRet = true;
00371         // motor off
00372         IntprtSetInt(8, 'M', 'O', 0, 0);
00373         usleep(20000);
00374         return bRet;
00375 }
00376 //-----------------------------------------------
00377 bool CanDriveHarmonica::start()
00378 {
00379         // motor on
00380         IntprtSetInt(8, 'M', 'O', 0, 1);
00381         usleep(20000);
00382 
00383         // ------------------- request status
00384         int iCnt;
00385         bool bRet = true;
00386         int iStatus;
00387         CanMsg Msg;
00388 
00389         //  clear the can buffer 
00390         do
00391         {
00392                 bRet = m_pCanCtrl->receiveMsg(&Msg);
00393         }
00394         while(bRet == true);
00395 
00396         // send request
00397         IntprtSetInt(4, 'S', 'R', 0, 0);
00398         
00399         iCnt = 0;
00400         while(true)
00401         {
00402                 m_pCanCtrl->receiveMsg(&Msg);
00403         
00404                 if( (Msg.getAt(0) == 'S') && (Msg.getAt(1) == 'R') )
00405                 {
00406                         iStatus = (Msg.getAt(7) << 24) | (Msg.getAt(6) << 16)
00407                                 | (Msg.getAt(5) << 8) | (Msg.getAt(4) );
00408                         
00409                         bRet = evalStatusRegister(iStatus);
00410                         break;
00411                 }
00412 
00413                 if ( iCnt > 300 )
00414                 {
00415                         std::cout << "CanDriveHarmonica::enableMotor(): No answer on status request" << std::endl;
00416                         bRet = false;
00417                         break;
00418                 }
00419 
00420                 usleep(10000);
00421                 iCnt++;
00422         }
00423 
00424         // ------------------- start watchdog timer
00425         m_WatchdogTime.SetNow();
00426         m_SendTime.SetNow();
00427 
00428         return bRet;
00429 }
00430 
00431 //-----------------------------------------------
00432 bool CanDriveHarmonica::reset()
00433 {
00434         // repeat initialization
00435         
00436         // start network
00437         CanMsg msg;
00438         msg.m_iID  = 0;
00439         msg.m_iLen = 2;
00440         msg.set(1,0,0,0,0,0,0,0);
00441         m_pCanCtrl->transmitMsg(msg);
00442 
00443         // init and start
00444         bool bRet = init();
00445         bRet |= start();
00446 
00447         return bRet;
00448 }
00449 //-----------------------------------------------
00450 bool CanDriveHarmonica::shutdown()
00451 {
00452         std::cout << "shutdown drive " << m_DriveParam.getDriveIdent() << std::endl;
00453 
00454         IntprtSetInt(8, 'M', 'O', 0, 0);
00455 
00456         return true;
00457 }
00458 
00459 //-----------------------------------------------
00460 bool CanDriveHarmonica::startWatchdog(bool bStarted)
00461 {
00462         if (bStarted == true)
00463         {
00464                 //save Watchdog state into member variable
00465                 m_bWatchdogActive = true;
00466                 // ------- init watchdog
00467                 // Harmonica checks PC hearbeat
00468                 // note: the COB-ID for a heartbeat message = 0x700 + Device ID
00469                 
00470                 const int c_iHeartbeatTimeMS = 1000;
00471                 const int c_iNMTNodeID = 0x00;
00472                 
00473                 // consumer (PC) heartbeat time
00474                 sendSDODownload(0x1016, 1, (c_iNMTNodeID << 16) | c_iHeartbeatTimeMS);
00475                 
00476                 // error behavior after failure: 0=pre-operational, 1=no state change, 2=stopped"       
00477                 sendSDODownload(0x1029, 1, 2);
00478                 
00479                 // motor behavior after heartbeat failre: "quick stop"
00480                 sendSDODownload(0x6007, 0, 3);
00481 
00482                 // acivate emergency events: "heartbeat event"
00483                 // Object 0x2F21 = "Emergency Events" which cause an Emergency Message
00484                 // Bit 3 is responsible for Heartbeart-Failure.--> Hex 0x08
00485                 sendSDODownload(0x2F21, 0, 0x08);
00486                 usleep(20000);
00487  
00488         }
00489         else
00490         {       
00491                 //save Watchdog state into member variable
00492                 m_bWatchdogActive = false;
00493 
00494                 //Motor action after Hearbeat-Error: No Action          
00495                 sendSDODownload(0x6007, 0, 0);
00496 
00497                 //Error Behavior: No state change
00498                 sendSDODownload(0x1029, 1, 1);
00499 
00500                 // Deacivate emergency events: "heartbeat event"
00501                 // Object 0x2F21 = "Emergency Events" which cause an Emergency Message
00502                 // Bit 3 is responsible for Heartbeart-Failure.
00503                 sendSDODownload(0x2F21, 0, 0x00);
00504                 usleep(25000);
00505                         
00506                 
00507         }
00508 
00509         return true;
00510 }
00511 
00512 //-----------------------------------------------
00513 bool CanDriveHarmonica::disableBrake(bool bDisabled)
00514 {
00515         return true;
00516 }
00517 
00518 //-----------------------------------------------
00519 double CanDriveHarmonica::getTimeToLastMsg()
00520 {
00521         m_CurrentTime.SetNow();
00522 
00523         return m_CurrentTime - m_WatchdogTime;
00524 }
00525 //-----------------------------------------------
00526 bool CanDriveHarmonica::getStatusLimitSwitch()
00527 {
00528         return m_bLimSwRight;
00529 }
00530 //-----------------------------------------------
00531 bool CanDriveHarmonica::initHoming()
00532 {
00533         const int c_iPosRef = m_DriveParam.getEncOffset();
00534         
00535                 // 1. make sure that, if on elmo controller still a pending homing from a previous startup is running (in case of warm-start without switching of the whole robot), this old sequence is disabled
00536                 // disarm homing process
00537                 IntprtSetInt(8, 'H', 'M', 1, 0);
00538 
00539                 // always give can and controller some time to understand the command
00540                 usleep(20000);
00541 
00542                 /* THIS is needed for head_axis on cob3-2!
00543                 
00544                 //set input logic to 'general purpose'
00545                 IntprtSetInt(8, 'I', 'L', 2, 7);                       
00546                 usleep(20000);  
00547                 */
00548 
00549                 // 2. configure the homing sequence
00550                 // 2.a set the value to which the increment counter shall be reseted as soon as the homing event occurs
00551                 // value to load at homing event
00552                 IntprtSetInt(8, 'H', 'M', 2, c_iPosRef);
00553                 usleep(20000);
00554                 
00555                 // 2.b choose the chanel/switch on which the controller listens for a change or defined logic level (the homing event) (high/low/falling/rising)
00556                 // home event
00557                 // iHomeEvent = 5 : event according to defined FLS switch (for scara arm)
00558                 // iHomeEvent = 9 : event according to definded DIN1 switch (for full steerable wheels COb3)
00559                 // iHomeEvent =11 : event according to ?? (for COb3 Head-Axis)
00560                 IntprtSetInt(8, 'H', 'M', 3, m_DriveParam.getHomingDigIn());
00561                 //IntprtSetInt(8, 'H', 'M', 3, 11); //cob3-2
00562                 usleep(20000);
00563 
00564 
00565                 // 2.c choose the action that the controller shall perform after the homing event occured
00566                 // HM[4] = 0 : after Event stop immediately
00567                 // HM[4] = 2 : Do nothing!
00568                 IntprtSetInt(8, 'H', 'M', 4, 2);
00569                 usleep(20000);
00570 
00571                 // 2.d choose the setting of the position counter (i.e. to the value defined in 2.a) after the homing event occured
00572                 // HM[5] = 0 : absolute setting of position counter: PX = HM[2]
00573                 IntprtSetInt(8, 'H', 'M', 5, 0);
00574                 usleep(20000);
00575 
00576                 // 3. let the motor turn some time to give him the possibility to escape the approximation sensor if accidently in home position already at the beginning of the sequence (done in CanCtrlPltf...)
00577 
00578         return true;    
00579 }
00580 
00581 
00582 //-----------------------------------------------
00583 bool CanDriveHarmonica::execHoming() //not used by CanCtrlPltf, that has its own homing implementation
00584 {
00585 
00586         int iCnt;
00587         CanMsg Msg;
00588         bool bRet = true;
00589 
00590         int iNrDrive = m_DriveParam.getDriveIdent();
00591 
00592         // 4. arm the homing process -> as soon as the approximation sensor is reached and the homing event occurs the commands set in 2. take effect
00593         // arm homing process
00594         IntprtSetInt(8, 'H', 'M', 1, 1);
00595                 
00596         // 5. clear the can buffer to get rid of all uneccessary and potentially disturbing commands still floating through the wires
00597         do
00598         {
00599                 // read from can
00600                 bRet = m_pCanCtrl->receiveMsg(&Msg);
00601         }
00602         while(bRet == true);
00603 
00604         // 6. now listen for status of homing, to synchronize program flow -> proceed only after homing was succesful (homing disarmed by elmo) or timeout occured
00605 
00606         // set timeout counter to zero
00607         iCnt = 0;
00608 
00609         do
00610         {
00611                 // 6.a ask for status of homing process (armed/disarmed)
00612                 // ask for first byte in Homing Configuration
00613                 IntprtSetInt(4, 'H', 'M', 1, 0);
00614 
00615                 // 6.b read message from can
00616                 m_pCanCtrl->receiveMsgRetry(&Msg, 10);
00617                 
00618                 // 6.c see if received message is answer of request and if so what is the status
00619                 if( (Msg.getAt(0) == 'H') && (Msg.getAt(1) == 'M') )
00620                 {       
00621                         // status message (homing armed = 1 / disarmed = 0) is encoded in 5th byte
00622                         if(Msg.getAt(4) == 0)
00623                         {
00624                                 // if 0 received: elmo disarmed homing after receiving the defined event
00625                                 std::cout << "Got Homing-Signal "  << std::endl;
00626                                 m_bLimSwRight = true;
00627                                 break;
00628                         }
00629                 }
00630 
00631                 // increase count for timeout
00632                 usleep(10000);
00633                 iCnt++;
00634 
00635         }
00636         while((m_bLimSwRight == false) && (iCnt<2000)); // wait some time
00637         
00638         // 7. see why finished (homed or timeout) and log out
00639         if(iCnt>=2000)
00640         {
00641                 std::cout << "Homing failed - limit switch " << iNrDrive << " not reached" << std::endl;
00642                 bRet = false;
00643         }
00644         else
00645         {
00646                 std::cout << "Homing successful - limit switch " << iNrDrive << " ok" << std::endl;
00647                 bRet = true;
00648         }
00649         //IntprtSetInt(8, 'I', 'L', 2, 9); //cob3-2                  |  -----------------------------------------------------------------------------------
00650         //usleep(20000);   
00651 
00652         return bRet;
00653 }
00654 //-----------------------------------------------
00655 void CanDriveHarmonica::setGearPosVelRadS(double dPosGearRad, double dVelGearRadS)
00656 {
00657         int iPosEncIncr;
00658         int iVelEncIncrPeriod;
00659                 
00660         m_DriveParam.PosVelRadToIncr(dPosGearRad, dVelGearRadS, &iPosEncIncr, &iVelEncIncrPeriod);
00661         
00662         if(iVelEncIncrPeriod > m_DriveParam.getVelMax())
00663         {
00664                 iVelEncIncrPeriod = (int)m_DriveParam.getVelMax();
00665         }
00666 
00667         if(iVelEncIncrPeriod < -m_DriveParam.getVelMax())
00668         {
00669                 iVelEncIncrPeriod = (int)-m_DriveParam.getVelMax();
00670         }
00671 
00672         if(m_iTypeMotion == MOTIONTYPE_POSCTRL)
00673         {
00674                         //new: set VELOCITY for PTP Motion              
00675                         IntprtSetInt(8, 'S', 'P', 0, iVelEncIncrPeriod);
00676 
00677                         // Position Relativ ("PR") , because of positioning of driving wheel
00678                         // which is not initialized to zero on a specific position
00679                         // only when command is for homed steering wheel set absolute
00680                         if (m_DriveParam.getIsSteer() == true)
00681                                 IntprtSetInt(8, 'P', 'A', 0, iPosEncIncr);
00682                         else 
00683                                 IntprtSetInt(8, 'P', 'R', 0, iPosEncIncr);
00684 
00685                         IntprtSetInt(4, 'B', 'G', 0, 0);
00686                 
00687         }
00688 
00689         if(m_iTypeMotion == MOTIONTYPE_VELCTRL)
00690         {       
00691                 iVelEncIncrPeriod *= m_DriveParam.getSign();
00692                 IntprtSetInt(8, 'J', 'V', 0, iVelEncIncrPeriod);
00693                 IntprtSetInt(4, 'B', 'G', 0, 0);
00694         }
00695         
00696         // request pos and vel by TPDO1, triggered by SYNC msg
00697         // (to request pos by SDO usesendSDOUpload(0x6064, 0) )
00698         CanMsg msg;
00699         msg.m_iID  = 0x80;
00700         msg.m_iLen = 0;
00701         msg.set(0,0,0,0,0,0,0,0);
00702         m_pCanCtrl->transmitMsg(msg);
00703 }
00704 
00705 //-----------------------------------------------
00706 void CanDriveHarmonica::setGearVelRadS(double dVelGearRadS)
00707 {
00708         int iVelEncIncrPeriod;
00709         
00710         // calc motor velocity from joint velocity
00711         iVelEncIncrPeriod = m_DriveParam.getSign() * m_DriveParam.VelGearRadSToVelMotIncrPeriod(dVelGearRadS);
00712 
00713         if(iVelEncIncrPeriod > m_DriveParam.getVelMax())
00714         {
00715                 std::cout << "SteerVelo asked for " << iVelEncIncrPeriod << " EncIncrements" << std::endl;
00716                 iVelEncIncrPeriod = (int)m_DriveParam.getVelMax();
00717         }
00718 
00719         if(iVelEncIncrPeriod < -m_DriveParam.getVelMax())
00720         {
00721                 std::cout << "SteerVelo asked for " << iVelEncIncrPeriod << " EncIncrements" << std::endl;
00722                 iVelEncIncrPeriod = -1 * (int)m_DriveParam.getVelMax();
00723         }
00724         
00725         IntprtSetInt(8, 'J', 'V', 0, iVelEncIncrPeriod);
00726         IntprtSetInt(4, 'B', 'G', 0, 0);
00727 
00728         // request pos and vel by TPDO1, triggered by SYNC msg
00729         // (to request pos by SDO use sendSDOUpload(0x6064, 0) )
00730         // sync msg is: iID 0x80 with msg (0,0,0,0,0,0,0,0)
00731         CanMsg msg;
00732         msg.m_iID  = 0x80;
00733         msg.m_iLen = 0;
00734         msg.set(0,0,0,0,0,0,0,0);
00735         m_pCanCtrl->transmitMsg(msg);
00736 
00737         // send heartbeat to keep watchdog inactive
00738         msg.m_iID  = 0x700;
00739         msg.m_iLen = 5;
00740         msg.set(0x00,0,0,0,0,0,0,0);
00741         m_pCanCtrl->transmitMsg(msg);
00742 
00743         m_CurrentTime.SetNow();
00744         double dt = m_CurrentTime - m_SendTime;
00745         if ((dt > 1.0) && m_bWatchdogActive)
00746         {
00747                 std::cout << "Time between send velocity of motor " << m_DriveParam.getDriveIdent() 
00748                         << " is too large: " << dt << " s" << std::endl;
00749         }
00750         m_SendTime.SetNow();
00751 
00752 
00753         // request status
00754         m_iCountRequestDiv++;
00755         if (m_iCountRequestDiv > m_Param.iDivForRequestStatus)
00756         {
00757                 requestStatus();
00758                 m_iCountRequestDiv = 0;
00759         }
00760 }
00761 
00762 //-----------------------------------------------
00763 void CanDriveHarmonica::getGearPosRad(double* dGearPosRad)
00764 {
00765         *dGearPosRad = m_dPosGearMeasRad;
00766 }
00767 
00768 //-----------------------------------------------
00769 void CanDriveHarmonica::getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS)
00770 {
00771         *pdAngleGearRad = m_dPosGearMeasRad;
00772         *pdVelGearRadS = m_dVelGearMeasRadS;
00773 }
00774 
00775 //-----------------------------------------------
00776 void CanDriveHarmonica::getGearDeltaPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS)
00777 {
00778         *pdAngleGearRad = m_dPosGearMeasRad - m_dAngleGearRadMem;
00779         *pdVelGearRadS = m_dVelGearMeasRadS;
00780         m_dAngleGearRadMem = m_dPosGearMeasRad;
00781 }
00782 
00783 //-----------------------------------------------
00784 void CanDriveHarmonica::getData(double* pdPosGearRad, double* pdVelGearRadS,
00785                                                                 int* piTorqueCtrl, int* piStatusCtrl)
00786 {
00787         *pdPosGearRad = m_dPosGearMeasRad;
00788         *pdVelGearRadS = m_dVelGearMeasRadS;
00789         *piTorqueCtrl = m_iTorqueCtrl;
00790         *piStatusCtrl = m_iStatusCtrl;
00791 }
00792 
00793 //-----------------------------------------------
00794 void CanDriveHarmonica::requestPosVel()
00795 {
00796         // request pos and vel by TPDO1, triggered by SYNC msg
00797         CanMsg msg;
00798         msg.m_iID  = 0x80;
00799         msg.m_iLen = 0;
00800         msg.set(0,0,0,0,0,0,0,0);
00801         m_pCanCtrl->transmitMsg(msg);
00802         // (to request pos by SDO use sendSDOUpload(0x6064, 0) )
00803 }
00804 
00805 //-----------------------------------------------
00806 void CanDriveHarmonica::sendHeartbeat()
00807 {
00808         CanMsg msg;
00809         msg.m_iID  = 0x700;
00810         msg.m_iLen = 5;
00811         msg.set(0x00,0,0,0,0,0,0,0);
00812         m_pCanCtrl->transmitMsg(msg);
00813 }
00814 
00815 //-----------------------------------------------
00816 void CanDriveHarmonica::requestStatus()
00817 {
00818         IntprtSetInt(4, 'S', 'R', 0, 0);
00819 }
00820 
00821 //-----------------------------------------------
00822 void CanDriveHarmonica::requestMotorTorque()
00823 {       
00824         // send command for requesting motor current:
00825         IntprtSetInt(4, 'I', 'Q', 0, 0);        // active current
00826         //IntprtSetInt(4, 'I', 'D', 0, 0);      // reactive current
00827 }
00828 
00829 //-----------------------------------------------
00830 bool CanDriveHarmonica::isError()
00831 {
00832         if (m_iMotorState != ST_MOTOR_FAILURE)
00833         {
00834                 // Check timeout of can communication
00835                 double dWatchTime = getTimeToLastMsg();
00836 
00837                 if (dWatchTime>m_Param.dCanTimeout)
00838                 {
00839                         if ( m_bOutputOfFailure == false)
00840                         {
00841                                 std::cout << "Motor " << m_DriveParam.getDriveIdent() <<
00842                                         " has no can communication for " << dWatchTime << " s." << std::endl;
00843                         }
00844 
00845                         m_iMotorState = ST_MOTOR_FAILURE;
00846                         m_FailureStartTime.SetNow();
00847                 }
00848                 
00849         }
00850         
00851         return (m_iMotorState == ST_MOTOR_FAILURE);
00852 }
00853 //-----------------------------------------------
00854 bool CanDriveHarmonica::setTypeMotion(int iType)
00855 {
00856         int iMaxAcc = int(m_DriveParam.getMaxAcc());
00857         int iMaxDcc = int(m_DriveParam.getMaxDec());
00858         CanMsg Msg;
00859 
00860         if (iType == MOTIONTYPE_POSCTRL)
00861         {
00862                 // 1.) Switch to UnitMode = 5 (Single Loop Position Control) //
00863         
00864                 // switch off Motor to change Unit-Mode
00865                 IntprtSetInt(8, 'M', 'O', 0, 0);
00866                 usleep(20000);
00867                 // switch Unit-Mode
00868                 IntprtSetInt(8, 'U', 'M', 0, 5);
00869                         
00870                 // set Target Radius to X Increments
00871                 IntprtSetInt(8, 'T', 'R', 1, 15);
00872                 // set Target Time to X ms
00873                 IntprtSetInt(8, 'T', 'R', 2, 100);
00874 
00875                 // set maximum Acceleration to X Incr/s^2               
00876                 IntprtSetInt(8, 'A', 'C', 0, iMaxAcc);
00877                 // set maximum decceleration to X Incr/s^2
00878                 IntprtSetInt(8, 'D', 'C', 0, iMaxDcc);  
00879                 usleep(100000);
00880                 
00881                 
00882         }
00883         else if (iType == MOTIONTYPE_TORQUECTRL)
00884         {
00885                 // Switch to TorqueControll-Mode
00886                 // switch off Motor to change Unit-Mode
00887                 IntprtSetInt(8, 'M', 'O', 0, 0);
00888                 usleep(50000);
00889                 // switch Unit-Mode 1: Torque Controlled
00890                 IntprtSetInt(8, 'U', 'M', 0, 1);
00891                 // disable external compensation input
00892                 // to avoid noise from that input pin
00893                 IntprtSetInt(8, 'R', 'M', 0, 0);
00894 
00895                 // debugging:
00896                 std::cout << "Motor"<<m_DriveParam.getDriveIdent()<<" Unit Mode switched to: TORQUE controlled" << std::endl;
00897                 usleep(100000);
00898         }
00899         else
00900         {
00901                 //Default Motion Type = VelocityControled
00902                 // switch off Motor to change Unit-Mode
00903                 IntprtSetInt(8, 'M', 'O', 0, 0);
00904                 // switch Unit-Mode
00905                 IntprtSetInt(8, 'U', 'M', 0, 2);
00906                 // set profiler Mode (only if Unit Mode = 2)
00907                 IntprtSetInt(8, 'P', 'M', 0, 1);
00908 
00909                 // set maximum Acceleration to X Incr/s^2               
00910                 IntprtSetInt(8, 'A', 'C', 0, iMaxAcc);
00911                 // set maximum decceleration to X Incr/s^2
00912                 IntprtSetInt(8, 'D', 'C', 0, iMaxDcc);  
00913                 usleep(100000);
00914         }
00915         
00916         m_iTypeMotion = iType;
00917         return true;
00918 }
00919 
00920 
00921 //-----------------------------------------------
00922 void CanDriveHarmonica::IntprtSetInt(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, int iData)
00923 {
00924         char cIndex[2];
00925         char cInt[4];
00926         CanMsg CMsgTr;
00927 
00928         CMsgTr.m_iID = m_ParamCanOpen.iRxPDO2;  
00929         CMsgTr.m_iLen = iDataLen;
00930 
00931         cIndex[0] = iIndex;
00932         cIndex[1] = (iIndex >> 8) & 0x3F;  // The two MSB must be 0. Cf. DSP 301 Implementation guide p. 39.
00933 
00934         cInt[0] = iData;
00935         cInt[1] = iData >> 8;
00936         cInt[2] = iData >> 16;
00937         cInt[3] = iData >> 24;
00938 
00939         CMsgTr.set(cCmdChar1, cCmdChar2, cIndex[0], cIndex[1], cInt[0], cInt[1], cInt[2], cInt[3]);
00940         m_pCanCtrl->transmitMsg(CMsgTr);
00941 }
00942 
00943 //-----------------------------------------------
00944 void CanDriveHarmonica::IntprtSetFloat(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, float fData)
00945 {
00946         char cIndex[2];
00947         char cFloat[4];
00948         CanMsg CMsgTr;
00949         char* pTempFloat = NULL;
00950         
00951         CMsgTr.m_iID = m_ParamCanOpen.iRxPDO2;  
00952         CMsgTr.m_iLen = iDataLen;
00953 
00954         cIndex[0] = iIndex;
00955         // for sending float values bit 6 has to be zero and bit 7 one (according to Elmo Implementation guide)
00956         cIndex[1] = (iIndex >> 8) & 0x3F;       // setting bit 6 to zero with mask 0b10111111->0xBF
00957         cIndex[1] = cIndex[1] | 0x80;           // setting bit 7 to one with mask 0b10000000 ->0x80
00958 
00959         pTempFloat = (char*)&fData;
00960         for( int i=0; i<4; i++ )
00961                 cFloat[i] = pTempFloat[i];
00962         
00963         CMsgTr.set(cCmdChar1, cCmdChar2, cIndex[0], cIndex[1], cFloat[0], cFloat[1], cFloat[2], cFloat[3]);
00964         m_pCanCtrl->transmitMsg(CMsgTr);
00965 }
00966 
00967 //-----------------------------------------------
00968 // SDO Communication Protocol, most functions are used for general SDO Segmented Transfer
00969 //-----------------------------------------------
00970 
00971 //-----------------------------------------------
00972 void CanDriveHarmonica::sendSDOAbort(int iObjIndex, int iObjSubIndex, unsigned int iErrorCode)
00973 {
00974         CanMsg CMsgTr;
00975         const int ciAbortTransferReq = 0x04 << 5;
00976         
00977         CMsgTr.m_iLen = 8;
00978         CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
00979 
00980         unsigned char cMsg[8];
00981         
00982         cMsg[0] = ciAbortTransferReq;
00983         cMsg[1] = iObjIndex;
00984         cMsg[2] = iObjIndex >> 8;
00985         cMsg[3] = iObjSubIndex;
00986         cMsg[4] = iErrorCode;
00987         cMsg[5] = iErrorCode >> 8;
00988         cMsg[6] = iErrorCode >> 16;
00989         cMsg[7] = iErrorCode >> 24;
00990 
00991         CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
00992         m_pCanCtrl->transmitMsg(CMsgTr);
00993 }
00994 
00995 //-----------------------------------------------
00996 void CanDriveHarmonica::receivedSDOTransferAbort(unsigned int iErrorCode){
00997         std::cout << "SDO Abort Transfer received with error code: " << iErrorCode;
00998         seg_Data.statusFlag = segData::SDO_SEG_FREE;
00999 }
01000 
01001 //-----------------------------------------------
01002 void CanDriveHarmonica::sendSDOUpload(int iObjIndex, int iObjSubIndex)
01003 {
01004         CanMsg CMsgTr;
01005         const int ciInitUploadReq = 0x40;
01006         
01007         CMsgTr.m_iLen = 8;
01008         CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
01009 
01010         unsigned char cMsg[8];
01011         
01012         cMsg[0] = ciInitUploadReq;
01013         cMsg[1] = iObjIndex;
01014         cMsg[2] = iObjIndex >> 8;
01015         cMsg[3] = iObjSubIndex;
01016         cMsg[4] = 0x00;
01017         cMsg[5] = 0x00;
01018         cMsg[6] = 0x00;
01019         cMsg[7] = 0x00;
01020 
01021         CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
01022         m_pCanCtrl->transmitMsg(CMsgTr);
01023 }
01024 
01025 //-----------------------------------------------
01026 void CanDriveHarmonica::sendSDODownload(int iObjIndex, int iObjSubIndex, int iData)
01027 {
01028         CanMsg CMsgTr;
01029 
01030         const int ciInitDownloadReq = 0x20;
01031         const int ciNrBytesNoData = 0x00;
01032         const int ciExpedited = 0x02;
01033         const int ciDataSizeInd = 0x01;
01034         
01035         CMsgTr.m_iLen = 8;
01036         CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
01037 
01038         unsigned char cMsg[8];
01039         
01040         cMsg[0] = ciInitDownloadReq | (ciNrBytesNoData << 2) | ciExpedited | ciDataSizeInd;
01041         cMsg[1] = iObjIndex;
01042         cMsg[2] = iObjIndex >> 8;
01043         cMsg[3] = iObjSubIndex;
01044         cMsg[4] = iData;
01045         cMsg[5] = iData >> 8;
01046         cMsg[6] = iData >> 16;
01047         cMsg[7] = iData >> 24;
01048 
01049         CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
01050         m_pCanCtrl->transmitMsg(CMsgTr);
01051 }
01052 
01053 //-----------------------------------------------
01054 void CanDriveHarmonica::evalSDO(CanMsg& CMsg, int* pIndex, int* pSubindex)
01055 {
01056         *pIndex = (CMsg.getAt(2) << 8) | CMsg.getAt(1);
01057         *pSubindex = CMsg.getAt(3);
01058 }
01059 
01060 //-----------------------------------------------
01061 int CanDriveHarmonica::getSDODataInt32(CanMsg& CMsg)
01062 {
01063         int iData = (CMsg.getAt(7) << 24) | (CMsg.getAt(6) << 16) |
01064                 (CMsg.getAt(5) << 8) | CMsg.getAt(4);
01065 
01066         return iData;
01067 }
01068 
01069 //-----------------------------------------------
01070 int CanDriveHarmonica::receivedSDOSegmentedInitiation(CanMsg& msg) {
01071 
01072         if(seg_Data.statusFlag == segData::SDO_SEG_FREE || seg_Data.statusFlag == segData::SDO_SEG_WAITING) { //only accept new SDO Segmented Upload if seg_Data is free
01073                 seg_Data.resetTransferData();
01074                 seg_Data.statusFlag = segData::SDO_SEG_COLLECTING;
01075 
01076                 //read out objectIDs
01077                 evalSDO(msg, &seg_Data.objectID, &seg_Data.objectSubID);
01078 
01079                 //data in byte 4 to 7 contain the number of bytes to be uploaded (if Size indicator flag is set)
01080                 if( (msg.getAt(0) & 0x01) == 1) {
01081                         seg_Data.numTotalBytes = msg.getAt(7) << 24 | msg.getAt(6) << 16 | msg.getAt(5) << 8 | msg.getAt(4);
01082                 } else seg_Data.numTotalBytes = 0;
01083 
01084                 sendSDOUploadSegmentConfirmation(seg_Data.toggleBit);
01085         }
01086 
01087         return 0;
01088 
01089 }
01090 
01091 //-----------------------------------------------
01092 int CanDriveHarmonica::receivedSDODataSegment(CanMsg& msg){
01093 
01094         int numEmptyBytes = 0;
01095 
01096         //Read SDO Upload Protocol:
01097         //Byte 0: SSS T NNN C | SSS=Cmd-Specifier, T=ToggleBit, NNN=num of empty bytes, C=Finished
01098         //Byte 1 to 7: Data
01099 
01100         if( (msg.getAt(0) & 0x10) != (seg_Data.toggleBit << 4) ) { 
01101                 std::cout << "Toggle Bit error, send Abort SDO with \"Toggle bit not alternated\" error" << std::endl;
01102                 sendSDOAbort(seg_Data.objectID, seg_Data.objectSubID, 0x05030000); //Send SDO Abort with error code Toggle-Bit not alternated
01103                 return 1;
01104         }
01105                 
01106         if( (msg.getAt(0) & 0x01) == 0x00) { //Is finished-bit not set?
01107                 seg_Data.statusFlag = segData::SDO_SEG_COLLECTING;
01108         } else {
01109                 //std::cout << "SDO Segmented Transfer finished bit found!" << std::endl;
01110                 seg_Data.statusFlag = segData::SDO_SEG_PROCESSING;
01111         };
01112 
01113         numEmptyBytes = (msg.getAt(0) >> 1) & 0x07;
01114         //std::cout << "NUM empty bytes in SDO :" << numEmptyBytes << std::endl;
01115         
01116         for(int i=1; i<=7-numEmptyBytes; i++) {
01117                 seg_Data.data.push_back(msg.getAt(i));
01118         }
01119 
01120         if(seg_Data.statusFlag == segData::SDO_SEG_PROCESSING) {
01121                 finishedSDOSegmentedTransfer();         
01122         } else {
01123                 seg_Data.toggleBit = !seg_Data.toggleBit;
01124                 sendSDOUploadSegmentConfirmation(seg_Data.toggleBit);
01125         }
01126                 
01127         return 0;
01128 }
01129 
01130 //-----------------------------------------------
01131 void CanDriveHarmonica::sendSDOUploadSegmentConfirmation(bool toggleBit) {
01132 
01133         CanMsg CMsgTr;
01134         int iConfirmSegment = 0x60; //first three bits must be css = 3 : 011 00000
01135         iConfirmSegment = iConfirmSegment | (toggleBit << 4); //fourth bit is toggle bit: 011T0000
01136         
01137         CMsgTr.m_iLen = 8;
01138         CMsgTr.m_iID = m_ParamCanOpen.iRxSDO;
01139 
01140         unsigned char cMsg[8];
01141         
01142         cMsg[0] = iConfirmSegment;
01143         cMsg[1] = 0x00;
01144         cMsg[2] = 0x00;
01145         cMsg[3] = 0x00;
01146         cMsg[4] = 0x00;
01147         cMsg[5] = 0x00;
01148         cMsg[6] = 0x00;
01149         cMsg[7] = 0x00;
01150 
01151         CMsgTr.set(cMsg[0], cMsg[1], cMsg[2], cMsg[3], cMsg[4], cMsg[5], cMsg[6], cMsg[7]);
01152         m_pCanCtrl->transmitMsg(CMsgTr);
01153 }
01154 
01155 //-----------------------------------------------
01156 void CanDriveHarmonica::finishedSDOSegmentedTransfer() {
01157         seg_Data.statusFlag = segData::SDO_SEG_PROCESSING;
01158         
01159         if( (seg_Data.data.size() != seg_Data.numTotalBytes) & (seg_Data.numTotalBytes != 0) ) {
01160                 std::cout << "WARNING: SDO tranfer finished but number of collected bytes " 
01161                         << seg_Data.data.size() << " != expected number of bytes: " << seg_Data.numTotalBytes << std::endl;
01162                 //abort processing?
01163         }
01164         
01165         if(seg_Data.objectID == 0x2030) {
01166                 if(ElmoRec->processData(seg_Data) == 0) seg_Data.statusFlag = segData::SDO_SEG_FREE;
01167         }
01168 }
01169 
01170 //-----------------------------------------------
01171 double CanDriveHarmonica::estimVel(double dPos)
01172 {
01173         double dVel;
01174         double dt;
01175 
01176         m_CurrentTime.SetNow();
01177 
01178         dt = m_CurrentTime - m_VelCalcTime;
01179 
01180         dVel = (dPos - m_dOldPos)/dt;
01181 
01182         m_dOldPos = dPos;
01183         m_VelCalcTime.SetNow();
01184 
01185         return dVel;
01186 }
01187 //-----------------------------------------------
01188 bool CanDriveHarmonica::evalStatusRegister(int iStatus)
01189 {
01190         bool bNoError;
01191 
01192         // --------- Error status
01193         if( isBitSet(iStatus, 0) )
01194         {
01195                 // ------------ Error
01196                 if ( m_bOutputOfFailure == false )
01197                 {
01198                         std::cout << "Error of drive: " << m_DriveParam.getDriveIdent() << std::endl;
01199 
01200                         if( (iStatus & 0x0000000E) == 2)
01201                                 std::cout << "- drive error under voltage" << std::endl;
01202 
01203                         if( (iStatus & 0x0000000E) == 4)
01204                                 std::cout << "- drive error over voltage" << std::endl;
01205 
01206                         if( (iStatus & 0x0000000E) == 10)
01207                                 std::cout << "- drive error short circuit" << std::endl;
01208 
01209                         if( (iStatus & 0x0000000E) == 12)
01210                                 std::cout << "- drive error overheating" << std::endl;
01211 
01212                         // Request detailed description of failure
01213                         IntprtSetInt(4, 'M', 'F', 0, 0);
01214                 }
01215 
01216                 m_iNewMotorState = ST_MOTOR_FAILURE;
01217 
01218                 bNoError = false;
01219         }
01220         else if ( isBitSet(iStatus, 6) )
01221         {
01222                 // General failure 
01223                 if ( m_bOutputOfFailure == false )
01224                 {
01225                         std::cout << "Motor " << m_DriveParam.getDriveIdent() << " failure latched" << std::endl;
01226 
01227                         // Request detailed description of failure
01228                         IntprtSetInt(4, 'M', 'F', 0, 0);
01229 
01230                         m_FailureStartTime.SetNow();
01231                 }
01232                 m_iNewMotorState = ST_MOTOR_FAILURE;
01233 
01234                 bNoError = false;
01235         }
01236         else
01237         {
01238                 // ---------- No error 
01239                 bNoError = true;
01240 
01241                 // Clear flag for failure output only if at least one
01242                 // status message without error has been received.
01243                 // Printing an error message on recovery is avoided.
01244                 m_bOutputOfFailure = false;
01245 
01246                 // --------- General status bits
01247                 // check if Bit 4 (-> Motor is ON) ist set
01248                 if( isBitSet(iStatus, 4) )
01249                 {
01250                         if (m_iMotorState != ST_OPERATION_ENABLED)
01251                         {
01252                                 std::cout << "Motor " << m_DriveParam.getDriveIdent() << " operation enabled" << std::endl;
01253                                 m_FailureStartTime.SetNow();
01254                         }
01255 
01256                         m_iNewMotorState = ST_OPERATION_ENABLED;
01257                 }
01258                 else
01259                 {
01260                         if (m_iMotorState != ST_OPERATION_DISABLED)
01261                         {
01262                                 std::cout << "Motor " << m_DriveParam.getDriveIdent() << " operation disabled" << std::endl;
01263                         }
01264 
01265                         m_iNewMotorState = ST_OPERATION_DISABLED;
01266                 }
01267 
01268                 // Current limit
01269                 if( isBitSet(iStatus, 13) )
01270                 {
01271                         if (m_bCurrentLimitOn == false)
01272                                 std::cout << "Motor " << m_DriveParam.getDriveIdent() << "current limit on" << std::endl;
01273 
01274                         m_bCurrentLimitOn = true;
01275                 }
01276                 else
01277                         m_bCurrentLimitOn = false;
01278         }
01279 
01280         // Change state
01281         m_iMotorState = m_iNewMotorState;
01282 
01283         if (m_iMotorState == ST_MOTOR_FAILURE)
01284                 m_bOutputOfFailure = true;
01285 
01286         return bNoError;
01287 }
01288 
01289 //-----------------------------------------------
01290 void CanDriveHarmonica::evalMotorFailure(int iFailure)
01291 {
01292 
01293         std::cout << "Motor " << m_DriveParam.getDriveIdent() << " has a failure: " << iFailure << std::endl;
01294         
01295         if( isBitSet(iFailure, 2) )
01296         {
01297                 std::cout << "- feedback loss" << std::endl;
01298         }
01299 
01300         if( isBitSet(iFailure, 3) )
01301         {
01302                 std::cout << "- peak current excced" << std::endl;
01303         }
01304 
01305         if( isBitSet(iFailure, 7) )
01306         {
01307                 std::cout << "- speed track error" << std::endl;
01308         }
01309         
01310         if( isBitSet(iFailure, 8) )
01311         {
01312                 std::cout << "- position track error" << std::endl;
01313         }
01314 
01315         if( isBitSet(iFailure, 17) )
01316         {
01317                 std::cout << "- speed limit exceeded" << std::endl;
01318         }
01319         
01320         if( isBitSet(iFailure, 21) )
01321         {
01322                 std::cout << "- motor stuck" << std::endl;
01323         }
01324 }
01325 
01326 //-----------------------------------------------
01327 void CanDriveHarmonica::setMotorTorque(double dTorqueNm)
01328 {
01329         // convert commanded motor current into amperes
01330         float fMotCurr = m_DriveParam.getSign() * dTorqueNm / m_DriveParam.getCurrToTorque();
01331 
01332         // check for limitations
01333         if  (fMotCurr > m_DriveParam.getCurrMax())
01334         {
01335                 fMotCurr = m_DriveParam.getCurrMax();
01336                 std::cout << "Torque command too high: " << fMotCurr << " Nm. Torque has been limitited." << std::endl;
01337         }
01338         if (fMotCurr < -m_DriveParam.getCurrMax())
01339         {
01340                 fMotCurr = -m_DriveParam.getCurrMax();
01341                 std::cout << "Torque command too high: " << fMotCurr << " Nm. Torque has been limitited." << std::endl;
01342         }
01343 
01344         // send Command 
01345         IntprtSetFloat(8, 'T', 'C', 0, fMotCurr);
01346 
01347         // request pos and vel by TPDO1, triggered by SYNC msg
01348         CanMsg msg;
01349         msg.m_iID  = 0x80;
01350         msg.m_iLen = 0;
01351         msg.set(0,0,0,0,0,0,0,0);
01352         m_pCanCtrl->transmitMsg(msg);
01353 
01354         // send heartbeat to keep watchdog inactive
01355         sendHeartbeat();
01356         
01357         m_CurrentTime.SetNow();
01358         double dt = m_CurrentTime - m_SendTime;
01359         if (dt > 1.0)
01360         {
01361                 std::cout << "Time between send current/torque of motor " << m_DriveParam.getDriveIdent() 
01362                         << " is too large: " << dt << " s" << std::endl;
01363         }
01364         m_SendTime.SetNow();
01365 
01366 
01367         // request status
01368         m_iCountRequestDiv++;
01369         if (m_iCountRequestDiv > m_Param.iDivForRequestStatus)
01370         {
01371                 requestStatus();
01372                 m_iCountRequestDiv = 0;
01373         }
01374 
01375 }
01376 
01377 //-----------------------------------------------
01378 void CanDriveHarmonica::getMotorTorque(double* dTorqueNm)
01379 {
01380         // With motor sign:
01381         *dTorqueNm = m_DriveParam.getSign() * m_dMotorCurr * m_DriveParam.getCurrToTorque();
01382         
01383 }
01384 
01385 
01386 
01387 //----------------
01388 //----------------
01389 //Function, that proceeds (Elmo-) recorder readout
01390 
01391 //-----------------------------------------------
01392 int CanDriveHarmonica::setRecorder(int iFlag, int iParam, std::string sParam) {
01393 
01394         switch(iFlag) {
01395                 case 0: //Configure Elmo Recorder for new Record, param = iRecordingGap, which specifies every which time quantum (4*90usec) a new data point is recorded
01396                         if(iParam < 1) iParam = 1;
01397                         ElmoRec->isInitialized(true);
01398                         ElmoRec->configureElmoRecorder(iParam, m_DriveParam.getDriveIdent()); //int startImmediately is default = 1
01399                         return 0;
01400                                         
01401                 case 1: //Query upload of previous recorded data, data is being proceeded after complete upload, param = recorded ID, filename
01402                         if(!ElmoRec->isInitialized(false)) return 1;
01403                         
01404                         if(seg_Data.statusFlag == segData::SDO_SEG_FREE) {
01405                                 if( (iParam != 1) && (iParam != 2) && (iParam != 10) && (iParam != 16) ) {
01406                                         iParam = 1;
01407                                         std::cout << "Changed the Readout object to #1 as your selected object hasn't been recorded!" << std::endl;
01408                                 }
01409                                 ElmoRec->setLogFilename(sParam);
01410                                 seg_Data.statusFlag = segData::SDO_SEG_WAITING;
01411                                 ElmoRec->readoutRecorderTry(iParam); //as subindex, give the recorded variable
01412                                 return 0;
01413                         } else {
01414                                 std::cout << "Previous transmission not finished or colected data hasn't been proceeded yet" << std::endl;
01415                                 return 2;
01416                         }
01417                         
01418                         break;
01419                         
01420                 case 2: //request status, still collecting data during ReadOut process?
01421                         if(seg_Data.statusFlag == segData::SDO_SEG_COLLECTING) {
01422                                 //std::cout << "Transmission of data is still in progress" << std::endl;
01423                                 return 2;
01424                         } else if(seg_Data.statusFlag == segData::SDO_SEG_PROCESSING) {
01425                                 //std::cout << "Transmission of data finished, data not proceeded yet" << std::endl;
01426                                 return 2;
01427                         } else if(seg_Data.statusFlag == segData::SDO_SEG_WAITING) {
01428                                 //std::cout << "Still waiting for transmission to begin" << std::endl;
01429                                 return 2;
01430                         } else { //finished transmission and finished proceeding
01431                                 return 0;
01432                         }
01433                         
01434                         break;
01435                         
01436                 case 99: //Abort ongoing SDO data Transmission and clear collected data
01437                         sendSDOAbort(0x2030, 0x00, 0x08000020); //send general error abort
01438                         seg_Data.resetTransferData(); 
01439                         return 0;
01440         }
01441 
01442         return 0;
01443 }


cob_canopen_motor
Author(s): Christian Connette
autogenerated on Sun Oct 5 2014 23:02:12