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);
}
public TunableParam GetParam(string name, double defaultValue)
{
TunableParam param;
if (paramTable.TryGetValue(name, out param)) {
return param;
}
else {
param = new TunableParam(name, defaultValue);
paramTable.Add(name, param);
return param;
}
}
public TunableParam GetParam(string name, double defaultValue)
{
TunableParam param;
if (paramTable.TryGetValue(name, out param))
{
return(param);
}
else
{
param = new TunableParam(name, defaultValue);
paramTable.Add(name, param);
return(param);
}
}
public TunableParamTable(Stream xmlConfig)
{
if (xmlConfig == null || xmlConfig.Length == 0)
{
return;
}
// load from config
Assembly thisAssm = typeof(TunableParamTable).Assembly;
Stream xsdStream = thisAssm.GetManifestResourceStream(this.GetType(), "TunableParams.xsd");
XmlSchema mappingSchema = XmlSchema.Read(xsdStream, ValidationEvent);
XmlReaderSettings readerSettings = new XmlReaderSettings();
readerSettings.Schemas.Add(mappingSchema);
readerSettings.IgnoreComments = true;
readerSettings.IgnoreWhitespace = true;
XmlReader reader = XmlReader.Create(xmlConfig, readerSettings);
// read through the Parameters
bool skipRead = false;
while (skipRead || reader.Read())
{
skipRead = false;
if (reader.NodeType == XmlNodeType.EndElement && reader.LocalName == "tunableParams")
{
break;
}
else if (reader.NodeType == XmlNodeType.Element && reader.LocalName == "param")
{
string name = reader.GetAttribute("name");
string group = reader.GetAttribute("group");
double value = reader.ReadElementContentAsDouble();
TunableParam param = GetParam(name, group, value);
param.Value = value;
skipRead = true;
}
}
}
public void SetParameter(TunableParam param)
{
TunableParam localParam = table.GetParam(param.Name, param.Value);
localParam.Value = param.Value;
}
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;
}
