C:\Users\KHOAND_VN\Desktop\New Text Document.

Friday, January 2, 2015 10:40 AM

public void Trajectory_F(double Time, int qi, int qf, double Tint,double Tend)
{
double T = Tend - Tint;
double Ta = 0.11;
double Ts = 1;
int h = qf - qi;
double qDotV = h / (Ts - Ta);
if (Time <= Ta)
{
q2dot = qDotV / Ta;
qDot = (qDotV / Ta) * (Time - Tint) / 16;
q = qi + (qDotV / (2 * Ta)) * (Time - Tint) * (Time - Tint);
}
else
{
if ((Time > Tint + Ta) && (Time <= Ts - Ta))
{
q2dot = 0;
qDot = qDotV / 16;
q = qi + qDotV * (Time - Tint - Ta / 2);
}
else
{
if ((Time > Ts - Ta) && (Time <= Ts))
{
q2dot = -qDotV / Ta;
qDot = (qDotV / Ta) * (Ts - Time) / 16;
q = qf - (qDotV / (2 * Ta)) * (Ts - Time) * (Ts - Time);
}
else
{
q2dot = 0;
qDot = 0;
q = qf;
}
}
}
}

-1-