cinosmovepathtrajectory.h

Go to the documentation of this file.

//******************************************************************************
//******************************************************************************
#ifndef INC_CINOSMOVEPATHTRAJECTORY_H
#define INC_CINOSMOVEPATHTRAJECTORY_H
//------------------------------------------------------------------------------
// defines
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
// includes
//------------------------------------------------------------------------------
//
// system
#include <inos.h>
#include <cinosbaseramp.h>
//
// C++
//
// project
//
//------------------------------------------------------------------------------
// struct definition
//------------------------------------------------------------------------------
//
struct SINOSMovePathActual
{
    uint32 m_uSegmentId;
    double m_dPosition;
    double m_dVelocity;
    double m_dAcceleration;
    double m_dJerk;
 
    static SINOSMovePathActual* Alloc();
    static void Free(SINOSMovePathActual* apActual);
 
    SINOSMovePathActual* m_pNext;
    static SINOSMovePathActual* m_pFirst;
 
    // dynamic
    DECLARE_DYNAMIC(SINOSMovePathActual);
};
//
//------------------------------------------------------------------------------
// class definition
//------------------------------------------------------------------------------
// 
class CINOSMovePathTrajectory
{
    //--- user interface ---------------------------------------------------
 
    // public member functions
    public :
 
        virtual uint32 GetMoveTime(double& adMoveTime, double adDelta = 0.0)
            {
                // base class always returns a 'not supported' error;
                return INOS_MOVEPATH_ERROR_TRJ_NOT_SUPPORTED;
            }
 
        virtual uint32 GetMoveTicks(uint64& adMoveTicks, double arDelta = 0.0);
 
        virtual uint32 GetMoveLength(double& adMoveLength);
 
        virtual uint32 GetMoveRemaining(double& adMoveRemaining);
 
        virtual uint32 GetMaxConstantVelocity(double& adMaxConstantVelocity,
                CINOSMovePathCurve* apCurve);
 
        virtual uint32 Prepare();
 
        virtual uint32 PrepareAppend() = 0;
 
        virtual uint32 Append() = 0;
 
        virtual uint32 PrepareRun();
        virtual uint32 Run(TINOSMovePathVec& vOffset, bool abDelegate);
        virtual uint32 Stop() = 0;
        virtual uint32 Break() = 0;
        virtual uint32 Continue() = 0;
        virtual uint32 Cancel();
 
        virtual uint32 GetData(
            TINOSMovePathVec& vPos,
            TINOSMovePathVec& vVel,
            TINOSMovePathVec& vAcc,
            TINOSMovePathVec& vJrk);
        virtual uint32 GetData(uint32& ouSegmentId,
            TINOSMovePathVec& vPos,
            TINOSMovePathVec& vVel,
            TINOSMovePathVec& vAcc,
            TINOSMovePathVec& vJrk);
 
        virtual CINOSMovePathSegment* GetActSegment() {
            return m_pActSegment;
        }
 
        virtual uint32 GetBufferSegment(
                uint32 auOffset,
                CINOSMovePathSegment*& opSegment);
 
        virtual uint32 CancelData();
        uint32 GetBufferFillLevel() const;
 
        virtual uint32 HandleData(
            CINOSMovePathSegment* apSegment,
            TINOSMovePathVec &avPos,
            TINOSMovePathVec &avVel,
            TINOSMovePathVec &avAcc,
            TINOSMovePathVec &avJrk);
 
        enum {
            eFlgNone            = 0x00000000,
            eFlgStopSupported   = 0x00000001,
            eFlgNeedsInterpolatorSegments   = 0x00000002,
        };
        uint32 GetFlags()
            { return m_uFlags; };
 
        void GetActual(SINOSRampData& aActual)
            {
                aActual.m_rS = m_Actual.m_dPosition;
                aActual.m_rV = m_Actual.m_dVelocity;
                aActual.m_rA = m_Actual.m_dAcceleration;
                aActual.m_rJ = m_Actual.m_dJerk;
            }
 
    //--- internals --------------------------------------------------------
 
    friend class CINOSMovePath;
    friend class CINOSMcRobot;
 
    // constructor / destructor
    public :
        CINOSMovePathTrajectory (class CINOSMovePath* apPath = NULL);
        virtual ~CINOSMovePathTrajectory ();
 
    // protected members
    protected :
        class CINOSMovePath* GetPath()
            { return m_pPath; };
        void SetPath(class CINOSMovePath* apPath)
            { m_pPath = apPath; };
        class CINOSMovePathCurve* GetPrepareCurve();
        class CINOSMovePathCurve* GetRunCurve();
        class CINOSMovePathResourceAxis* GetAxis(uint32 auIndex);
 
        virtual uint32 iPrepare() = 0;
        virtual uint32 PrepareEvents();
        virtual bool GenData(CINOSMovePathSegment*& opSegment, double& adP,
            TINOSMovePathVec& vPos) = 0;
        enum {
            ePutDataFlgNone = 0x00000000,       // none
            ePutDataFlgWaitIfFull = 0x00000001, // wait if buffer full
        };
        virtual uint32 PutData( CINOSMovePathSegment* apSegment, double adP,
            TINOSMovePathVec& vPos, uint32 auFlags);
 
