protected virtual void OnOdometryUpdate(OdometryUpdateEventArgs e)
{
if (OdometryUpdated != null)
{
OdometryUpdated(this, e);
}
}
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e)
{
Gdk.Threads.Enter();
try
{
robot.UpdateOdometry(e);
}
finally
{
Gdk.Threads.Leave();
}
}
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());
}
}
}
private void SerialProxy_OdometryUpdate(object sender, OdometryUpdateEventArgs e)
{
robot.UpdateOdometry(e);
}
/// <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();
}
}
