private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e)
{
Gdk.Threads.Enter ();
try
{
robot.UpdateOdometry (e);
}
finally
{
Gdk.Threads.Leave ();
}
}
/// <summary>
/// Updates the odometry.
/// </summary>
/// <param name="e">E.</param>
public void UpdateOdometry(OdometryUpdateEventArgs e)
{
bool raiseEvent = false;
// First update.
if (originalRotation == double.MaxValue)
{
PathPointList.Add (new double[2] { x, y });
originalRotation = e.Theta;
raiseEvent = true;
}
else
{
// Check if we have changed rotation.
heading = e.Theta - originalRotation;
double change = e.Theta - originalRotation;
// Rotated by more than a degree?
if (change >= 0.02 || change <= -0.02)
raiseEvent = true;
if (heading < 0)
{
heading += Math.PI * 2;
}
else if (heading > 2 * Math.PI)
{
heading -= Math.PI * 2;
}
heading = Math.Round (heading, 2);
/*
* Calculate the change as follows:
* xm = (displacement / sensor cpi) * conversion of inches to meters
* ym = (displacement / sensor cpi) * conversion of inches to meters
*/
double xm = (e.X / mouseCpi) * inchToMeter;
double ym = (e.Y / mouseCpi) * inchToMeter;
bool hasMoved = CalculateDisplacement (xm, ym);
if (hasMoved)
PathPointList.Add (new double[2] { x, y });
if (!raiseEvent)
raiseEvent = true;
}
if (raiseEvent)
RaiseRobotUpdate ();
}
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e)
{
Console.WriteLine(e.ToString());
}
private void Read()
{
while (serialPort.IsOpen)
{
try
{
string message = serialPort.ReadLine ();
switch (message [0])
{
case 'o':
string[] parameters = message.Split (',');
OdometryUpdateEventArgs args = new OdometryUpdateEventArgs (
Int32.Parse (parameters [1]),
Int32.Parse (parameters [2]),
Double.Parse (parameters [3]));
this.OnOdometryUpdate (args);
break;
case 's':
string[] stringReadings = message.Split (',');
List<double> readings = new List<double> ();
// Somewhat inefficient. Start from 1 to skip first character
// which is the message header.
for (int i = 1; i < stringReadings.Length; i++)
{
readings.Add (Double.Parse (stringReadings [i]));
}
readings.Reverse ();
ScanEventArgs args2 = new ScanEventArgs (readings);
OnScanPerformed (args2);
break;
default:
break;
}
}
catch (TimeoutException ex)
{
Console.WriteLine (ex.ToString ());
}
catch (FormatException ex)
{
Console.WriteLine (ex.ToString ());
}
catch (IndexOutOfRangeException ex)
{
Console.WriteLine (ex.ToString ());
}
catch (ThreadAbortException)
{
// Ignore it.
}
catch (Exception ex)
{
Console.WriteLine (ex.ToString ());
}
}
}
protected virtual void OnOdometryUpdate(OdometryUpdateEventArgs e)
{
if (OdometryUpdated != null)
{
OdometryUpdated (this, e);
}
}
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e)
{
robot.UpdateOdometry (e);
}
