Esempio n. 1
0
        public void UpdateInfo(ScannedRobotEvent evnt, Coordinates robotPostion, double robotHeadings)
        {
            Bearings.AddNew(evnt.BearingRadians);
            Distance.AddNew(evnt.Distance);
            Energy.AddNew(evnt.Energy);
            Heading.AddNew(evnt.HeadingRadians);
            Velocity.AddNew(evnt.Velocity);
            Time.AddNew(evnt.Time);

            OriginAngle.AddNew(Utils.NormalRelativeAngle(robotHeadings + Bearings.GetCurrent()));

            double y = Distance.GetCurrent() * Math.Cos(OriginAngle.GetCurrent());
            double x = Distance.GetCurrent() * Math.Sin(OriginAngle.GetCurrent());

            Position.AddNew(robotPostion + new Coordinates(x, y));
        }