private void RawLoad(XElement element, PackedData pack, ref float scaler, ref float offset)
{
if (element == null)
{
return;
}
scaler = Utility.ConvertToFloat(XMLReader.XElementAttributeValueByName(element, "scaler"));
var item = XMLReader.XElementByName(element, "raw");
if (item == null)
{
return;
}
offset = Utility.ConvertToFloat(XMLReader.XElementAttributeValueByName(item, "offset"));
int.TryParse(XMLReader.XElementAttributeValueByName(item, "index"), out pack.index);
switch (XMLReader.XElementAttributeValueByName(item, "type"))
{
case "_byte": pack.type = DataType._byte; break;
case "_int16": pack.type = DataType._int16; break;
case "_int32": pack.type = DataType._int32; break;
case "_float": pack.type = DataType._float; break;
}
}
public void Initialize(string pathToConfig)
{
if (Initialized)
{
return;
}
_joints.Clear();
if (!System.IO.File.Exists(pathToConfig))
{
throw new Exception($"Файл {pathToConfig} не найден.");
}
XDocument xDoc;
xDoc = XDocument.Load(pathToConfig);
try
{
var element = XMLReader.XElementByName(xDoc, "costume");
if (element == null)
{
return;
}
ID = XMLReader.XElementAttributeValueByName(element, "id");
_ip = XMLReader.XElementAttributeValueByName(element, "ip");
int.TryParse(XMLReader.XElementAttributeValueByName(element, "port"), out _port);
int.TryParse(XMLReader.XElementAttributeValueByName(element, "pps"), out _pps);
float ofs = 0;
RawLoad(XMLReader.XElementByName(xDoc, "voltage"), Voltage.Raw, ref Voltage.valueScaler, ref ofs);
element = XMLReader.XElementByName(xDoc, "joints");
if (element != null)
{
foreach (var item in XMLReader.XElementsByName(element, "joint"))
{
var joint = new CostumeJoint()
{
Name = XMLReader.XElementAttributeValueByName(item, "name"),
};
RawLoad(item, joint.Raw, ref joint.valueScaler, ref joint.rawValueOffset);
_joints.Add(joint);
}
}
// Инициализируем контроллеры на линии
for (int i = 0; i < 7; i++)
{
byte idx = (byte)(i + 1);
bufTX[16 * i] = idx;
bufTX[16 * i + 128] = idx;
}
Initialized = true;
new Thread(Exchange).Start();
}
finally
{
xDoc = null;
}
}
