private void Asservissement()
{
_linkAsserv.RegisterName();
// Calcul du temps écoulé depuis la dernière mise à jour de la position
double interval = 0;
if (_asserChrono != null)
{
long currentTick = _asserChrono.ElapsedMilliseconds;
interval = currentTick - _lastTimerTick;
_lastTimerTick = currentTick;
}
else
{
_asserChrono = Stopwatch.StartNew();
}
if (interval > 0)
{
_lockMove.WaitOne();
if (_currentPolarPoint >= 0)
{
double distanceBeforeNext = Position.Coordinates.Distance(_polarTrajectory[_currentPolarPoint]);
double distanceBeforeEnd = distanceBeforeNext;
distanceBeforeEnd += DistanceBetween(_polarTrajectory, _currentPolarPoint, _polarTrajectory.Count - 1);
if (distanceBeforeEnd > GetCurrentAngleBreakDistance())
{
_currentSpeed = Math.Min(SpeedConfig.LineSpeed, _currentSpeed + SpeedConfig.LineAcceleration / (1000.0 / interval));
}
else
{
_currentSpeed = _currentSpeed - SpeedConfig.LineDeceleration / (1000.0 / interval);
}
double distanceToRun = _currentSpeed / (1000.0 / interval);
double distanceTested = distanceBeforeNext;
bool changePoint = false;
while (distanceTested < distanceToRun && _currentPolarPoint < _polarTrajectory.Count - 1)
{
_currentPolarPoint++;
distanceTested += _polarTrajectory[_currentPolarPoint - 1].Distance(_polarTrajectory[_currentPolarPoint]);
changePoint = true;
}
Segment segment;
Circle circle;
if (changePoint)
{
segment = new Segment(_polarTrajectory[_currentPolarPoint - 1], _polarTrajectory[_currentPolarPoint]);
circle = new Circle(_polarTrajectory[_currentPolarPoint - 1], distanceToRun - (distanceTested - _polarTrajectory[_currentPolarPoint - 1].Distance(_polarTrajectory[_currentPolarPoint])));
}
else
{
segment = new Segment(Position.Coordinates, _polarTrajectory[_currentPolarPoint]);
circle = new Circle(Position.Coordinates, distanceToRun);
}
RealPoint newPos = segment.GetCrossingPoints(circle)[0];
AngleDelta a = -Maths.GetDirection(newPos, _polarTrajectory[_currentPolarPoint]).angle;
_currentPosition = new Position(new AnglePosition(a), newPos);
OnPositionChanged(Position);
if (_currentPolarPoint == _polarTrajectory.Count - 1)
{
_currentPolarPoint = -1;
_destination.Copy(Position);
}
}
else
{
bool needLine = _destination.Coordinates.Distance(Position.Coordinates) > 0;
bool needAngle = Math.Abs(_destination.Angle - Position.Angle) > 0.01;
if (needAngle)
{
AngleDelta diff = Math.Abs(_destination.Angle - Position.Angle);
double speedWithAcceleration = Math.Min(SpeedConfig.PivotSpeed, _currentSpeed + SpeedConfig.PivotAcceleration / (1000.0 / interval));
double remainingDistanceWithAcceleration = CircleArcLenght(WheelSpacing, diff) - (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval);
if (remainingDistanceWithAcceleration > DistanceFreinage(speedWithAcceleration))
{
double distParcourue = (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval);
AngleDelta angleParcouru = (360 * distParcourue) / (Math.PI * WheelSpacing);
_currentSpeed = speedWithAcceleration;
Position.Angle += (_sensPivot.Factor() * angleParcouru);
}
else if (_currentSpeed > 0)
{
double speedWithDeceleration = Math.Max(0, _currentSpeed - SpeedConfig.PivotDeceleration / (1000.0 / interval));
double distParcourue = (_currentSpeed + speedWithDeceleration) / 2 / (1000.0 / interval);
AngleDelta angleParcouru = (360 * distParcourue) / (Math.PI * WheelSpacing);
_currentSpeed = speedWithDeceleration;
Position.Angle += (_sensPivot.Factor() * angleParcouru);
}
else
{
Position.Copy(_destination);
}
OnPositionChanged(Position);
}
else if (needLine)
{
double speedWithAcceleration = Math.Min(SpeedConfig.LineSpeed, _currentSpeed + SpeedConfig.LineAcceleration / (1000.0 / interval));
double remainingDistanceWithAcceleration = Position.Coordinates.Distance(_destination.Coordinates) - (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval);
// Phase accélération ou déccélération
if (remainingDistanceWithAcceleration > DistanceFreinage(speedWithAcceleration))
{
double distance = (_currentSpeed + speedWithAcceleration) / 2 / (1000.0 / interval);
_currentSpeed = speedWithAcceleration;
Position.Move(distance * _sensMove.Factor());
}
else if (_currentSpeed > 0)
{
double speedWithDeceleration = Math.Max(0, _currentSpeed - SpeedConfig.LineDeceleration / (1000.0 / interval));
double distance = Math.Min(_destination.Coordinates.Distance(Position.Coordinates), (_currentSpeed + speedWithDeceleration) / 2 / (1000.0 / interval));
_currentSpeed = speedWithDeceleration;
Position.Move(distance * _sensMove.Factor());
}
else
{
// Si on est déjà à l'arrêt on force l'équivalence de la position avec la destination.
Position.Copy(_destination);
IsInLineMove = false;
}
OnPositionChanged(Position);
}
}
_lockMove.Release();
}
}
