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