示例#1
0
 public void Set(MkString s)
 {
     for (m_nLastIndex = 0; s.Length > m_nLastIndex && MKSTRING_SIZE > m_nLastIndex; m_nLastIndex++)
     {
         m_caCharArray [m_nLastIndex] = s.Buffer [m_nLastIndex];
     }
     m_caCharArray [m_nLastIndex] = (char)0;
 }
示例#2
0
        /*
         * Calculates the distance of a point to the exit of the labirynth.
         * 	sCmd - command
         * There are 2 formats of ?Eval command:
         *
         * 1. ?Eval <dist>
         *    This command returns 360 strings representing the floating
         * 	  point values of the distances of the corresponding points around
         *    the robot to the exit.
         * 	  The <dist> argument is the distance of the points being queried
         * 	  from the robot. The 1-st value represents the distance from the
         *    point at angle 0 (behind robot) from robot to the exit, 2-nd value
         * 	  is the distance from point at angle 1 (counter-clockwise)
         *	  and so on.
         *
         * 2. ?Eval <angle> <dist>
         *    Returns only single string representing a floating point value
         *    of a distance of the point to the exit. The point is located
         * 	  at distance <dist> and angle <angle> from the robot. The angle
         * 	  is provided in radians and starts behind the robot increasing
         * 	  in the counter-clockwise direction.
         *
         */
        string CalcEval(string sCmd)
        {
            var sRet = new MkString ();
            double fEval = -1.0e6;
            double fAngle = 0.0f;
            double fDist = 0.0f;
            double fX = 0.0f;
            double fY = 0.0f;
            string [] saSplit = null;
            const string sDelim = " ";
            char [] delim = sDelim.ToCharArray ();
            saSplit = sCmd.Split (delim, 3);
            if (2 < saSplit.Length)
            { // 2 parametes eval, query single point
                fAngle = Convert.ToDouble (saSplit [1]);
                int na = (int) Math.Round (fAngle * Constants.RAD2DEG);
                na = NormalizeRadarIndex (na);
                fDist = Convert.ToDouble (saSplit [2]);
                fX = m_fXPos + (fDist * m_faCosTbl [na]);
                fY = m_fYPos + (fDist * m_faSinTbl [na]);
                fEval = -(fX * fX + fY * fY);
                sRet.Set (fEval.ToString ());
            }
            else
            { // 1 parameter eval, query whole bunch
                fDist = Convert.ToDouble (saSplit [1]);
                const int na = Constants.RADAR_RESN2;
                for (int i = 0, j = na; Constants.RADAR_RESN > i; i++, j++)
                {
                    if (Constants.RADAR_RESN <= j)
                        j = 0;
                    fX = m_fXPos + (fDist * m_faCosTbl [j]);
                    fY = m_fYPos + (fDist * m_faSinTbl [j]);
                    fEval = -(fX * fX + fY * fY);
                    sRet.Append (fEval.ToString ());
                    if (Constants.RADAR_RESN - 1 > i)
                        sRet.AddSpace ();
                }
            }

            return (sRet.Buffer);
        }
