/// <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) { this.track = null; stop = false; velocity = 0; } // Falls ein neuer Track gesetzt wurde, diesen initialisieren und starten if (track != null && track != 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 = track; halt = false; } // Aktuelle Prozessdaten erfassen // ------------------------------ int deltaTicks = Environment.TickCount - ticks; // Zeit [ms] ticks += deltaTicks; deltaTime = deltaTicks / 1000.0f; if (track != null) { if ((track.Done) || ((halt && (velocity == 0)))) { track = null; halt = false; } else if (track.ResidualLength > 0) { // Neue Prozessparameter berechnen // ------------------------------- if (halt) { // Roboter mit der eingestellten Beschleunigung bremsen und anhalten velocity = Math.Max(0, velocity - deltaTime * track.Acceleration); } else { // Beschleunigung (od. Verzögerung bei Reduktion der nominalSpeed) if (track.NominalSpeed > velocity) { velocity = Math.Min(track.NominalSpeed, velocity + deltaTime * track.Acceleration); } else if (track.NominalSpeed < velocity) { velocity = Math.Max(track.NominalSpeed, velocity - deltaTime * track.Acceleration); } // Verzögerung auf Zielposition // Geschwindigkeit auf max. zulässige Bremsgeschwindigkeit limitieren float ve; float s = track.ResidualLength; if (s >= 0) { ve = (float)Math.Sqrt(2.0 * track.Acceleration * s); } else { ve = 0; } if (float.IsNaN(ve)) ve = 0; velocity = Math.Min(ve, velocity); //System.Console.WriteLine(velocity); } // Neue Prozessparameter aktivieren // -------------------------------- float leftSpeed, rightSpeed; track.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 { track = null; } } else { // Idle-Zustand setzen // ------------------- lock (drivesLock) { motorCtrlLeft.Speed = 0; motorCtrlRight.Speed = 0; motorCtrlRight.Go(); motorCtrlLeft.Go(); } } // Aktuellen Status speichern updateInfo(deltaTime); } }
/// <summary> /// Fährt eine Strecke gerade aus. /// </summary> /// /// <remarks> /// Der Fahrbefehl wird nur ausgeführt, falls gerade kein anderer Fahrbefehl abgearbeitet wird! /// </remarks> /// /// <param name="length">Strecke, die zurückgelegt werden soll [m]</param> /// <param name="speed">die gewünschte Geschwindigkeit [m/s]</param> /// <param name="acceleration">die gewünschte Beschleunigung [m/s]</param> public void RunLine(float length, float speed, float acceleration) { if (disposed) throw new ObjectDisposedException("Drive"); if (track == null) track = new TrackLine(length, speed, acceleration); }
/// <summary> /// Dreht an Ort und Stelle /// </summary> /// /// <remarks> /// Der Fahrbefehl wird nur ausgeführt, falls gerade kein anderer Fahrbefehl abgearbeitet wird! /// </remarks> /// /// <param name="angle">der gewünschte Winkel [°]</param> /// <param name="speed">die gewünschte Geschwindigkeit [m/s]</param> /// <param name="acceleration">die gewünschte Beschleunigung [m/s]</param> public void RunTurn(float angle, float speed, float acceleration) { if (disposed) throw new ObjectDisposedException("Drive"); if (track == null) track = new TrackTurn(angle, speed, acceleration); }
/// <summary> /// Fährt eine Rechtskurve /// </summary> /// /// <remarks> /// Der Fahrbefehl wird nur ausgeführt, falls gerade kein anderer Fahrbefehl abgearbeitet wird! /// </remarks> /// /// <param name="radius">der gewünschte Radius</param> /// <param name="angle">der gewünschte Winkel [°]</param> /// <param name="speed">die gewünschte Geschwindigkeit [m/s]</param> /// <param name="acceleration">die gewünschte Beschleunigung [m/s]</param> public void RunArcRight(float radius, float angle, float speed, float acceleration) { if (disposed) throw new ObjectDisposedException("Drive"); if (track == null) track = new TrackArcRight(radius, angle, speed, acceleration); }