mitlib.pub/MITLib/IODll/MotionProfile.h

#ifndef MOTION_CMotionProfile_H
#define MOTION_CMotionProfile_H

#ifdef	BUILD_IODLL
#define		IODLLCLASS	__declspec(dllexport)
#else
#define		IODLLCLASS	__declspec(dllimport)
#endif

#define MAX_PVT 100
/**
 * @class CMotionProfile
 * This class handles creation and calculation on motor CMotionProfiles
 */
class IODLLCLASS CMotionProfile
{
public:
    /**
     * Constructor
     */
    CMotionProfile(void);

    /**
     * Constructor
     */
    CMotionProfile(const CMotionProfile &p);

    /**
     * Add another CMotionProfile to this one.
     */
    void Append(const CMotionProfile &p);

    /**
     * Destructor
     */
    virtual ~CMotionProfile(void);

    /**
     * Prints PVT table to the console
     */
    void Dump() const;

    /**
     * Prints CMotionProfile in steps of delta_t to the console.
     * Format: time, position, velocity
     * @param delta_t Time in seconds. Example 0.01
     */
    void Plot(double delta_t) const;

    /**
     * Create a new move with set paramters
     * @param distance distance to travel
     * @param vMax maximum velocity
     * @param aMax maximum acceleration
     * @param tJerk jerk time, time in which you can reach aMax
     */
    void CreateMove( double distance, double vMax, double aMax, double tJerk);
    void CreateMove( double v0, double distance, double vMax, double aMax, double tJerk);
    void CreateSpeedMove( double v0, double distance, double vMax, double aMax, double nextSpeed );
    bool CreateTrajectConstantEndSpeed(double Dist, double Vmax, double Amax,
    		double Tjerk, double Ve);

    /**
     * Calculates total time for current CMotionProfile
     * @return total move time
     */
    double GetMoveTime(void) const;

    /**
     * Count number of splines programmed for current CMotionProfile
     * @return total number of splines for current CMotionProfile
     */
    int GetTotalSplines() const;

    /**
     * Calculate position and speed for given time
     * @param t time (s)
     * @param position will contain the position at time t for current CMotionProfile
     * @param velocity will contain the velocity at time t for current CMotionProfile
     * @param acceleration will contain the acceleration at time t for current CMotionProfile
     */
    void TimeToDistance( double t, double &position, double &velocity, double &acceleration ) const;

    /**
     * Calculate total distance for current CMotionProfile
     * @return total distance for current CMotionProfile
     */
    double GetTotalDistance() const;

    /**
     * Reset all information in the CMotionProfile
     */
    void Reset();
    void Reverse();
    /**
     * Calculate the distance travel for S-curve.
     * @param Vp - Max speed
     * @param Ve - End speed
     * @param Ap - Max acceleration
     * @param Tj - T Jerk
     */
    double CalcS(double Vp, double Ve, double Ap, double Tj);
    double GetEndSpeed();
	double abc(double a, double b, double c);
private:
    int nPVT;
    struct _PVT {
        double P;
        double V;
        double T;
    } PVTs[MAX_PVT];
    double vStart;

    void CreatePVT(double distance, double aMax, double tA, double tC, double tJ );
    void AddPVT( double a0, double jerk, double t );
    void TimeToDistance( double v0, const struct _PVT &pvt, double t, double &position, double &velocity, double &acceleration  ) const;

    void VeCalc1(double Ve, double Ap, double Tj, double P);
    void VeCalc2(double Ve, double Tj, double P);
    double SolveVp1(double Vp, double Ve, double Ap, double Tj, double P);
    double SolveVp2(double Vp, double Ve, double Ap, double Tj, double P);
    double SolveVp3(double Vp, double Ve, double Ap, double Tj, double P);
    void VeCalc3(double Vp, double Ve, double Ap, double Tj, double P);
    void VeCalc4(double Ve, double Ap1, double Ap2, double Tj, double P);
    void VeCalc5(double Vp, double Ve, double Ap, double Tj, double P);

    /**
     * Add a stopping PVT and return the distance it took to stop.
     */
    double InsertStop(double V, double aMax);
};
#endif