public static double GetPowerProfileVelocity(double Vi, double Vf, double D, double d)
{
// Vi = initial velocity of segment
// Vf = final velocity of segment
// D = distance of segment
// d = distance to the end of the segment
if (profile_power_param == null)
{
profile_power_param = Services.Params.GetParam("profile power", "speed", 1.25);
}
double fact = d / D;
if (fact > 1)
{
fact = 0;
}
if (fact < 0)
{
fact = 0;
}
fact = Math.Pow(fact, 1 / profile_power_param.Value);
return(fact * (Vi - Vf) + Vf);
}
static CurvaturePathFollowing()
{
// obtain values of tunable Parameters
TunableParamTable table = Services.Params;
q1_param = table.GetParam("steering q1", "steering", 0);
q2_param = table.GetParam("steering q2", "steering", 0);
r_param = table.GetParam("steering r", "steering", 10000);
offtrack_error_max_param = table.GetParam("offtrack error max", "steering", 2);
}