moveCommand.h

Go to the documentation of this file.

#ifndef __MOVE_COMMAND_H__
#define __MOVE_COMMAND_H__

#include <schunk_powercube_chain/datastructsManipulator.h>
#include <schunk_powercube_chain/TimeStamp.h>

class moveCommand
{
public:
        moveCommand() {;}
        virtual ~moveCommand() {;}

        //               Child Classes have to define these:                   //

        virtual double getPos(double TimeElapsed)=0;

        virtual double getVel(double TimeElapsed)=0;

        virtual double getTotalTime()=0;

        //                Functions useful for simulation:                     //
        //          start() has to be called before the other functions        //

        virtual void start() { m_timeStarted.SetNow(); }

        virtual bool isActive() { m_now.SetNow(); return ( m_now-m_timeStarted > getTotalTime() )?false:true;
                                                          std::cerr << "========Total Time: " << getTotalTime() << "\n";}

        virtual double pos() { m_now.SetNow(); return getPos( m_now-m_timeStarted ); }

        virtual double vel() { m_now.SetNow(); return getVel( m_now-m_timeStarted ); }

        virtual double timeRemaining() { m_now.SetNow(); return (m_timeStarted - m_now) + getTotalTime(); }

protected:
        TimeStamp m_timeStarted;
        TimeStamp m_now;
};



class RampCommand : public moveCommand
{
public:
        RampCommand(double x0, double v0, double xtarget, double amax, double vmax);
        RampCommand(const RampCommand& rc);

        virtual RampCommand& operator=(const RampCommand& rc);

        virtual ~RampCommand() { if (m_nachumkehr) delete m_nachumkehr; }

        virtual double getPos(double TimeElapsed);
        double getPos() { return moveCommand::pos(); }

        virtual double getVel(double TimeElapsed);
        double getVel() { return moveCommand::vel(); }

        virtual double getTotalTime();

        virtual bool inPhase1() { m_now.SetNow(); return ( m_now-m_timeStarted <= m_T1 )?true:false; }
        virtual bool inPhase3()
        { m_now.SetNow(); return ( m_now-m_timeStarted > m_T1 + m_T2 && m_now-m_timeStarted < m_T1 + m_T2 + m_T3)?true:false; }

        virtual double T1() { return (m_umkehr)?(m_T1 + m_nachumkehr->T1()):m_T1; }
        virtual double T2() { return (m_umkehr)?m_nachumkehr->T2():m_T2; }
        virtual double T3() { return (m_umkehr)?m_nachumkehr->T3():m_T3; }

        static void calculateAV(double x0, double v0, double xtarget, double time, double T3, double amax,
                                                    double vmax, double& a, double& v);

private:

        static std::ofstream debug;

        double m_x0, m_v0;
        double m_xtarget;
        double m_amax, m_vmax;

        double m_T1, m_T2, m_T3;
        double m_a1, m_v2, m_a3;
        bool m_umkehr;
        RampCommand * m_nachumkehr;
};

#endif