CanDriveHarmonica.h

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2010
 *
 * Fraunhofer Institute for Manufacturing Engineering   
 * and Automation (IPA)
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Project name: care-o-bot
 * ROS stack name: cob_driver
 * ROS package name: cob_canopen_motor
 * Description:
 *                                                              
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *                      
 * Author: Christian Connette, email:christian.connette@ipa.fhg.de
 * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
 *
 * Date of creation: Feb 2009
 * ToDo: - Assign Adsress of digital input for homing switch "iHomeDigIn" via parameters (in evalReceived Message, Line 116).
 *       - Homing Event should be defined by a parameterfile and handed to CanDrive... e.g. via the DriveParam.h (in inithoming, Line 531).
 *               - Check whether "requestStatus" can/should be done in the class implementing the component
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Fraunhofer Institute for Manufacturing 
 *       Engineering and Automation (IPA) nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as 
 * published by the Free Software Foundation, either version 3 of the 
 * License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public 
 * License LGPL along with this program. 
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/


#ifndef CANDRIVEHARMONICA_INCLUDEDEF_H
#define CANDRIVEHARMONICA_INCLUDEDEF_H

//-----------------------------------------------
#include <cob_canopen_motor/CanDriveItf.h>
#include <cob_utilities/TimeStamp.h>

#include <cob_canopen_motor/SDOSegmented.h>
#include <cob_canopen_motor/ElmoRecorder.h>
//-----------------------------------------------

class CanDriveHarmonica : public CanDriveItf
{
public:
        // ------------------------- Types
        struct ParamType
        {
                int iNumRetryOfSend;
                int iDivForRequestStatus;
                double dCanTimeout;     
        };

        enum StateHarmonica
        {
                ST_PRE_INITIALIZED,
                ST_OPERATION_ENABLED,
                ST_OPERATION_DISABLED,
                ST_MOTOR_FAILURE
        };

        struct ParamCanOpenType
        {
                int iTxPDO1;
                int iTxPDO2;
                int iRxPDO2;
                int iTxSDO;
                int iRxSDO;
        };

        // ------------------------- Interface
        void setCanItf(CanItf* pCanItf){ m_pCanCtrl = pCanItf; }

        bool init();

        bool isInitialized() { return m_bIsInitialized; }
                        
        bool start();

        bool stop();
        bool reset();

        bool shutdown();

        bool disableBrake(bool bDisabled);

        bool initHoming();

        bool execHoming();
        
        bool endHoming();

        double getTimeToLastMsg();

        bool getStatusLimitSwitch();

        bool startWatchdog(bool bStarted);

        bool evalReceivedMsg(CanMsg& msg);

        bool evalReceivedMsg() { return true; }


        void setGearPosVelRadS(double dPosRad, double dVelRadS);

        void setGearVelRadS(double dVelEncRadS);

        bool setTypeMotion(int iType);

        void getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS);

        void getGearDeltaPosVelRadS(double* pdDeltaAngleGearRad, double* pdVelGearRadS);

        void getGearPosRad(double* pdPosGearRad);

        void setDriveParam(DriveParam driveParam) { m_DriveParam = driveParam; }

        bool isError();
        
        unsigned int getError() { return 0; }

        // ------ Dummy implementations for completing the interface

        void requestPosVel();

        void requestStatus();

        void getStatus(int* piStatus, int* piTempCel) { *piStatus = 0; *piTempCel = 0; }


        // ------------------------- Interface: Harmonica specific
        CanDriveHarmonica();

        void getData(double* pdPosGearRad, double* pdVelGearRadS,
                int* piTorqueCtrl, int* piStatusCtrl);

        void setCanOpenParam( int iTxPDO1, int iTxPDO2, int iRxPDO2, int iTxSDO, int iRxSDO);
        
        void IntprtSetInt(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, int iData);
        
        void sendHeartbeat();
        

        bool setEMStop() {
                std::cout << "The function setEMStop() is not implemented!!!" << std::endl;
                return false;
        }
        
        bool resetEMStop() {
                std::cout << "The function resetEMStop() is not implemented!!!" << std::endl;
                return false;
        }

        void setMotorTorque(double dTorqueNm);

        void requestMotorTorque();
        
        void getMotorTorque(double* dTorqueNm);

        int setRecorder(int iFlag, int iParam = 0, std::string sParam = "/home/MyLog_");
        
        
        //--------------------------
        //CanDriveHarmonica specific functions (not from CanDriveItf)
        //--------------------------
        void IntprtSetFloat(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, float fData);

        void sendSDOUpload(int iObjIndex, int iObjSub);
        
    void sendSDOAbort(int iObjIndex, int iObjSubIndex, unsigned int iErrorCode);

        void sendSDODownload(int iObjIndex, int iObjSub, int iData);
        
        void evalSDO(CanMsg& CMsg, int* pIndex, int* pSubindex);
        
        int getSDODataInt32(CanMsg& CMsg);
        
    
protected:
        // ------------------------- Parameters
        ParamCanOpenType m_ParamCanOpen;
        DriveParam m_DriveParam;
        bool m_bLimitSwitchEnabled;
        ParamType m_Param;

        // ------------------------- Variables
        CanItf* m_pCanCtrl;
        CanMsg m_CanMsgLast;

        ElmoRecorder* ElmoRec;

        int m_iTypeMotion;
        int m_iStatusCtrl;
        int     m_iTorqueCtrl;

        TimeStamp m_CurrentTime;
        TimeStamp m_WatchdogTime;
        TimeStamp m_VelCalcTime;
        TimeStamp m_FailureStartTime;
        TimeStamp m_SendTime;
        TimeStamp m_StartTime;

        double m_dAngleGearRadMem;
        double m_dVelGearMeasRadS;
        double m_dPosGearMeasRad;

        bool m_bLimSwLeft;
        bool m_bLimSwRight;

        double m_dOldPos;

        std::string m_sErrorMessage;

        int m_iMotorState;
        int m_iNewMotorState;

        int m_iCountRequestDiv;

        bool m_bCurrentLimitOn;

        int m_iNumAttempsRecFail;

        bool m_bOutputOfFailure;

        bool m_bIsInitialized;

        double m_dMotorCurr;

        bool m_bWatchdogActive;

        segData seg_Data;


        // ------------------------- Member functions
        double estimVel(double dPos);

        bool evalStatusRegister(int iStatus);
        void evalMotorFailure(int iFailure);
        
        int m_iPartnerDriveRatio;
        int m_iDistSteerAxisToDriveWheelMM;

        bool isBitSet(int iVal, int iNrBit)
        {
                if( (iVal & (1 << iNrBit)) == 0)
                        return false;
                else
                        return true;
        }

        void sendSDOUploadSegmentConfirmation(bool toggleBit);
    
        int receivedSDODataSegment(CanMsg& msg);

        int receivedSDOSegmentedInitiation(CanMsg& msg);
    
        void receivedSDOTransferAbort(unsigned int iErrorCode);
    
        void finishedSDOSegmentedTransfer();

};
//-----------------------------------------------
#endif