simplecyehard.java

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

				//{
				//Thread.sleep(sleep_time);
				//}
			//catch (InterruptedException e){};
			cycles++;
		  }
		else
		  {
		    Srv.SendStopMotors();
		  }

		}

	boolean canTurn(double angle_between_trailer_and_front, double heading_difference)
		{
		if ((Math.abs(angle_between_trailer_and_front ) <= (Math.PI)/2) || 
			(angle_between_trailer_and_front <= -1*Math.PI/2 && heading_difference > 0.0)
			|| (angle_between_trailer_and_front >= Math.PI/2 && heading_difference < 0.0))
			{
			    CanTurn = true;;
				return true;
			}
		else 
			{
			CanTurn = false;
			return false;
			}
		}			

	public boolean getCommandError(long timestamp)
           {
	     if (CanTurn)
	       return false;
	     else
	       return true;
	   }

        public double getVoltage(long timestamp)
           {
	     return Srv.GetLastB();
	   }

	double dsignum(double a)
		{
		if (a < 0.0)
			{
			return -1.0;
			}
		else
			{
			return 1.0;
			}
		}

	/**
	 * Quit the I/O thread.  Do this only when you are done with the
	 * robot! 
	 */
	public void quit()
		{
		time_sum = (double)(System.currentTimeMillis() - start_time);
		System.out.println("SimpleCyeHard.stop: stopping the robot");
		Srv.SendStopMotors(); // stops the robot
		keep_running = false; // stop the thread
		System.out.println("SimpleCyeHard.stop: avg robot I/O cycle" +
			" took " + run_time_sum/cycles + " ms");

		// vision stuff - again, take out etc. when Visual Object
		myVision.quit();
		}


	/**
	 * Gets time elapsed since the robot was instantiated.  Unlike 
	 * simulation, this is real elapsed time.
	 */
	public long getTime()
		{
		return(System.currentTimeMillis() - start_time);
		}


	/**
	 * Get an array of Vec2s that point egocentrically from the
	 * center of the robot to the obstacles currently sensed by the 
	 * bumpers and sonars.
	 * @param timestamp only get new information 
	 *        if timestamp > than last call or timestamp == -1 .
	 * @return the sensed obstacles.
	 */
	public Vec2[] getObstacles(long timestamp)
		{
		Vec2[] retval = null;
		retval = new Vec2[num_Obstacles];
		if (num_Obstacles == 1)
		  {
		   Vec2 tmp = new Vec2(WIDTH/2.0,0);
		   tmp.sett(steer.t);
		   retval[0] = new Vec2(tmp);
		  }
		return(retval);
		}

	/**
	 * 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.
	 */

	private	long	last_VisualObjectst = 0;
	    
	    // take out once remove out of VisualObjectSensor
	public void setVisionNoise(double mean, double stddev, long seed)
	    {
	    }
	
	public Vec2[] getVisualObjects(long timestamp, int channel)
		{
		  int colour_id = 0; // default
		  // convert channel into appropriate colour id
		  if (channel == 3) // in simulator this is soccer ball
		    colour_id = 0; // 
		  else if (channel == 2) // in simulator this is obstacle
		      colour_id = 1;
		  else if (channel == 4) // orange objects
		      colour_id = 4;
		  else if (channel == 5) // yellow objects
		      colour_id = 2;
		  else if (channel == 6) // blue objects
		      colour_id = 3;
		  else if (channel == 7) // green objects
		      colour_id = 4;
		  boolean visionSuccess = myVision.processFrame();
		  while(!visionSuccess)
		    {
		      System.out.println("problems processing Frame");
		      visionSuccess = myVision.processFrame();
		    }
		  int numberRegions = myVision.getNumRegions(colour_id);
		  JCMVision.Region myRegions[] = myVision.getRegions(colour_id, numberRegions);

		  // THIS IS A HACK FOR NOW - ensures only one ball returned but get all the obstacles
		  if ((channel == 3 || channel == 4) && numberRegions !=0 )
		      numberRegions = 1;
		  // THIS IS A HACK FOR NOW

		  // now to convert to coordinates
		  double xVal,yVal;
		  int x1,y1,x2,y2,xwidth,ywidth, ballSize;

		  double newTheta, equivX,equivY, distance;
		    if (numberRegions == 0 && channel == 3)
		      {
			  Srv.SendBuzzerOn(true);
			  Srv.Wait(100); // 100 ms
			  Srv.SendBuzzerOn(false);
		      }
		  
		    Vector tempObjs = new Vector();
		    int numberValidRegions = 0;
		    boolean addObject = true;

		  for(int i=0;i<numberRegions;i++)
		    {
			addObject = true;
			equivX = 0.0;
			equivY = 0.0;
		      x1 = myRegions[i].x1;
		      x2 = myRegions[i].x2;
		      y1 = myRegions[i].y1;
		      y2 = myRegions[i].y2;

		      xVal = ((double)x1+(double)x2)/2.0;
		      yVal = ((double)y1+(double)y2)/2.0;
		      

		      xwidth = Math.abs(x1-x2);
		      ywidth = Math.abs(y1-y2);
		      
		      

	       	      // CONVERT FROM PIXELS TO X,Y LOCATION - based on size
	       	      newTheta = Math.atan(1.0/(XWIDTH/Math.tan(FOV/2.0)))*(XWIDTH-xVal);
	       	      if (newTheta < 0.0)
	       		  newTheta += 2*Math.PI; // get between 0 and 2pi
	       	      System.out.println("new theta " + newTheta);

		       if (channel == 3 || channel == 4)
		       	  {
			      // ball 

			      ballSize = Math.min(MAX_BALL_PIXEL,  Math.max(xwidth,ywidth));
		      
			      distance = DistanceLookUp[ballSize];
			      equivY = distance*Math.cos(newTheta);
			      equivX = distance*Math.sin(newTheta);
			  }
		       else if (channel == 2 || channel == 5 || channel == 6 || channel == 7)
			   {
			       // obstacles
			       
			       /*			       if (y2 >= 70)
				   distance = -0.0039*y2+0.5688;
			       else
				   distance = 38.86*Math.pow(y2, -1.124);
			       */
			       try {
				      
			       distance = Math.min(Math.min(PixelToDistance[Math.min(159, (int)xVal)][y2],PixelToDistance[Math.min(159,x1)][y2]),PixelToDistance[Math.min(159, x2)][y2]);
			       if (distance == PixelToDistance[Math.min(159, x1)][y2])
				   	newTheta = Math.atan(1.0/(XWIDTH/Math.tan(FOV/2.0)))*(XWIDTH-x1);
			       if (distance == PixelToDistance[Math.min(159, x2)][y2])
				   	newTheta = Math.atan(1.0/(XWIDTH/Math.tan(FOV/2.0)))*(XWIDTH-x2);

			       } catch (Exception e) {
				   System.err.println(e);
				   System.err.println("x1 = "+x1);
				   System.err.println("x2 = "+x1);
				   System.err.println("y2 = "+y2);
				   System.err.println("xVal = "+xVal);
				   distance=1;
			       }
			       System.err.println("x, y " + xVal + " " + y2 + " " + distance);
			       equivY = distance*Math.cos(newTheta);
			       equivX = distance*Math.cos(newTheta);

			       if (Math.min(xwidth, ywidth) <= 2)
				   addObject = false; // too small to worry about - noise
			       if (y2 >= 119 && y1 >= 110 && xVal > 77 && xVal < 83)
				   addObject = false; // is itself so don't add

			   }
		       if (addObject)
			   {
		       Vec2 tObj = new Vec2(equivX, equivY);
		      tObj.sett(newTheta+steer.t); // now returns heading independent of robot's heading
		      tempObjs.addElement(tObj);
		      numberValidRegions++;
			   }
		    }

		 Vec2[] objs = new Vec2[numberValidRegions];
		 for(int r=0;r<numberValidRegions;r++)
		     {
			 objs[r] = new Vec2((Vec2)tempObjs.elementAt(r));
		     }
	  
		return(objs);
		}

	protected int	obstacle_rangeInch = (int)(Units.MeterToInch(1.0)+0.5);
	private	double	obstacle_rangeM = 1.0;
	/**
	 * Set the maximum range at which a sensor reading should be considered
	 * an obstacle.  Beyond this range, the readings are ignored.
	 * The default range on startup is 1 meter.
	 * @param range the range in meters.
	 */
	public void setObstacleMaxRange(double range)
		{
		obstacle_rangeM = range;
		obstacle_rangeInch = (int)(Units.MeterToInch(range)+0.5);
		}


	/**
	 * Get the position of the robot in global coordinates.
	 * @param timestamp only get new information 
	 *        if timestamp > than last call or timestamp == -1.
	 * @return the position.
	 * @see SimpleCyeHard#resetPosition
	 */
	public Vec2 getPosition(long timestamp)
		{
		Vec2 retval = null;
		retval = new Vec2(last_Position.x, last_Position.y);
		return(retval);
		}
		

	/**
	 * Reset the odometry of the robot in global coordinates.
	 * This might be done when reliable sensor information provides
	 * a very good estimate of the robot's location, or if you
	 * are starting the robot in a known location other than (0,0).
	 * Do this only if you are certain you're right!
	 * @param position the new position.
	 * @see SimpleCyeHard#getPosition
	 */
	public void resetPosition(Vec2 p)
		{
		origin = new Vec2(p.x - last_Position.x, 
			p.y - last_Position.y);
		}


	/**
	 * Get the current heading of the steering motor (radians).
	 * @param timestamp only get new information 
	 *        if timestamp > than last call or timestamp == -1 .
	 * @return the heading in radians.
	 * @see SimpleCyeHard#resetSteerHeading
	 * @see SimpleCyeHard#setSteerHeading
	 */
	public double getSteerHeading(long timestamp)
		{
		double retval = 0;
		retval = steer.t;
		return(retval);
		}

	
	/**
	 * Reset the steering odometry of the robot in 
	 * global coordinates.
	 * This might be done when reliable sensor information provides
	 * a very good estimate of the robot's heading, or you are starting
	 * the robot at a known heading other than 0.
	 * Do this only if you are certain you're right!
	 * It is also a good idea not to be moving when you do this.
	 * @param heading the new heading in radians.
	 * @see SimpleCyeHard#getSteerHeading
	 * @see SimpleCyeHard#setSteerHeading
	 */
	public void resetSteerHeading(double heading)
		{
		  // this function currently does nothing
		}


	protected double desired_heading;
	/**
	 * Set the desired heading for the steering motor.
	 * If the turn is greater than 90 degrees we set the turn
	 * to be less than that and move in reverse.
	 * @param heading the heading in radians.
	 * @see SimpleCyeHard#getSteerHeading
	 * @see SimpleCyeHard#resetSteerHeading
	 */
	public void setSteerHeading(long timestamp, double heading)
		{
		/* ensure in legal range */
		desired_heading = Units.ClipRad(heading);
		}
	


	
	protected double desired_speed = 0;
	/**
	 * Set the desired speed for the robot (translation).  
	 * The speed must be between 0.0 and 1.0; where 0.0 is stopped
	 * and 1.0 is "full blast".  It will be clipped
	 * to whichever limit it exceeds.  
	 * The actual commanded speed is zero until the 
	 * steering motor is close to the desired heading.
	 * Use setBaseSpeed to adjust the top speed.
	 * @param timestamp only get new information 
	 *        if timestamp > than last call 
	 * @param speed the desired speed from 0 to 1.0, where 1.0 is the 
	 *	base speed.
	 * @see SimpleCyeHard#setSteerHeading
	 * @see SimpleCyeHard#setBaseSpeed 
	 */ 
	public void setSpeed(long timestamp, double speed)
		{
		/* ensure legal range */
		if (speed > 1.0) speed = 1.0;
		if (speed < -1.0) speed = -1.0; // changed this to allow negative speeds
		// else if (speed < 0) speed = 0;
		desired_speed = speed;
		}


	protected double base_speed = SimpleCye.MAX_TRANSLATION*SPEED_CONVERSION;
	/**
	 * Set the base speed for the robot (translation) in
	 * meters per second.
         * Base speed is how fast the robot will move when
         * setSpeed(1.0) is called.
	 * The speed must be between 0 and MAX_TRANSLATION.  
	 * It will be clipped to whichever limit it exceeds.
	 * @param speed the desired speed from 0 to 1.0, where 1.0 is the 
	 *	base speed.
	 * @see SimpleCyeHard#setSpeed
	 */
	public void setBaseSpeed(double speed)
		{
		if (speed > SimpleCye.MAX_TRANSLATION) speed = SimpleCye.MAX_TRANSLATION;
		else if (speed < 0) speed = 0;
		base_speed = speed*SPEED_CONVERSION;
		}


	private String display_string = "blank";
	/**
         * Set the String that is printed on the robot's display.
         * For real robots, this appears printed on the console.
         * @param s String, the text to display.
         */
        public void setDisplayString(String s)
		{
		// only print it if it is different than last time
		if (display_string != s)
			{
			display_string = s;
			System.out.println(display_string);
			}
		}

	  public Color getForegroundColor() { return Color.black;}
	  public Color getBackgroundColor() { return Color.black;}

	    public double sgn(double val)
	    {
		if(val < 0.0)
		    return -1.0;
		else if(val > 0.0)
		    return 1.0;
		else
		    return 0;
	    }
}