public void OnTrackDone(Track track) { EventHandler handler = TrackDone; if (handler != null) handler(track, EventArgs.Empty); }
/// <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); } }
public void AddTrack(Track track) { lock (_tracksToRun) { _tracksToRun.Add(track); } }