multiforagen150hard.java

TeamBots 是一个可移植的多代理机器人仿真器

				speed_command*base_speed);
		
			/*--- send movement command to the robot ---*/
			// only if new commands 
			if ((old_hard_command != hard_command) ||
				(old_desired_heading != desired_heading) ||
				(old_desired_turret_heading != 
					desired_turret_heading))
	
				// this should return sensor data, but fails to 
				nomad150_hardware.mv(
					Ndirect.MV_VM,
					hard_command,
					Ndirect.MV_PR,
					Units.RadToDeg10(turn),
					Ndirect.MV_PR,
					Units.RadToDeg10(turret_turn));

			/*--- set the old variables ---*/
			old_hard_command = hard_command;
			old_desired_heading = desired_heading;
			old_desired_turret_heading = desired_turret_heading;

			/*--- sleep an appropriate time ---*/
			this_cycle = System.currentTimeMillis() - current_time;
			run_time_sum += this_cycle;
			//sleep_time = 50;
			//sleep_time = MIN_CYCLE_TIME - this_cycle;
			//if (sleep_time<50) sleep_time = 50;
			//try
				//{
				//Thread.sleep(sleep_time);
				//}
			//catch (InterruptedException e){};
			cycles++;
			}
		}


	private	long	last_VisualObjectst = 0;
	/**
	 * Get an array of Vec2s that represent the
	 * locations of visually sensed objects egocentrically
 	 * from center of the robot to the objects currently sensed by the 
	 * vision system.	
	 * @param timestamp only get new information 
	 *        if timestamp > than last call or timestamp == -1 .
	 * @param channel (1-6) which type/color of object to retrieve.
	 * @return the sensed objects.
	 */
	public Vec2[] getVisualObjects(long timestamp, int channel)
		{
		Vec2[] objs = new Vec2[0];
		if (newt!=null)
			{
			objs = nt.getVisualObjects(channel);
			for (int i = 0; i < objs.length; i++)
				{
				objs[i].sett(last_TurretHeading + objs[i].t);
				}
			if (DEBUG/*true*/) 
				System.out.println(objs.length+
				" turret "+last_TurretHeading);
			}
		return(objs);
		}

	   /**
	    * this is a dummy implementation to keep compatibility with VisualSensorObject.
	    * at this point, vision noise is not built into the class. for an example,
	    * see SimpleCyeSim.
	    */
	  public void setVisionNoise(double mean, double stddev, long seed) { }


	private	long	last_ObjectInGrippert = 0;
	private	int	last_ObjectInGripper = -1;
	private	long	last_ObjectInGrippermem = -1;
	/**
	 * Get the kind of object in the gripper.
	 * @param timestamp only get new information 
	 *        if timestamp > than last call or timestamp == -1 .
	 * @return channel (0-5) which type/color of object 
	 *         `in the gripper, -1 otherwise.
	 * @see MultiForageN150Hard#getVisualObjects
	 */
	public int getObjectInGripper(long timestamp)
		{
		long delaytime = 500; // 0.5 sec memory
		int retval = -1;
		Vec2 gpos = new Vec2(GRIPPER_POSITION,0);
		gpos.sett(last_TurretHeading);
		for(int i = 0; i < 2; i++)
			{
			Vec2[] objs = getVisualObjects(timestamp, i);
			for(int j=0; j<objs.length; j++)
				{
				objs[j].sub(gpos);
				if (objs[j].r < GRIPPER_CAPTURE_RADIUS)
					{
					retval = i;
					break;
					}
				}
			if (retval != -1)
				{
				last_ObjectInGrippermem = timestamp;
				last_ObjectInGripper = retval;
				break;
				}
			}
		long timesince = timestamp - last_ObjectInGrippermem;
		if ((retval == -1)&&(timesince <= delaytime))
			retval = last_ObjectInGripper;
		return(retval);
		}


        protected double  gripper_finger = 1.0;
        protected boolean trigger_mode = false;
        /**
         * Set the gripper "finger" position from 0 to 1, with
         * 0 being closed and 1 being open.
         * In simulation, any setting less than 1 means closed.
         * @param position the desired position from 0 to 1.
         */
        public void setGripperFingers(long time_stamp, double position)
                {
                if (position>1) position = 1;
                if (position>=0)
                        {
                        trigger_mode = false;
                        }
                else if (position==-1)
                        {
                        position = 1;
                        trigger_mode = true;
                        }
                else if (position < 0) position = 0;
                gripper_finger = position;
                }


        protected double  gripper_height = 0;
        /**
         * Set the gripper height from 0 to 1, with
         * 0 being down and 1 being up.
         * In simulation this has no effect.
         * @param position the desired position from 0 to 1.
         */
        public void setGripperHeight(long time_stamp, double position)
                {
                if (position>1) position = 1;
                else if (position<0) position = 0;
                gripper_height = position;
                }


	/**
	 * NOT IMPLEMENTED.
	 * Get an array of Vec2s that represent the locations of 
	 * teammates (Kin).
	 * @param timestamp only get new information if 
	 * timestamp > than last call or timestamp == -1.
	 * @return the sensed teammates.
	 * @see EDU.gatech.cc.is.util.Vec2
	 */
	public Vec2[] getTeammates(long timestamp)
		{
		return(new Vec2[0]);
		}


	/**
	 * NOT IMPLEMENTED.
	 * Get an array of Vec2s that represent the
	 * locations of opponents.
	 * @param timestamp only get new information if 
	 * 	timestamp > than last call or timestamp == -1.
	 * @return the sensed opponents.
	 * @see EDU.gatech.cc.is.util.Vec2
	 */
	public Vec2[] getOpponents(long timestamp)
		{
		return(new Vec2[0]);
		}


	/**
	 * NOT IMPLEMENTED.
	 * Get the robot's player number, between 0
	 * and the number of robots on the team.
	 * Don't confuse this with getID which returns a unique number
	 * for the object in the simulation as a whole, not on its individual
	 * team.
	 * @param timestamp only get new information if 
	 *	timestamp > than last call or timestamp == -1.
	 * @return the player number.
	 */
	public int getPlayerNumber(long timestamp)
		{
		return(0);
		}


	/**
	 * NOT IMPLEMENTED.
	 * Set the maximum range at which kin may be sensed.  Primarily
	 * for use in simulation.
	 * @param r double, the maximum range.
	 */
	public void setKinMaxRange(double r)
		{
		}


        /**
	 * NOT IMPLEMENTED.
         * Broadcast a message to all teammates, except self.
         * @param m Message, the message to be broadcast.
         */
        public void broadcast(Message m)
		{
		}


        /**
	 * NOT IMPLEMENTED.
         * Transmit a message to just one teammate.  Transmission to
         * self is allowed.
         * @param id int, the ID of the agent to receive the message.
         * @param m Message, the message to transmit.
         * @exception CommunicationException if the receiving agent does not
         *              exist.
         */
        public void unicast(int id, Message m) 
                throws CommunicationException
		{
		}


        /**
	 * NOT IMPLEMENTED.
         * Transmit a message to specific teammates.  Transmission to
         * self is allowed.
         * @param ids int[], the IDs of the agents to receive the message.
         * @param m Message, the message to transmit.
         * @exception CommunicationException if one of the receiving agents 
         *              does not exist.
         */
        public void multicast(int[] ids, Message m)
                throws CommunicationException
		{
		}


        /**
	 * NOT IMPLEMENTED.
         * Get an enumeration of the incoming messages.  The messages
         * are automatically buffered by the implementation.
         * Unless the implementation guarantees it, you cannot
         * count on all messages being delivered.
         * Example, to print all incoming messages:
         * <PRE>
         * Transceiver c = new RobotComm();
         * CircularBufferEnumeration r = c.getReceiveChannel();
         * while (r.hasMoreElements())
         *      System.out.println(r.nextElement());
         * </PRE>
         * @return the CircularBufferEnumeration.
         */
        public CircularBufferEnumeration getReceiveChannel()
		{
		return(null);
		}

        /**
	 * NOT IMPLEMENTED.
         * Set the maximum range at which communication can occur.
         * In simulation, this corresponds to a simulation of physical limits,
         * on mobile robots it corresponds to a signal strength setting.
         * @param r double, the maximum range.
         */
        public void setCommunicationMaxRange(double r)
		{
		}

        /**
	 * NOT IMPLEMENTED.
         */
        public boolean connected()
		{
		return(false);
		}

	}