CanDriveHarmonica.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#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