private PololuMaestroPoint DefaultMaxPoint()
{
PololuMaestroPoint point = new PololuMaestroPoint()
{
FalconValue = DefaultFalconMaxValue, ServoValue = 8000
};
point.PropertyChanged += OnPointChanged;
return(point);
}
private void executeAddAdditionalPoint(object o)
{
PololuMaestroPoint previousPoint = AdditionalPointList.LastOrDefault() ?? MinPoint;
PololuMaestroPoint nextPoint = MaxPoint;
PololuMaestroPoint additionalPoint = new PololuMaestroPoint()
{
FalconValue = (previousPoint.FalconValue + nextPoint.FalconValue) / 2,
ServoValue = (ushort)((previousPoint.ServoValue + nextPoint.ServoValue) / 2)
};
AdditionalPointList.Add(additionalPoint);
}
private ushort FalconValueToServoValue(float?falconValue)
{
if (!falconValue.HasValue)
{
return(0);
}
// Clamp value to [Min, Max]
if (falconValue.Value <= MinPoint.FalconValue)
{
return(MinPoint.ServoValue);
}
if (falconValue.Value >= MaxPoint.FalconValue)
{
return(MaxPoint.ServoValue);
}
// Find the segment that contains falconValue
PololuMaestroPoint previousPoint = MinPoint;
PololuMaestroPoint nextPoint = null;
foreach (PololuMaestroPoint additionalPoint in AdditionalPointList)
{
if (falconValue.Value >= additionalPoint.FalconValue)
{
previousPoint = additionalPoint;
}
else
{
nextPoint = additionalPoint;
break;
}
}
if (nextPoint == null)
{
nextPoint = MaxPoint;
}
// Interpolate in segment
float normalizedValue = 0.5F;
if (previousPoint.FalconValue != nextPoint.FalconValue)
{
normalizedValue = (falconValue.Value - previousPoint.FalconValue) / (nextPoint.FalconValue - previousPoint.FalconValue);
}
return((ushort)(normalizedValue * (nextPoint.ServoValue - previousPoint.ServoValue) + previousPoint.ServoValue));
}
private bool canExecuteRemoveAdditionalPoint(object o)
{
PololuMaestroPoint additionalPoint = (PololuMaestroPoint)o;
return(AdditionalPointList.Contains(additionalPoint));
}
private void executeRemoveAdditionalPoint(object o)
{
PololuMaestroPoint additionalPoint = (PololuMaestroPoint)o;
AdditionalPointList.Remove(additionalPoint);
}