private void SendSetMotorConfigOptions(byte dst, MotorOptions opt)
{
if (connection_ != null)
{
List <byte> l = new List <byte>();
l.Add((byte)'M');
l.Add(dst);
WriteMotorOptions(opt, l);
SendCommand(connection_, l.ToArray(), 1, ReceiveSetMotorConfigOptions);
}
}
private void ReceiveMotorConfigOptionsSizes(byte[] data)
{
motorOptionSize_ = BitConverter.ToInt16(data, 0);
adapterOptionSize_ = BitConverter.ToInt16(data, 2);
{
MotorOptions opt = settings_.AltMotorOptions;
if (opt.valid_)
{
SendSetMotorConfigOptions(M_ALT, opt);
}
else
{
SendGetMotorConfigOptions(M_ALT);
}
opt = settings_.AzmMotorOptions;
if (opt.valid_)
{
SendSetMotorConfigOptions(M_AZM, opt);
}
else
{
SendGetMotorConfigOptions(M_AZM);
}
}
{
AdapterOptions opt = settings_.AltAdapterOptions;
if (opt.valid_)
{
SendSetAdapterConfigOptions(A_ALT, opt);
}
else
{
SendGetAdapterConfigOptions(A_ALT);
}
opt = settings_.AzmAdapterOptions;
if (opt.valid_)
{
SendSetAdapterConfigOptions(A_AZM, opt);
}
else
{
SendGetAdapterConfigOptions(A_AZM);
}
}
}
private void WriteMotorOptions(MotorOptions opt, List <byte> data)
{
data.AddRange(BitConverter.GetBytes(opt.encRes_));
data.AddRange(BitConverter.GetBytes(opt.maxSpeedRPM_));
data.AddRange(BitConverter.GetBytes(opt.Kp_));
data.AddRange(BitConverter.GetBytes(opt.KiF_));
data.AddRange(BitConverter.GetBytes(opt.Kd_));
data.Add((byte)(opt.approximationType_ == ApproximationType.LINEAR ? 0 : 1));
data.Add(opt.loProfile_.value_);
data.Add(opt.loProfile_.magnitude_);
data.AddRange(BitConverter.GetBytes(opt.loProfile_.period_));
data.Add(opt.hiProfile_.value_);
data.Add(opt.hiProfile_.magnitude_);
data.AddRange(BitConverter.GetBytes(opt.hiProfile_.period_));
}
private void ReadMotorOptions(byte[] data, int offset, out MotorOptions opt)
{
opt = new MotorOptions();
opt.valid_ = true;
opt.encRes_ = BitConverter.ToInt32(data, offset);
opt.maxSpeedRPM_ = BitConverter.ToSingle(data, offset + 4);
opt.Kp_ = BitConverter.ToSingle(data, offset + 8);
opt.KiF_ = BitConverter.ToSingle(data, offset + 12);
opt.Kd_ = BitConverter.ToSingle(data, offset + 16);
opt.approximationType_ = data[offset + 20] == 0 ? ApproximationType.LINEAR : ApproximationType.EXPONENTIAL;
opt.loProfile_.value_ = data[offset + 21];
opt.loProfile_.magnitude_ = data[offset + 22];
opt.loProfile_.period_ = BitConverter.ToInt16(data, offset + 23);
opt.hiProfile_.value_ = data[offset + 25];
opt.hiProfile_.magnitude_ = data[offset + 26];
opt.hiProfile_.period_ = BitConverter.ToInt16(data, offset + 27);
}