示例#3
0
        /*
         * World / environment communicates with the robots via stdio.
         * Bots are sending commands to the environment and environment takes
         * actions and sends responses when it is expected.
         * All the responses are returned as strings as well.
         *
         * Following bot control / query commands are currently supported:
         *
         * Move - moves the bot one step ahead
         *
         * Turn [angle] - rotates the robot by angle [rad] to the right (angle < 0)
         * 				  or to the left (angle >= 0). Note that this is a relative
         * 				  angle of turn to the current robot's absolute angle in the
         * 				  environment's coordinates system.
         *
         * Kill - bot commits suicide
         *
         * ?Radar - query the radar function, will return a list of 360 floating point
         *          values, where value at step #0 is the distance to the nearest
         *          obstacle exactly behind the robot, value at step #1 is the distance
         * 			to the obstacle at 1 degree, value at step #2 is the distance to
         * 			the obstacle at 2 degrees and so on, going counter-clockwise.
         * 			Value at step #180 represents distance to the obstacle in front of
         * 			the bot. Value at step #90, to the right, at step #270 to the left.
         * 			You should get the idea by now.
         *
         * ?PRadar <begin> <steps> - query the radar function but instead of full circle
         * 							 (360 deg), query from <begin> angle for a number
         * 							 of <steps>.
         *
         * ?Size - returns the size of the robot (diameter = 2 * r) in the world's
         * 		   absolute units. The size of the world is determined by the resolution
         * 		   of world's map bitmap file, from which all the radar rays are
         * 		   calculated.
         *
         * ?Killed - check if the robot was killed in previous step, returns 'true'
         * 			 or 'false' string.
         *
         * ?Eval dist - query the points located at <dist> distance from the robot
         * 				for their distances from the labirynth exit.
         * 			    It's like a ?Radar, but instead of returning distances to the
         * 				obstacles, returns the negative values of distances of these
         * 				points to the exit. Robot program code can use these values
         * 				to decide its next step while looking for the exit in its
         * 				algorithm (the greater the value returned from ?Eval, the closer
         * 				given point is to the exit).
         *
         * ?Eval angle dist - similar to above, however queries only a single point
         * 					  at angle <angle> radians and <dist> distance from the
         * 					  bot for that point's distance from exit, returns single
         * 					  value.
         *
         * Reset - causes robot coordinates to reset to their initial location and
         * 		   rotate to 0 angle position (absolute angle of the environment's
         * 		   coordinates).
         *
         * ?Step - returns the length of a step (for Move command) from setup.
         * 		   Step can be smaller than the unit of the world's map resolution.
         * 		   In other words, it is a floating point value and can be less than 0.
         *
         */
        public string ProcessCommand(string sCmd)
        {
            m_sLastCommand = sCmd;
            Trace.WriteLine ("Processing command: " + sCmd);
            Trace.WriteLine ("Current coordinates: [" + m_fXPos + ", " + m_fYPos + "]");
            Trace.WriteLine ("Current angle: " + m_fAngle + " rad.");
            var sRet = new MkString ();
            if (sCmd.StartsWith("Move", StringComparison.Ordinal)) {

                Move();

            } else if (sCmd.StartsWith("Kill", StringComparison.Ordinal)
                       || sCmd.StartsWith("Exit", StringComparison.Ordinal) ) {

                m_bKilled = true;

            } else if (sCmd.StartsWith("Turn", StringComparison.Ordinal)) {

                string[] saSplit = null;
                const string sDelim = " ";
                char[] delim = sDelim.ToCharArray();
                saSplit = sCmd.Split(delim, 2);
                double fAngle = Convert.ToDouble(saSplit[1]);
                Turn(fAngle);

            } else if (sCmd.StartsWith("?Radar", StringComparison.Ordinal)) {

                sRet.Set(m_World.GetRadarAt(m_fXPos, m_fYPos, m_fAngle));

            } else if (sCmd.StartsWith("?PRadar", StringComparison.Ordinal)) {

                string[] saSplit = null;
                const string sDelim = " ";
                char[] delim = sDelim.ToCharArray();
                saSplit = sCmd.Split(delim, 3);
                int begin = Convert.ToUInt16(saSplit[1]);
                int steps = Convert.ToUInt16(saSplit[2]);
                sRet.Set(m_World.GetPartialRadarAt(m_fXPos, m_fYPos, m_fAngle, begin, steps));

            } else if (sCmd.StartsWith("?Size", StringComparison.Ordinal)) {

                sRet.Set(m_World.m_Config.GetRobotSize().ToString());

            } else if (sCmd.StartsWith("?Killed", StringComparison.Ordinal)) {

                sRet.Set((m_bKilled) ? "true" : "false");

            } else if (sCmd.StartsWith("?Eval", StringComparison.Ordinal)) {

                sRet.Set(CalcEval(sCmd));

            } else if (sCmd.StartsWith("Reset", StringComparison.Ordinal)) {

                m_bKilled = false;
                m_fXPos = m_fInitX;
                m_fYPos = m_fInitY;
                m_nPosX = (int)m_fXPos;
                m_nPosY = (int)m_fYPos;
                m_bReset = true;
                m_fAngle = 0.0f;

            } else if (sCmd.StartsWith("?Step", StringComparison.Ordinal)) {

                sRet.Set(m_World.m_Config.GetStep().ToString());
            }
            if (0 < sRet.Length)
            {
                if (!sCmd.StartsWith("?Radar", StringComparison.Ordinal)
                    && !sCmd.StartsWith("?PRadar", StringComparison.Ordinal)
                   )
                    Trace.WriteLine("Return value: " + sRet.Buffer);
                else
                    Trace.WriteLine("Return value: " + m_World.m_sRadarDetailed.Buffer);
            }

            return (sRet.Buffer);
        }