/// <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);
}
