/// <summary>
/// Diese Methode aktualisiert die Positionsinformationen indem die
/// alten und neuen Daten sowie die gefahrende Distanz (Motorenticks)
/// seit dem letzten Aufruf der Methode verrechnet werden.
/// (siehe auch Übung 7)
/// </summary>
/// <param name="timeInterval">Zeit seit dem letzten Aufruf der Methode</param>
private void UpdateInfo(double timeInterval)
{
// Motor-Status aktualisieren
_info.DriveStatus = DriveCtrl.DriveState;
lock (_infoLock)
{
lock (_drivesLock)
{
_info.MotorStatusL = MotorCtrlLeft.Status;
_info.MotorStatusR = MotorCtrlRight.Status;
_info.SpeedL = -MotorCtrlLeft.Speed;
_info.SpeedR = MotorCtrlRight.Speed;
_info.DistanceL = -MotorCtrlLeft.Distance;
_info.DistanceR = MotorCtrlRight.Distance;
}
if (_actualTrack != null) _info.Runtime = _actualTrack.ElapsedTime;
}
// Position und Richtung im Weltkoordinatensystem bestimmen
// --------------------------------------------------------
float dL = _info.DistanceL - _oldInfo.DistanceL;
float dR = _info.DistanceR - _oldInfo.DistanceR;
float d;
float x1, y1, phi1;
float x2, y2, phi2;
d = (dL + dR)/2.0f;
x1 = _info.Position.X;
y1 = _info.Position.Y;
phi1 = _info.Position.Angle/180.0f*(float) Math.PI;
if (dL == dR) // Spezialfall geradeaus fahren
{
x2 = x1 + d*(float) Math.Cos(phi1);
y2 = y1 + d*(float) Math.Sin(phi1);
phi2 = phi1;
}
else if (dL == -dR) // Spezialfall an Ort drehen
{
x2 = x1;
y2 = y1;
phi2 = phi1 + dR/(Constants.AxleLength/2.0f);
}
else // allgemeiner Fall
{
float radius = Constants.AxleLength*d/(dR - dL);
float x0 = x1 + radius*(float) Math.Cos(phi1 + (float) Math.PI/2.0);
float y0 = y1 + radius*(float) Math.Sin(phi1 + (float) Math.PI/2.0);
float dphi = d/radius;
x2 = x0 + (x1 - x0)*(float) Math.Cos(dphi) - (y1 - y0)*(float) Math.Sin(dphi);
y2 = y0 + (x1 - x0)*(float) Math.Sin(dphi) + (y1 - y0)*(float) Math.Cos(dphi);
phi2 = phi1 + dphi;
}
phi2 = phi2%(2*(float) Math.PI);
// Neue Position speichern
lock (_infoLock)
{
_info.Position.X = x2;
_info.Position.Y = y2;
_info.Position.Angle = phi2/(float) Math.PI*180;
if (_oldInfo.Position.X != _info.Position.X
|| _oldInfo.Position.Y != _info.Position.Y) {
if (OnPositionUpdated != null)
{
OnPositionUpdated(_info.Position);
}
}
_oldInfo = _info;
}
}
/// <summary>
/// Diese Methode aktualisiert die Positionsinformationen indem die
/// alten und neuen Daten sowie die gefahrende Distanz (Motorenticks)
/// seit dem letzten Aufruf der Methode verrechnet werden.
/// (siehe auch Übung 7)
/// </summary>
/// <param name="timeInterval">Zeit seit dem letzten Aufruf der Methode</param>
private void UpdateInfo(double timeInterval)
{
// Motor-Status aktualisieren
_info.DriveStatus = DriveCtrl.DriveState;
lock (_infoLock)
{
lock (_drivesLock)
{
_info.MotorStatusL = MotorCtrlLeft.Status;
_info.MotorStatusR = MotorCtrlRight.Status;
_info.SpeedL = -MotorCtrlLeft.Speed;
_info.SpeedR = MotorCtrlRight.Speed;
_info.DistanceL = -MotorCtrlLeft.Distance;
_info.DistanceR = MotorCtrlRight.Distance;
}
if (_actualTrack != null)
{
_info.Runtime = _actualTrack.ElapsedTime;
}
}
// Position und Richtung im Weltkoordinatensystem bestimmen
// --------------------------------------------------------
float dL = _info.DistanceL - _oldInfo.DistanceL;
float dR = _info.DistanceR - _oldInfo.DistanceR;
float d;
float x1, y1, phi1;
float x2, y2, phi2;
d = (dL + dR) / 2.0f;
x1 = _info.Position.X;
y1 = _info.Position.Y;
phi1 = _info.Position.Angle / 180.0f * (float)Math.PI;
if (dL == dR) // Spezialfall geradeaus fahren
{
x2 = x1 + d * (float)Math.Cos(phi1);
y2 = y1 + d * (float)Math.Sin(phi1);
phi2 = phi1;
}
else if (dL == -dR) // Spezialfall an Ort drehen
{
x2 = x1;
y2 = y1;
phi2 = phi1 + dR / (Constants.AxleLength / 2.0f);
}
else // allgemeiner Fall
{
float radius = Constants.AxleLength * d / (dR - dL);
float x0 = x1 + radius * (float)Math.Cos(phi1 + (float)Math.PI / 2.0);
float y0 = y1 + radius * (float)Math.Sin(phi1 + (float)Math.PI / 2.0);
float dphi = d / radius;
x2 = x0 + (x1 - x0) * (float)Math.Cos(dphi) - (y1 - y0) * (float)Math.Sin(dphi);
y2 = y0 + (x1 - x0) * (float)Math.Sin(dphi) + (y1 - y0) * (float)Math.Cos(dphi);
phi2 = phi1 + dphi;
}
phi2 = phi2 % (2 * (float)Math.PI);
// Neue Position speichern
lock (_infoLock)
{
_info.Position.X = x2;
_info.Position.Y = y2;
_info.Position.Angle = phi2 / (float)Math.PI * 180;
if (_oldInfo.Position.X != _info.Position.X ||
_oldInfo.Position.Y != _info.Position.Y)
{
if (OnPositionUpdated != null)
{
OnPositionUpdated(_info.Position);
}
}
_oldInfo = _info;
}
}
