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