/// <summary>
/// Diese Thread-Methode ist dafür zuständig, die Fahrbefehle abzuarbeiten.
/// Dazu werden folgende Schritte ausgeführt:
/// - evtl. neuen Track initialisieren
/// - Aktuelle Prozessdaten (Zeit) erfassen
/// - Neue Parameter berechnen
/// - Neue Parameter setzen
/// - Informationen aktualisieren
/// </summary>
private void RunTracks()
{
float velocity = 0;
float deltaTime;
_stop = false;
_halt = false;
_run = true;
Track oldTrack = null;
int ticks = Environment.TickCount;
while (_run)
{
Thread.Sleep(1); // Möglichst schneller Process Control Loop
if (_stop)
{
lock (_tracksToRun)
{
_tracksToRun.Clear();
}
_actualTrack = null;
_stop = false;
velocity = 0;
}
// Check if new Track needs to be assigned
lock (_tracksToRun)
{
if (_actualTrack == null && _tracksToRun.Count > 0)
{
_actualTrack = _tracksToRun[0];
}
}
// Falls ein neuer Track gesetzt wurde, diesen initialisieren und starten
if (_actualTrack != null && _actualTrack != oldTrack)
{
lock (_infoLock)
{
lock (_drivesLock)
{
// Aktuelle, gefahrene Distanz des linken un rechten Rades speichern
_oldInfo.DistanceL = -MotorCtrlLeft.Distance;
_oldInfo.DistanceR = MotorCtrlRight.Distance;
}
_info.Runtime = 0;
}
oldTrack = _actualTrack;
}
// Aktuelle Prozessdaten erfassen
// ------------------------------
int deltaTicks = Environment.TickCount - ticks; // Zeit [ms]
ticks += deltaTicks;
deltaTime = deltaTicks / 1000.0f;
if (_actualTrack != null)
{
if (_actualTrack.Done)
{
OnTrackDone(_actualTrack);
lock (_tracksToRun)
{
_tracksToRun.Remove(_actualTrack);
}
_actualTrack = null;
if (_tracksToRun.Count == 0)
{
OnTracksFinished();
}
}
else if (_halt && (velocity == 0))
{
// Wait until restart
}
else if (_actualTrack.ResidualLength > 0)
{
// Neue Prozessparameter berechnen
// -------------------------------
if (_halt)
{
// Roboter mit der eingestellten Beschleunigung bremsen und anhalten
velocity = Math.Max(0, velocity - deltaTime * _actualTrack.Acceleration);
}
else
{
// Beschleunigung (od. Verzögerung bei Reduktion der nominalSpeed)
if (_actualTrack.NominalSpeed > velocity)
{
velocity = Math.Min(_actualTrack.NominalSpeed, velocity + deltaTime * _actualTrack.Acceleration);
if (_speedsUp == false)
{
_speedsUp = true;
OnSpeedsUp();
}
}
else if (_actualTrack.NominalSpeed < velocity)
{
velocity = Math.Max(_actualTrack.NominalSpeed, velocity - deltaTime * _actualTrack.Acceleration);
if (_speedsUp)
{
_speedsUp = false;
OnSlowsDown();
}
}
// Verzögerung auf Zielposition
// Geschwindigkeit auf max. zulässige Bremsgeschwindigkeit limitieren
float ve;
float s;
lock (_tracksToRun)
{
if (_actualTrack.GetType() == typeof(TrackTurn))
{
s = _actualTrack.ResidualLength;
}
else
{
s = _tracksToRun.TakeWhile(track => track.GetType() != typeof(TrackTurn))
.Sum(track => track.ResidualLength);
}
}
if (s >= 0)
{
ve = (float)Math.Sqrt(2.0 * _actualTrack.Acceleration * s);
}
else
{
ve = 0;
}
if (float.IsNaN(ve))
{
ve = 0;
}
if (ve < velocity)
{
velocity = ve;
if (_speedsUp)
{
_speedsUp = false;
OnSlowsDown();
}
}
//System.Console.WriteLine(velocity);
}
// Neue Prozessparameter aktivieren
// --------------------------------
float leftSpeed, rightSpeed;
_actualTrack.IncrementalStep(deltaTime, velocity, out leftSpeed, out rightSpeed);
MotorCtrlLeft.Speed = leftSpeed;
MotorCtrlRight.Speed = rightSpeed;
// Motorenparameter sind gesetzt
// => möglichst gleichzeitig aktivieren (durch .Go())
MotorCtrlLeft.Go();
MotorCtrlRight.Go();
}
else
{
OnTrackDone(_actualTrack);
lock (_tracksToRun)
{
_tracksToRun.Remove(_actualTrack);
}
_actualTrack = null;
if (_tracksToRun.Count == 0)
{
OnTracksFinished();
}
}
}
else
{
// Idle-Zustand setzen
// -------------------
lock (_drivesLock)
{
MotorCtrlLeft.Speed = 0;
MotorCtrlRight.Speed = 0;
MotorCtrlRight.Go();
MotorCtrlLeft.Go();
}
}
// Aktuellen Status speichern
UpdateInfo(deltaTime);
}
}
