public Vector GetPosition()
{
double heading = Compass.Angle;
heading /= MathEx.TwoPi;
Orientation o = Orientation.Invalid;
if (HeadingRanges[0].Contains(heading) || HeadingRanges[4].Contains(heading))
o = Orientation.North;
else if (HeadingRanges[1].Contains(heading))
o = Orientation.East;
else if (HeadingRanges[2].Contains(heading))
o = Orientation.South;
else if (HeadingRanges[3].Contains(heading))
o = Orientation.West;
if (o != Orientation.Invalid)
{
Distances d;
if (o == Orientation.North)
d = new Distances(Front.DistanceCM, Right.DistanceCM, Back.DistanceCM, Left.DistanceCM);
else if (o == Orientation.East)
d = new Distances(Left.DistanceCM, Front.DistanceCM, Right.DistanceCM, Back.DistanceCM);
else if (o == Orientation.South)
d = new Distances(Back.DistanceCM, Left.DistanceCM, Front.DistanceCM, Right.DistanceCM);
else
d = new Distances(Right.DistanceCM, Back.DistanceCM, Left.DistanceCM, Front.DistanceCM);
int robotSize = (int)(SensorSpacing / 2 * MathEx.Cos(heading * MathEx.TwoPi));
d.IncrementBy(robotSize);
Vector pos = d.ApproximatePosition;
if(lastDistances != null) {
Distances changes = d.ChangeFrom(lastDistances);
if (!Tolerance.Contains(d.ApproximateWidth - PitchSize.X))
{
if(changes.West > changes.East)
pos = new Vector(PitchSize.X - d.East, pos.Y);
else
pos = new Vector(d.West, pos.Y);
}
if (!Tolerance.Contains(d.ApproximateHeight - PitchSize.Y)) {
if(changes.South > changes.North)
pos = new Vector(pos.X, PitchSize.Y - d.North);
else
pos = new Vector(pos.X, d.South);
}
}
lastDistances = d;
return pos;
}
return null;
}
public void Poll()
{
int[] distances = new int[4];
_dataProcedure.InvokeEx(_dataBuffer);
for (int i = 0; i < IR.Length; i++)
((NativeIRSensor) IR[i]).Intensity = _dataBuffer[i];
for (int i = 0; i < US.Length; i++)
((NativeUSSensor)US[i]).DistanceCM = _dataBuffer[i + IR.Length];
USDistances = new Distances
{
Top = US[0].DistanceCM,
Right = US[1].DistanceCM,
Bottom = US[2].DistanceCM,
Left = US[3].DistanceCM,
};
}
public Distances ChangeFrom(Distances old)
{
return new Distances(
System.Math.Abs(North - old.North),
System.Math.Abs(East - old.East),
System.Math.Abs(South - old.South),
System.Math.Abs(West - old.West)
);
}