public SpeedSample SampleLine(int startPos, int endPos, int samplesCnt)
{
TimeSpan accelDuration, maxSpeedDuration, brakingDuration, totalDuration;
totalDuration = _config.LineDuration(Math.Abs(endPos - startPos), out accelDuration, out maxSpeedDuration, out brakingDuration);
TimeSpan division = new TimeSpan(totalDuration.Ticks / samplesCnt);
List <double> speeds = new List <double>();
List <double> positions = new List <double>();
List <TimeSpan> times = new List <TimeSpan>();
double currentSpeed = 0, currentPosition = startPos;
int direction = ((endPos - startPos) > 0 ? 1 : -1);
double accelPerDiv = (_config.LineAcceleration * division.TotalSeconds) * direction;
TimeSpan currentTime = new TimeSpan();
while (speeds.Count < samplesCnt)
{
speeds.Add(currentSpeed);
positions.Add(currentPosition);
times.Add(currentTime);
currentTime += division;
if (currentTime < accelDuration)
{
currentSpeed += accelPerDiv;
}
else if (currentTime > accelDuration + maxSpeedDuration)
{
currentSpeed -= accelPerDiv;
}
else
{
currentSpeed = _config.LineSpeed * direction;
}
if (direction < 0)
{
currentSpeed = Math.Min(0, Math.Max(currentSpeed, _config.LineSpeed * direction));
}
else
{
currentSpeed = Math.Max(0, Math.Min(currentSpeed, _config.LineSpeed * direction));
}
currentPosition += currentSpeed;
}
return(new SpeedSample(times, positions, speeds));
}
public TimeSpan CalculDuration(SpeedConfig config, Robot robot)
{
TimeSpan totalDuration = new TimeSpan();
foreach (int dist in ForwardMoves.Union(BackwardMoves))
{
totalDuration += config.LineDuration(dist);
}
foreach (AngleDelta ang in LeftRotations.Union(RightsRotations))
{
totalDuration += config.PivotDuration(ang, robot.WheelSpacing);
}
return(totalDuration);
}
