multiforagen150hard.java

一个多机器人的仿真平台

/*
 * MultiForageN150Hard.java
 */

package EDU.gatech.cc.is.abstractrobot;

import java.util.Enumeration;
import EDU.gatech.cc.is.util.Vec2;
import EDU.gatech.cc.is.util.Units;
import EDU.gatech.cc.is.nomad150.Ndirect;
import EDU.gatech.cc.is.communication.*;
import EDU.gatech.cc.is.util.*;
import EDU.gatech.cc.is.newton.*;


/**
 * MultiForageN150Hard implements MultiForageN150 for
 * Nomad 150 hardware using the Ndirect class.
 * You should see the specifications in MultiForageN150
 * and Ndirect class documentation for details.
 * <P>
 * To reduce I/O between the controller and the robot, a thread is
 * set up to perform periodic I/O.  The sensor data and motor commands
 * are exchanged through MultiForageN150Hard class variables (globals).
 *
 * <P>
 * <A HREF="../COPYRIGHT.html">Copyright</A>
 * (c)1997, 1998 Tucker Balch
 *
 * @see MultiForageN150
 * @see EDU.gatech.cc.is.nomad150.Ndirect
 * @see EDU.gatech.cc.is.Newton
 * @author Tucker Balch
 * @version $Revision: 1.2 $
 */

