package EDU.cmu.cs.coral.cye;import java.io.*;import Serialio.*;/** * Handles all of the communication between the JCyeSrv class  * and the Cye robot.  Thread is extended, to enable the CyeComm  * object to run in the background and poll the robot at a set frequency. * * If you are creating an instance of this class manually in your code, you * are almost certainly doing something wrong.  This class should only  * be created as a side effect of instantiating a JCyeSrv object. * * @author Brian Chemel and Tucker Balch * * @see java.lang.Thread * @see EDU.cmu.cs.coral.cye.JCyeSrv * @see EDU.cmu.cs.coral.cye.JCyeMsg * @see EDU.cmu.cs.coral.cye.JCyeStatus */public class JCyeComm extends Thread 	{	/*	 * Useful Constants	 */	/**	 * Indicates the connection to the robot is wired (no radio 	 * warm up reqd).	 */	public final static int WIRED = 0;	/**	 * Indicates the connection is via old radio (must wait for warm up).	 */	public final static int OLD_RADIO = 1;	/**	 * Indicates the connection is via new radio (must send a bunch of padding chars).	 */	public final static int NEW_RADIO = 2;	/**	 * Indicates the connection is to an original robot.	 */	public final static byte ORIGINAL_ROBOT = 0;	/**	 * Indicates the connection is to a newer black robot;	 */	public final static byte BLACK_ROBOT = 1;	/**	 * Indicates the connection is to a newer orange robot;	 */	public final static byte ORANGE_ROBOT = 3;	/**	 * Indicates the connection is to a newer yellow robot;	 */	public final static byte YELLOW_ROBOT = 4;	/**	 * Indicates the connection is to a newer chrom robot;	 */	public final static byte CHROME_ROBOT = 5;	/*	 * Useful Constants	 */	    private final static int INIT_ATTEMPTS = 5;	private final static int TX_ATTEMPTS = 10;	private final static int SLEEP_TIME = 100;	private final static int RADIO_WARMUP_TIME = 45;	private final static int TX_DRAIN_TIME = 40;	    private final static int PADDING_CHARS = 40;	    private final static byte PADDING_CHARACTER = (byte)240;	private final static boolean DEBUG = false;	/*	 * Class Data	 */ 	private String dev_name = "/dev/ttyS0";	private int baudrate = 9600;	private SerialPortLocal serPort;	private SerialConfig serCfg;	private int SendMsgNum;	private int AckMsgNum;	private int lastX;	private int lastY;	private int lastH;	private double lastB;	private boolean sawObstacle;	private int connection = WIRED;	private byte id = 0;	/** 	 * The constructor for class <code>JCyeComm</comm>. 	 * Initializes the serial port driver, then carries 	 * out the ISTART/ASTART sequence to initialize the robot.	 *	 * @param d the name of the serial device to open.	 * @param b baud rate (9600 or 19200).	 * @param c connection type (WIRED or OLD_RADIO or NEW_RADIO).	 * @param i ID number.	 */	protected JCyeComm(String d, int b, int c, byte i) throws JCyeException		{		dev_name = d;		if (b == 9600)			baudrate = SerialConfig.BR_9600;		else if (b == 19200)			baudrate = SerialConfig.BR_19200;		else			throw (new JCyeException(" invalid baud rate"));		connection = c;		if ((c != WIRED)&&(c != OLD_RADIO)&&(c != NEW_RADIO))			throw (new JCyeException(" invalid connection type"));		if(DEBUG) {		    System.out.println("Connection type: ");		    switch(c) {		    case(WIRED):			System.out.println(" Wired");			break;		    case(OLD_RADIO):			System.out.println(" Old Radio");			break;		    case(NEW_RADIO):			System.out.println(" New Radio");			break;		    default:			System.out.println(" Unknown");			break;		    }		}		id = i;		SendMsgNum = 0;		AckMsgNum = 0;		try			{			InitializeSerialPort();			}		catch (IOException e)			{			System.err.println(e);			throw (new JCyeException("Couldn't intialize port"));			}		boolean initSuccess = InitializeRobot();		int initRetry = 0;		while((initSuccess == false) && (initRetry < INIT_ATTEMPTS)) {		    initSuccess = InitializeRobot();		    initRetry++;		}		if(initSuccess == false)			{	    		throw (new JCyeException("Error: Unable to initialize serial comm with robot."));			}		}	/**	 * Initialize the <code>JCyeComm</code> object's 	 * <code>SerialPortLocal</code>.  First creates a 	 * <code>SerialConfig</code> object with the proper settings,	 * then uses that to create the <code>SerialPortLocal</code>.	 */	private void InitializeSerialPort() throws IOException		{		    if(DEBUG) {			System.out.println("Initializing serial connection with: ");			System.out.println(" device name: " + dev_name);			System.out.println(" baud rate: " + baudrate);		    } 		// Create a SerialConfig object with the 		// proper settings for OLD radio link		serCfg = new SerialConfig(dev_name);		serCfg.setBitRate(baudrate);		serCfg.setDataBits(serCfg.LN_8BITS);		serCfg.setParity(serCfg.PY_NONE);		serCfg.setStopBits(serCfg.ST_1BITS);		serCfg.setHandshake(serCfg.HS_NONE);		serCfg.setHardFlow(false);		// Create the SerialPortLocal object		serPort = new SerialPortLocal(serCfg);		serPort.setTimeoutRx(100);		serPort.setTimeoutTx(5);		serPort.setDTR(true);		serPort.txFlush();		serPort.rxFlush();		}	/**	 * Initializes the connection with the Cye robot.	 * This consists of sending an ISTART message, and 	 * waiting for the ASTART acknowledgement back from 	 * the robot.	 *	 * @return the success (<code>true</code>) or failure 	 * of the initialization sequence.	 */	private boolean InitializeRobot()		{		// Send ISTART message		JCyeMsg InitMsg = new JCyeMsg(id);		InitMsg.SetRxMsgNum(0);		InitMsg.SetTxMsgNum(0);		InitMsg.Add(InitMsg.CMD_ISTART);		JCyeMsg ackMsg = TransmitUntilAck(InitMsg, TX_ATTEMPTS);		// Look for ASTART message, return false if no valid 		// ASTART response.		if(ackMsg.GetBufByte(2) == ackMsg.CMD_ASTART) return true;		return false;		}	/**	 * Sends a message (represented as an object of class 	 * <code>JCyeMsg</code>) to the Cye robot, and waits 	 * for an acknowledgement or retry failure.	 * 	 * @param Msg the <code>JCyeMsg</code> to send to the robot.	 *	 * @return the acknowledgement message from the robot, 	 *  represented as an object of type <code>JCyeMsg</code>.	 */	protected JCyeMsg SendMsg (JCyeMsg Msg)		{		JCyeMsg reply = TransmitUntilAck(Msg, TX_ATTEMPTS);		return reply;		}	/**	 * The background <code>run</code> method for 	 * <code>JCyeComm</code>.  This method polls the 	 * robot (with a <code>CMD_POLL_REQUEST</code> message,	 * then goes back to sleep for <code>SLEEP_TIME</code> ms.	 */	public void run()		{		while(true) 			{			// If we're idle, poll the robot			JCyeMsg PollMsg = new JCyeMsg(id);			PollMsg.Add(PollMsg.CMD_POLL_REQUEST);			// PollMsg.Add(PollMsg.CMD_REQUEST_STATE);			TransmitUntilAck(PollMsg, 1);    			// Go to sleep for SLEEP_TIME.			try 				{				Thread.sleep(SLEEP_TIME);				}			catch (InterruptedException e) 				{				System.err.println("Error in JCyeComm run.");				System.err.println(e);				}			}		}	/**	 * Transmit a <code>JCyeMsg</code> over the serial port, 	 * with no acknowledgement or retry.  First, call the 	 * <code>JCyeMsg</code>'s <code>toTransmitArray</code> method, 	 * which mashes all of the random message fields down 	 * into a byte array.  Then, spit that flattened byte 	 * array out the serial port.	 *	 * @param Msg the <code>JCyeMsg</code> to transmit.	 */	private void Transmit(JCyeMsg Msg)		{		JCyeMsg txMsg = Msg.toTransmitArray();		int l = txMsg.GetMsgLen() - 1;		try 			{			    if (connection == OLD_RADIO) {				serPort.setDTR(false);				try					{					Thread.sleep(RADIO_WARMUP_TIME);					}				catch (Exception e) {}			    }			    			    if (connection == NEW_RADIO) {				// Write 40 padding chars to sync radio				for(int i = 0; i < PADDING_CHARS; i++) {				    serPort.putByte(PADDING_CHARACTER);				}			    }							    serPort.putData(txMsg.GetMsgBuf(), 0, 				txMsg.GetMsgLen() - 1);			    while(serPort.txBufCount() != 0);			    if (connection == OLD_RADIO) {				try					{					Thread.sleep(TX_DRAIN_TIME);					}				catch (Exception e) {}				serPort.setDTR(true);			    }			}		catch (Exception e) 			{			/*--- Just log the error for now ---*/			System.err.println("Error in JCyeComm Transmit.");			System.err.println(e);			// System.exit(1);			}		}	/**	 * Transmit a <code>JCyeMsg</code> until we receive an 	 * acknowledge message, with a specified number of retry 	 * attempts if we're not immediately successful.	 * This method makes use of the methods <code>Transmit</code> 	 * and <code>Receive</code> to do the dirty work.	 *	 * @param Msg the <code>JCyeMsg</code> to transmit.	 * @param attempts the number of retry attempts permitted 	 * before we return failure.	 *	 * @return the acknowledgement <code>JCyeMsg</code> 	 *  received from the robot. In case of	 *  failure, MsgID will be set to -1.	 */	private synchronized JCyeMsg TransmitUntilAck(JCyeMsg Msg, int attempts)		{		Msg.SetMsgID(Msg.GetBufByte(0));		switch(Msg.GetMsgID()) 			{			case JCyeMsg.CMD_ISTART:    				SendMsgNum = 0;    				Msg.SetTxMsgNum(SendMsgNum);				break;			case JCyeMsg.CMD_POLL_REQUEST:				Msg.SetTxMsgNum(SendMsgNum);				break;			default:				SendMsgNum++;				SendMsgNum &= 0x0F;				Msg.SetTxMsgNum(SendMsgNum);				break;			}		JCyeMsg rxMsg = new JCyeMsg(id);		boolean gotAck = false;		boolean polling = false;		int txFailures = 0;		Msg.SetRxMsgNum(AckMsgNum);		if(DEBUG) 			{			System.out.println("Transmitting Message: ");			System.out.print(" Msg ID:" + Msg.GetMsgID());			if(Msg.GetMsgID() > 32) 				System.out.println(" (" 				+ (char)Msg.GetMsgID() + ")");