        uint32 PutCmd(uint32 auCommand);
        uint32 HandleCmd(uint32 auError = INOS_OK);
        uint32 HandlePrestartEvent();
        uint32 HandleEvents();
        void EmitEvent(CINOSMovePathEvent* apEvent, uint64 auTicksStart);
        uint32 WaitCmd(uint32 auCommand, uint32 auError = INOS_MOVEPATH_ERROR_NO_COMMAND_QUEUE);
 
        virtual void iActiveHook();
 
        virtual uint32 iCancelData();
 
        double CalcRotationAngle(TINOSMovePathVec& vUnitVector1,
            TINOSMovePathVec& vUnitVector2);
        void CalcFollow(TINOSMovePathVec& vPos,
            TINOSMovePathVec& vDir);
        void GetPositionLast(TINOSMovePathVec& ovLast);
 
        virtual CINCOObject* RequestRegister();
 
        void ReleaseRegister()
        {
            // check counter
            if (m_uRegisterCnt > 0) {
                // dec count
                m_uRegisterCnt--;
                // destroy ?
                if (m_uRegisterCnt == 0) {
                    // yes
                    delete m_pRegister;
                    // done
                    m_pRegister = nullptr;
                } // end if
            } // end if
 
        } // end ReleaseRegister
 
        CINCOObject* GetRegister()
        {
            return m_pRegister;
        }
 
    // protected members
    protected :             
        inosName32 m_cType;
        uint32 m_uFlags;
        class CINOSMovePath* m_pPath;
        double m_dCycleTime;
        double m_dCycleTime_1;
        double m_dSlowMotion;
        uint8 m_uMainAxisIndex;
        uint8 m_uMainOversampling = 1;
        inos_std::vector<uint8> m_uMainCycleDownSampling;
 
        inos_std::vector<CINOSMovePathResource*> m_vResources;
 
        uint32 m_uNoResources = 0;
 
        char m_cSpeedSet[64] = {0};
        SINOSRampParam m_sSpeedSet;
 
        enum {
            eFlgIntCancelRequest = 0x00000001
        };
        uint32 m_uFlagsInternal = 0;
 
        // buffer handling
 
        enum EBufferType {
            eTypeActive,
            eTypePassive
        } m_eBufferType;
 
        enum {
            eFlgFull = 0x00000001,
            eFlgNoMoreData = 0x00000002,
            eFlgWrapAround = 0x00000004,
            eFlgCalculated = 0x00000008,
            eFlgDelegate = 0x00000010,
            eFlgFollow = 0x00000020,
            eFlgEnd = 0x00000040,
        };
        uint32 m_uBufferFlags;
        uint32 m_uBufferSize;
        uint32 m_uBufferReady;
        uint32 m_uBufferLow;
        uint32 m_uBufferWr;
        uint32 m_uBufferTotalWr;
        uint32 m_uBufferRd;
        uint32 m_uBufferTotalRd;
        uint32 m_uBufferCnt;
        CINOSMovePathSegment** m_ppBufferSegment;
        double* m_pBufferP;
        TINOSMovePathVec* m_pBuffer;
        uintid m_uActiveHookId;
        TINOSMovePathVec m_vOffset;
 
        // path logging handling
 
        CINOSMovePathSegment* m_pPathSegment;
 
        // generating data
 
        uint64 m_uTicksStart;
        uint32 m_uTicksCurrent;
        uint8 m_uBusId;
        uint32 m_uBusTicksPerMs;
        uint32 m_uBusTicksPerAxisTick;
        double m_dTime;
        double m_dP;
 
        // running data
        SINOSMovePathActual m_Actual = {0};
        SINOSMovePathActual* m_pActual;
        double m_dActP = 0.0;
        TINOSMovePathVec m_vPos;
        TINOSMovePathVec m_vVel;
        TINOSMovePathVec m_vAcc;
        TINOSMovePathVec m_vJrk;
 
        uint8 m_uCycleDownSampling[DF_INOS_MOVEPATH_MAX_AXES];
 
 
        CINOSMovePathSegment* m_pActSegment = nullptr;
        CINOSMovePathSegment* m_pNextSegment = nullptr;
        inos_std::vector<uint8>  m_vCycleDownSampling;
 
        // filter handling
 
        uint32 m_uFilterLength;
        double m_dFilterFactor;
        TINOSMovePathVec m_vFilterPos;
 
        // follow mechanism
 
        uint32 m_uFollowIndex;
        double m_dFollowFactor;
        double m_dFollowAngle;
        TINOSMovePathVec m_vFollowDir;
 
        uint32 m_uRegisterCnt = 0;
        CINCOObject* m_pRegister = nullptr;
 
    // dynamic
    DECLARE_DYNAMIC(CINOSMovePathTrajectory);
};
 
//------------------------------------------------------------------------------
// end of file
//------------------------------------------------------------------------------
 
#endif  // INC_CINOSMOVEPATHTRAJECTORY_H