public class MultiForageN150Hard extends SimpleN150Hard 
	implements MultiForageN150
	{
	protected Newton	newt = null;
	protected NewtonTrans nt;	
	private static final boolean DEBUG = false;	
	private static final int GRIPPER_F_RANGE = -1050;  //microseconds
	private static final int GRIPPER_F_BASE  = 1800;   //microseconds
	private static final int GRIPPER_H_RANGE = -700;   //microseconds
	private static final int GRIPPER_H_BASE  = 2000;   //microseconds

        /**
         * Instantiate a <B>MultiForageN150Hard</B> object.  You should only
         * instantiate one of these per robot connected to your
         * computer.  Standard call is MultiForageN150Hard(1,38400);
         * @param serial_port 1 = ttys0 (COM1), 2 = ttys1 (COM2) ...
         * @param baud baud rate for communication.
         * @exception Exception If unable to configure the hardware.
         */
	public MultiForageN150Hard(int serial_port, int baud) throws Exception
		{
		super(serial_port, baud);

		/*--- open up the newton hardware ---*/
		try 
			{
			newt = new Newton(3, 38400);
			}
		catch (Exception e)
			{
			System.out.println("MultiForageN150Hard: "+e);
			System.out.println("MultiForageN150Hard: run will "+
				"continue without vision support.");
			newt = null;
			}
		if (newt != null) nt = new NewtonTrans(newt,"newton.cfg");
		}


	protected double	old_gripper_finger = -99;
	protected double old_gripper_height = -99;
	protected int	hard_command = 0;
	protected int	old_hard_command = 0;

	/**
	 * Body of the thread that conducts periodic I/O with
	 * the robot.  It runs at most every MultiForageN150Hard.MIN_CYCLE_TIME 
	 * milliseconds to gather sensor data from the robot, and issue
	 * movement commands.
	 */
	public void run()
		{
		double	uncorrected_steering = 0;
		double	turn = 0;
		double	turret_turn = 0;
		double	speed_command = 0;
		int	result = 0;
		long	current_time, this_cycle, sleep_time;
		int	grip_obj = 0;

		if(keep_running)
			{
			/*--- mark current time ---*/
			current_time = System.currentTimeMillis();

			/*--- check vision first ---*/
			// read in the data
			if (newt!=null)
				{
				//for(int k=0; k<10; k++)
					nt.read_frame();
				}
			// check to see if something is in the gripper
			grip_obj = getObjectInGripper(-1);

			// close gripper if something there
			if (trigger_mode && (grip_obj == 1 || grip_obj == 0))
				gripper_finger = 0;

			/*--- move the gripper fingers ---*/
			if (old_gripper_finger != gripper_finger)
				{
				// 0 is closed, 1 is open
				// System.out.println("gripper finger "+gripper_finger);
				old_gripper_finger = gripper_finger;
				int gripper_finger_hard = (int)((double)
					GRIPPER_F_RANGE * gripper_finger
					+ (double)GRIPPER_F_BASE);
				nomad150_hardware.mv(Ndirect.MV_PWM_LOW_0, 
					15000-gripper_finger_hard,
					Ndirect.MV_IGNORE,0,
					Ndirect.MV_IGNORE,0);
				nomad150_hardware.mv(Ndirect.MV_PWM_HIGH_0, 
					gripper_finger_hard,
					Ndirect.MV_IGNORE,0,
					Ndirect.MV_IGNORE,0);
				}

			/*--- move the gripper height next ---*/
			if (old_gripper_height != gripper_height)
				{
				int gripper_height_hard = 0;
				// 0 is down 1 is up
				// System.out.println("gripper height "+gripper_height);
				old_gripper_height = gripper_height;
				gripper_height_hard = (int)((double)
					GRIPPER_H_RANGE * gripper_height
					+ (double)GRIPPER_H_BASE);
				nomad150_hardware.mv(Ndirect.MV_PWM_LOW_1, 
					15000-gripper_height_hard,
					Ndirect.MV_IGNORE,0,
					Ndirect.MV_IGNORE,0);
				nomad150_hardware.mv(Ndirect.MV_PWM_HIGH_1, 
					gripper_height_hard,
					Ndirect.MV_IGNORE,0,
					Ndirect.MV_IGNORE,0);
				}

			/*--- fill in the sensor data ---*/
			// ping robot to get the sensor data
			nomad150_hardware.gs();

			// position
			double x = Units.Inch10ToMeter(
				nomad150_hardware.get_x());
                        double y = Units.Inch10ToMeter(
				nomad150_hardware.get_y());
                        last_Position.setx(x);
                        last_Position.sety(y);
			last_Position.add(origin);

			// steer heading
                        uncorrected_steering = Units.Deg10ToRad(
				nomad150_hardware.get_steering());
                        if (in_reverse)
                        	{
                               	// rotate by 180
                               	last_SteerHeading = Units.ClipRad(
                                       	last_SteerHeading + Math.PI);
                               	}
			else
				last_SteerHeading = uncorrected_steering;
			// turret heading
			last_TurretHeading = Units.Deg10ToRad(
				nomad150_hardware.get_turret());

			// sonar data
			// get the raw data
			nomad150_hardware.get_sn(sonar_raw_data);
			long bumps = nomad150_hardware.get_bp();
			if (bumps != 240)
				num_Obstacles = 1;
			else
				num_Obstacles = 0;
			// hack so as to ignore sonar from sensor 0
			sonar_raw_data[0] = obstacle_rangeInch+100;
			for(int i = 0; i<16; i++) 
				if (sonar_raw_data[i] + 
					(int)Units.MeterToInch(SONAR_RADIUS) <
					obstacle_rangeInch)
					num_Obstacles++;
			last_Obstacles = new Vec2[num_Obstacles];
			int j = 0;
			for(int i = 0; i<16; i++) 
				if (sonar_raw_data[i] +
					(int)Units.MeterToInch(SONAR_RADIUS) <
					obstacle_rangeInch)
					{
					last_Obstacles[j] = new Vec2();
					last_Obstacles[j].setr(SONAR_RADIUS +
						Units.InchToMeter(
							sonar_raw_data[i]));
					last_Obstacles[j].sett(
						last_TurretHeading +
						(i * Units.PI2/16));
					j++;
					}
			if (bumps != 240)
				{
				Vec2 tmpvec = new Vec2(RADIUS,0);
				// top ring
				if (bumps == 65776) 
					tmpvec.sett(0);
				else if (bumps == 262384) 
					tmpvec.sett(Units.DegToRad(1*36));
				else if (bumps == 1048816) 
					tmpvec.sett(Units.DegToRad(2*36));
				else if (bumps == 4194544) 
					tmpvec.sett(Units.DegToRad(3*36));
				else if (bumps == 496) 
					tmpvec.sett(Units.DegToRad(4*36));
				else if (bumps == 1264) 
					tmpvec.sett(Units.DegToRad(5*36));
				else if (bumps == 4336) 
					tmpvec.sett(Units.DegToRad(6*36));
				else if (bumps == 16624) 
					tmpvec.sett(Units.DegToRad(7*36));
				else if (bumps == 241) 
					tmpvec.sett(Units.DegToRad(8*36));
				else if (bumps == 244) 
					tmpvec.sett(Units.DegToRad(9*36));
				// bottom ring
				else if (bumps == 131312) 
					tmpvec.sett(Units.DegToRad(18+0*36));
				else if (bumps == 524528) 
					tmpvec.sett(Units.DegToRad(18+1*36));
				else if (bumps == 2097392) 
					tmpvec.sett(Units.DegToRad(18+2*36));
				else if (bumps == 8388848) 
					tmpvec.sett(Units.DegToRad(18+3*36));
				else if (bumps == 752) 
					tmpvec.sett(Units.DegToRad(18+4*36));
				else if (bumps == 2288) 
					tmpvec.sett(Units.DegToRad(18+5*36));
				else if (bumps == 8432) 
					tmpvec.sett(Units.DegToRad(18+6*36));
				else if (bumps == 33008) 
					tmpvec.sett(Units.DegToRad(18+7*36));
				else if (bumps == 242) 
					tmpvec.sett(Units.DegToRad(18+8*36));
				else if (bumps == 248) 
					tmpvec.sett(Units.DegToRad(18+9*36));
				else
					{
					tmpvec.setr(999);
					}
				last_Obstacles[num_Obstacles-1] = tmpvec;
				}

			//--- compute steering command
			// figure out best turn direction
			turn = Units.BestTurnRad(uncorrected_steering, 
				desired_heading);

			// now decide whether to go in reverse or forward 
			if (turn>(Math.PI/2))
                        	{
                        	in_reverse = true;
                        	turn = turn - Math.PI;
                        	}
                	else if (turn<-(Math.PI/2))
                        	{
                        	in_reverse = true;
                        	turn = turn + Math.PI;
                        	}
                	else in_reverse = false;

			//--- compute turret steering command
			turret_turn = Units.BestTurnRad(
				last_TurretHeading,
				desired_turret_heading);

			//--- compute speed command
			// only go if within 90 deg of proper heading 
			// if (Math.abs(turn)<(Math.PI/3.0))
			if (Math.abs(turret_turn)<(Math.PI/2.0))
				speed_command = desired_speed;
			else speed_command = 0;
			// go backwards if in reverse 
			if (in_reverse) speed_command *= -1;
			hard_command = Units.MeterToInch10(