simplen150sim.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

		/* check if we should go in reverse */
		double turn = Units.BestTurnRad(steer.t, desired_heading);
		if (Math.abs(turn)>(Math.PI/2))
			{
			in_reverse = true;
			desired_heading = 
				Units.ClipRad(desired_heading+Math.PI);
			}
		else in_reverse = false;
		}
	

	/**
	*/
	public double getTurretHeading(long timestamp)
		{
		return(turret);
		}

	
	/**
	*/
	public void resetTurretHeading(double heading)
		{
		/* ensure in legal range */
		heading = Units.ClipRad(heading); 

		turret = heading;
		}

	
	double	desired_turret_heading = 0;
	/**
	*/
	public void setTurretHeading(long timestamp, double heading)
		{
		/* ensure in legal range */
		desired_turret_heading = Units.ClipRad(heading);
		}

	
	private double desired_speed = 0;
	/**
	*/ 
	public void setSpeed(long timestamp, double speed)
		{
		/* ensure legal range */
		if (speed > 1.0) speed = 1.0;
		else if (speed < 0) speed = 0;
		desired_speed = speed;
		}


	protected double base_speed = MAX_TRANSLATION;
	/**
	*/
	public void setBaseSpeed(double speed)
		{
		if (speed > MAX_TRANSLATION) speed = MAX_TRANSLATION;
		else if (speed < 0) speed = 0;
		base_speed = speed;
		}


	private	boolean	gripper_closed = false;
	private boolean trigger_mode = false;
	private SimulatedObject in_gripper = null;
	/**
	 * 0 closed 1 open.  -1 trigger mode.
	 */
	public void setGripperFingers(long timestamp, double grip)
		{
		if (grip>=1)
			{
			grip = 1;
			gripper_closed = false;
			trigger_mode = false;
			}
		if (grip==0)
			{
			trigger_mode = false;
			}
		if (grip < 0)
			{
			grip = 1; //open
			trigger_mode = true;
			gripper_closed = false;
			}

		//gripper_closed = true;
		/*--- if closing the gripper ---*/
		// see if there is something to pick up and gripper not already
		// closed
		if (((grip == 0)||trigger_mode)
			&&(gripper_closed==false)
			&&(in_gripper == null))
			{
			/*--- find gripper position in global coord ---*/
			Vec2 gpos = new Vec2(RADIUS,0);
			gpos.sett(turret);
			gpos.add(position);

			/*--- check all objects ---*/
			for(int i = 0; i<all_objects.length; i++)
				{
				/*--- check if it's not self ---*/
				if (all_objects[i].getID() != unique_id)
					{
					Vec2 tmp = all_objects[i].getCenter(gpos);

					// check if can pick it up
					if ((tmp.r< MultiForageN150.GRIPPER_CAPTURE_RADIUS)
						&&(all_objects[i].isPickupable())
						&&(all_objects[i].getVisionClass()>=0))
						{
						in_gripper = all_objects[i];
						all_objects[i].pickUp( (SimulatedObject) this);
						trigger_mode = false;
						gripper_closed = true;
						break;
						}
					}
				}
			}
		else if (grip == 1)
		/*--- if opening the gripper ---*/
			{
			// see if we should put something down
			if (in_gripper != null)
				{				
				Vec2 gpos = new Vec2(RADIUS,0);
				gpos.sett(turret);
				gpos.add(position);
				in_gripper.putDown(gpos);
				
				in_gripper = null;
				}
			//System.out.println("cooperative needed. ");
			gripper_closed = false;
			trigger_mode = false;
			}
		if (grip == 0)
			{
			trigger_mode = false;
			gripper_closed = true;
			}
		}


        private long    last_Opponentst = 0;
        private Vec2    last_Opponents[];
        /**
         * Get an array of Vec2s that point egocentrically from the
         * center of the robot to the opponents currently sensed by the
         * robot
         * @param timestamp only get new information if 
	     *	timestamp > than last call or timestamp == -1 .
         * @return the sensed Opponents.
         */
        public Vec2[] getOpponents(long timestamp)
                {
                if((timestamp > last_Opponentst)||(timestamp == -1))
                        {
                        if (timestamp != -1) last_Opponentst = timestamp;
                        last_Opponents = kin_sensor.getOpponents(all_objects);
                        }
                return(last_Opponents);
                }


        /**
         * Return an int represting the player's ID on the team.
         * This value may not be valid if the simulation has not
         * been "set up" yet.  Do not use it during initialization.
         * @param timestamp only get new information if
         *      timestamp > than last call or timestamp == -1 .
         * @return the number.
         */
        public int getPlayerNumber(long timestamp)
                {
                return(kin_sensor.getPlayerNumber(all_objects));
                }


        private long    last_Teammatest = 0;
        private Vec2    last_Teammates[] = new Vec2[0];
        /**
         * Get an array of Vec2s that point egocentrically from the
         * center of the robot to the teammates currently sensed by the
         * robot.
         * @param timestamp only get new information if
         *      timestamp > than last call or timestamp == -1 .
         * @return the sensed teammates.
         */
        public Vec2[] getTeammates(long timestamp)
                {
                if((timestamp > last_Teammatest)||(timestamp == -1))
                        {
                        if (timestamp != -1) last_Teammatest = timestamp;
                        last_Teammates = kin_sensor.getTeammates(all_objects);
                        }
                return(last_Teammates);
                }


        private double  kin_rangeM = 4.0;
        /**
         * Set the maximum range at which a sensor reading should be considered
         * kin.  Beyond this range, the readings are ignored.
         * Also used by opponent sensor.
         * The default range on startup is 1 meter.
         * @param range the range in meters.
         */
        public void setKinMaxRange(double range)
                {
                kin_rangeM = range;
                kin_sensor.setKinMaxRange(range);
                }

	  public Color getForegroundColor() { return foreground; }

	  public Color getBackgroundColor() { return background; }


	/**
	 * NOT IMPLEMENTED
	 */
	public void setGripperHeight(long timestamp, double position)
		{
		/* todo */
		}


        /*--- Transceiver methods ---*/


        public void multicast(int[] ids, Message m)
                throws CommunicationException
                {
                transceiver.multicast(ids, m, all_objects);
                }

        public void broadcast(Message m)
                {
                transceiver.broadcast(m, all_objects);
                }

        public void unicast(int id, Message m)
                throws CommunicationException
                {
                transceiver.unicast(id, m, all_objects);
                }

        public CircularBufferEnumeration getReceiveChannel()
                {
                return(transceiver.getReceiveChannel());
                }

        public void setCommunicationMaxRange(double m)
                {
                transceiver.setCommunicationMaxRange(m);
                }

        public void receive(Message m)
                {
                transceiver.receive(m);
                }

        public boolean connected()
                {
                return(true);
                }
                
                
        /////////////////////////////////////////////////////
        ////////The following codes are added by yym ////////
        /////////////////////////////////////////////////////
        
        private int waitFoodID = -1;
        
        // get the food ID of visual class 1.
        public int     getFoodID()
        {
        	if (!isWaiting)
        	{
        		if (in_gripper == null)
        			return -1;
        		if (in_gripper.getVisionClass() == 1)
        			return in_gripper.getID();
        		return -1;	
        	}
        	else
        	return waitFoodID;
        }
        
        // test whether the robot can get help to deliver the food.
        public boolean canGetHelp()
        {
        	if (in_gripper == null)
        		return false;        	
            if (in_gripper.getVisionClass() != 1)
            	return false;
            	   
			for(int i = 0; i<all_objects.length; i++)
			{
				if (all_objects[i].getVisionClass()== visionclass &&
				(all_objects[i].getID() != unique_id)&&
				(all_objects[i] instanceof SimpleN150))
				{
					if(((SimpleN150)all_objects[i]).getFoodID() == in_gripper.getID())
					{
						//notice the robot to escape waiting state
						((SimpleN150)all_objects[i]).setWaitTag(false);
						
						return true;
					}
					
				}
			}
			
			// record the food id which the robot is going to wait for
			// because while wait in_gripper will be null. 
			waitFoodID = in_gripper.getID();   
			    	
        	return false;        
        }
        
        // set the waiting tag 
        public  void  setWaitTag(boolean wait)
        {
        	isWaiting = wait;
        }
        // get the waiting tag
        public  boolean  getWaitTag()
        {
        	return isWaiting;
        }
        
        // before enter wait set the in_gripper as null
        public  void    setInGripper()
        {
        	((CoForageAttractorSim)in_gripper).setPickedUp(false); // for painting.
        	in_gripper = null;	
        }
        
        // Get the vision class type of the holding food.
        public  int     getFoodType()
        {
        	if (in_gripper != null)
        		return in_gripper.getVisionClass();
        	else 
        		return -1;
        }
        
        // check whether the new position of the robot will overlap with others.
        public  boolean checkOverlap(double safeRange)
        {
        	boolean isNear = false;
        	
        	for(int i = 0; i<all_objects.length; i++)
			{
				/*--- check if it's an obstacle or a pickupable food and not self ---*/
				if ((all_objects[i].getID() != unique_id) &&
						(all_objects[i].isObstacle() || all_objects[i].isPickupable()))
				{
					Vec2 tmp = all_objects[i].getClosestPoint(position);
					if (tmp.r < safeRange)
					{
						isNear = true;
						break;	
					}					
				}
			}
			return isNear;        	
        }
        
        // get the neighbor
        public Vec2[]  getNeighbour()
        {   
        // -1 implies return new value, 2 means vision class.    			
			return getVisualObjects(-1,2);
        }
        
        
        ///////by yxh 05220207 
        // get neighbours' location 
        public Vec2 getNeighbourLocation( int neighbourID )
        {
        	
        	Vec2 retval = new Vec2(0,0);
        		
			for(int i = 0; i<all_objects.length; i++)
				{
				
				  if(all_objects[i].getID() == neighbourID)
					{
						Vec2 tmp = all_objects[i].getCenter(
							position);
						if ((tmp.r<MultiForageN150.VISION_RANGE)&&
							  (Math.abs(Units.BestTurnRad(turret,tmp.t))
								< (MultiForageN150.VISION_FOV_RAD/2)))												
							retval = all_objects[i].getPosition();
						
						break; 
					}
				}		
			
		return(retval);
        }
        
        //////by yxh 052907    get the max id of the robots
        public int maxID()
        {
           	
           	//the channel of the robots' is 1,the channel of the obstacles is 2,used for distinguishing
           	int last_channel = 1;
           	int max_id = 0;
        	for (int i = 0; i<all_objects.length; i++)
        	{
        		if (all_objects[i].getVisionClass()==last_channel&&max_id < all_objects[i].getID())
        		max_id = all_objects[i].getID();
        	}
        	return (max_id);
        }
	}