You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
115 lines
3.8 KiB
C++
115 lines
3.8 KiB
C++
#ifndef MOTION_CMotionProfile_H
|
|
#define MOTION_CMotionProfile_H
|
|
|
|
#ifdef BUILD_IODLL
|
|
#define IODLLCLASS __declspec(dllexport)
|
|
#else
|
|
#define IODLLCLASS __declspec(dllimport)
|
|
#endif
|
|
|
|
/**
|
|
* Defines an alias representing the PVT.
|
|
* PVT's are a way of representing a motion profile by defining s series of point that the motor will need to pass.
|
|
* At given times \f$ T \f$ the motor will need to be at position \f$ P \f$ with velocity \f$ V \f$
|
|
*/
|
|
typedef struct
|
|
{
|
|
double P; /**< The position */
|
|
double V; /**< The velocity */
|
|
double T; /**< The time */
|
|
} PVT;
|
|
|
|
#define MAX_PVT 100
|
|
class IODLLCLASS CMotionProfile
|
|
{
|
|
public:
|
|
CMotionProfile(void);
|
|
CMotionProfile(const CMotionProfile &p);
|
|
|
|
void Append(const CMotionProfile &p);
|
|
virtual ~CMotionProfile(void);
|
|
|
|
void Dump() const;
|
|
|
|
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;
|
|
PVT 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 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
|