simplen150hard.java

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

		nomad150_hardware.st(); // really stop the robot just in case
		nomad150_hardware.sn_off(); // sonars off
		System.out.println("SimpleN150Hard.stop: avg robot I/O cycle" +
			" took " + run_time_sum/cycles + " ms");
		}


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


	private	long	last_Obstaclest = 0;
	protected	Vec2	last_Obstacles[] = new Vec2[0];
	protected	int	num_Obstacles;
	/**
	 * 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];
		for(int i = 0; i<num_Obstacles; i++)
			retval[i] = new Vec2(last_Obstacles[i].x,
				last_Obstacles[i].y);
		return(retval);
		}


	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);
		}


	private	long	last_Positiont = 0;
	protected Vec2 last_Position = new Vec2(0,0);
	/**
	 * 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 SimpleN150Hard#resetPosition
	 */
	public Vec2 getPosition(long timestamp)
		{
		Vec2 retval = null;
		retval = new Vec2(last_Position.x, last_Position.y);
		return(retval);
		}
		

	Vec2 origin = new Vec2(0,0);
	/**
	 * 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 SimpleN150Hard#getPosition
	 */
	public void resetPosition(Vec2 p)
		{
		origin = new Vec2(p.x - last_Position.x, 
			p.y - last_Position.y);
		}


	private long	last_SteerHeadingt = 0;
	protected double last_SteerHeading = 0;
	protected boolean in_reverse = false;
	/**
	 * 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 SimpleN150Hard#resetSteerHeading
	 * @see SimpleN150Hard#setSteerHeading
	 */
	public double getSteerHeading(long timestamp)
		{
		double retval = 0;
		retval = last_SteerHeading;
		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 SimpleN150Hard#getSteerHeading
	 * @see SimpleN150Hard#setSteerHeading
	 */
	public void resetSteerHeading(double heading)
		{
		/* get the current turret heading */
		nomad150_hardware.gs(); // must do this first
		int turret = nomad150_hardware.get_turret();
		
		/* ensure in legal range */
		heading = Units.ClipRad(heading); 
		
		/* if we're in reverse, the steer heading is PI out */
		if (in_reverse) heading = Units.ClipRad(heading + Math.PI);

		/* set the angles, remember to convert to 10ths of degrees */
		nomad150_hardware.da(Units.RadToDeg10(heading),turret);
		}


	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 SimpleN150Hard#getSteerHeading
	 * @see SimpleN150Hard#resetSteerHeading
	 */
	public void setSteerHeading(long timestamp, double heading)
		{
		/* ensure in legal range */
		desired_heading = Units.ClipRad(heading);
		}
	

	private	long	last_TurretHeadingt = 0;
	protected double last_TurretHeading = 0;
	/**
	 * Get the current heading of the turret motor.
	 * @param timestamp only get new information 
	 *        if timestamp > than last call or timestamp == -1 .
	 * @return the turret heading in radians.
	 * @see SimpleN150Hard#setTurretHeading
	 * @see SimpleN150Hard#resetTurretHeading
	 */
	public double getTurretHeading(long timestamp)
		{
		double retval = 0;
		retval = last_TurretHeading;
		return(retval);
		}

	
	/**
	 * Reset the turret odometry of the robot in global coordinates.
	 * This might be done when reliable sensor information provides
	 * a very good estimate of the robot's turret heading.
	 * 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 turret heading in radians.
	 * @see SimpleN150Hard#getTurretHeading
	 * @see SimpleN150Hard#setTurretHeading
	 */
	public void resetTurretHeading(double heading)
		{
		/* get the current steer heading */
		nomad150_hardware.gs();  // must do this first
		int steering = nomad150_hardware.get_steering();

		/* ensure in legal range */
		heading = Units.ClipRad(heading); 

		/* set the angles, remember to convet to 10ths of degrees */
		nomad150_hardware.da(steering, Units.RadToDeg10(heading));

		/* in_reverse doesn't matter for the turret */
		}

	
	double	desired_turret_heading = 0;
	/**
	 * Set the desired heading for the turret motor.
	 * We assume the turret is not symmetric, so there is no
	 * reverse trick as in the steering motor.
	 * @param heading the heading in radians.
	 * @see SimpleN150Hard#getTurretHeading
	 * @see SimpleN150Hard#resetTurretHeading
	 */
	public void setTurretHeading(long timestamp, double heading)
		{
		/* ensure in legal range */
		desired_turret_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 SimpleN150Hard#setSteerHeading
	 * @see SimpleN150Hard#setBaseSpeed 
	 */ 
	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 = SimpleN150.MAX_TRANSLATION;
	/**
	 * 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 SimpleN150Hard#setSpeed
	 */
	public void setBaseSpeed(double speed)
		{
		if (speed > SimpleN150.MAX_TRANSLATION) speed = SimpleN150.MAX_TRANSLATION;
		else if (speed < 0) speed = 0;
		base_speed = speed;
		}


	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;}

	}