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