jcyecomm.java

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

			else
				System.out.println();
			System.out.println(" RX/TX Msg Num: " 
			+ Msg.GetRxMsgNum() + "/" + Msg.GetTxMsgNum());
			}

		while ((!polling) && (!gotAck) && (txFailures < attempts)) 
			{
    			if(DEBUG) 
				{
				System.out.print("  Attempt number " 
					+ (txFailures + 1) 
					+ " to transmit message with ID " 
					+ Msg.GetMsgID());
				System.out.println(" and RX/TX " 
					+ Msg.GetRxMsgNum() + "/" 
					+ Msg.GetTxMsgNum());
				System.out.println("  Transmit packet begin timestamp(ms): " 
					+ System.currentTimeMillis());
				}

			// Transmit the damn Msg.
			Transmit(Msg);

			if(DEBUG) 
				{
				System.out.println("  Transmit packet end timestamp(ms): " 
					+ System.currentTimeMillis());
				System.out.println("  Message has been sent. Waiting for ack.");
				}

			rxMsg = Receive();
	    
			if(DEBUG) 
				{
				System.out.print("  Received message with ID " 
					+ rxMsg.GetMsgID());
				if(rxMsg.GetMsgID() > 32)
					System.out.println(" (" 
					+ (char)rxMsg.GetMsgID() + ")");
				else
					System.out.println();
				System.out.println("  Receive packet end timestamp(ms): " 
					+ System.currentTimeMillis());
				System.out.println("   Robot RX/TX Msg Num: " 
					+ rxMsg.GetRxMsgNum() + "/" 
					+ rxMsg.GetTxMsgNum());
				System.out.println("   Robot MsgLen: " 
					+ rxMsg.GetMsgLen());
				}

			AckMsgNum = rxMsg.GetTxMsgNum();
			Msg.SetRxMsgNum(AckMsgNum);
			if(DEBUG) 
				{
				System.out.println("   Updated AckMsgNum to " 
					+ AckMsgNum);
				}

			if(Msg.GetTxMsgNum() == rxMsg.GetRxMsgNum()) 
				{
				if(DEBUG) 
					{
					System.out.println("   Got ack match.");
					}

				gotAck = true;
				} 
			else if(Msg.GetBufByte(0) == Msg.CMD_POLL_REQUEST) 
				{
				if(DEBUG) 
					{
					System.out.println("   Bad ack match, but we're just polling.");
					}
				polling = true;
				}
			if(gotAck == false) 
				{
				if(DEBUG)
					{
					System.out.println("  Did not receive valid ack message.");
					}
				txFailures++;
				}
			}

		if(gotAck == true) 
			{
			if(DEBUG) 
				{
				System.out.println("  Receieved a valid ack message.");
				}
			HandleAckMsg(rxMsg);
			}

		if(DEBUG) 
			{
			System.out.println("Done with transmit.\n\n");
			}

		return rxMsg;
		}

	/**
 	 * Receive a message from the Cye robot.
	 * The message must be bracketed by a STX/ETX pair.
	 * After we have received a valid message, the packet 
	 * headers are parsed using <code>JCyeMsg</code>'s 
	 * <code>ParseReply</code> method.
	 *
	 * @return the <code>JCyeMsg</code> received from 
	 *  the robot. If there is a reception
	 *  error, MsgID is set to -1.
	 */
	private JCyeMsg Receive()
		{
		JCyeMsg Msg = new JCyeMsg(id);

		int i = 0;
		int bufCount = 0;
		int STXwaitcount = 0;

		if(DEBUG)
			{
			System.out.println("     Receive packet begin timestamp(ms): " 
			+ System.currentTimeMillis());
			System.out.print("     Received chars: ");
			}

		try 
			{
			while((i != 2) && (i != -1) && (STXwaitcount++ < 32)) 
				{
				i = serPort.getByte();
				if(DEBUG) 
					{
					if(i == 2) 
						{
						System.out.println();
						System.out.print("     RX data packet: < ");
						}
					System.out.print(i + " ");
					}
				}

			if(STXwaitcount >= 32) 
				{
				if(DEBUG) 
					{
					System.out.println("Garbage on rx.");
					}
				Msg.SetMsgID(-1);
				return Msg;
				}

			// Check for timeout on STX character
			if(i == -1)
				{
				if(DEBUG) 
					{
					System.out.println("Timeout waiting for STX.");
					}
				Msg.SetMsgID(-1);
				return Msg;
				}

			while((i != 3) && (i != -1) && (bufCount++ < 32)) 
				{
				i = serPort.getByte();
				if(DEBUG) 
					{
					System.out.print(i + " ");
					if(i == 3) System.out.print(">");
					}

				if(i == Msg.ESC) 
					{
					i = serPort.getByte();
					if(DEBUG) 
						{
						System.out.print(i + " ");
						}
					switch(i) 
						{
						case(Msg.ESC_STX):
							Msg.Add(Msg.STX);
							break;
						case(Msg.ESC_ETX):
							Msg.Add(Msg.ETX);
							break;
						case(Msg.ESC):
							Msg.Add(Msg.ESC);
							break;
						}
					}
				else Msg.Add((byte)i);
				}

			if(bufCount == 32) 
				{
				if(DEBUG) 
					{
					System.out.println("RX buffer overflow waiting for ETX.");
					}
				Msg.SetMsgID(-1);
				return Msg;
				}

			// Check for timeout on ETX character
			if(i == -1) 
				{
				if(DEBUG) 	
					{
					System.out.println("Timeout waiting for ETX.");
					}
				Msg.SetMsgID(-1);
				return Msg;
				}
			}

		catch (Exception ioe) 
			{
			System.err.println(ioe);
			System.exit(1);
			}

		Msg.ParseReply();

		if(DEBUG) 
			{
			System.out.println();
			System.out.println("     RX packet timestamp(ms): " 
				+ System.currentTimeMillis());
			}

		return Msg;
		}

	/**
	 * Decode a <code>JCyeMsg</code> reply from the robot.
	 * This method parses the data encoded in the reply, 
	 * and sets the various and sundry fields in <code>JCyeComm</code>.
	 * It is assumed that the message is already known to be 
	 * valid; passing an invalid message to this method can 
	 * seriously hose the status variables.
	 *
	 * @param Msg the <code>JCyeMsg</code> to be decoded.
	 */
	private void HandleAckMsg(JCyeMsg Msg)
		{
		switch(Msg.GetMsgID())
			{
			case Msg.REPLY_STATUS_UPDATE:
				JCyeStatus stat = new JCyeStatus(Msg);
				lastX = stat.GetX();
				lastY = stat.GetY();
				lastH = stat.GetHeading();
				lastB = stat.GetBattery();
				if(DEBUG)
					{
					System.out.println("   Received status update message.");
					System.out.println("    Robot X: " + lastX);
					System.out.println("    Robot Y: " + lastY);
					System.out.println("    Heading: " + lastH);
					System.out.println("    Battery: " + lastB);
					}
				break;
			case Msg.REPLY_OBSTACLE:
				if(DEBUG)
					{
					System.out.println("   Received obstacle update message.");
					}
				sawObstacle = true;
				break;
			case Msg.REPLY_CHARGE:
				if(DEBUG) 
					{	   
					System.out.println("   Received charge state update message.");
					}
				break;
			case Msg.REPLY_SOFTWARE_REV:
				if(DEBUG) 
					{
					System.out.println("   Received software revision number message.");
					}
				break;
			case Msg.REPLY_AT_HOME:
				if(DEBUG) 
					{
					System.out.println("   Received robot at home message.");
					}
				break;
			default:
				if(DEBUG) 
					{
					System.out.println("   Received unknown ack message.");
					}
				break;
			}
		}


	/**
	 * Returns the last reported X position of el roboto.
	 *
	 * @return the X position
	 */
	protected int GetLastX()
		{
		return lastX;
		}

	/**
	 * Returns the last reported Y position of el roboto.
	 *
	 * @return the Y position
	 */
	protected int GetLastY()
		{
		return lastY;
		}

	/**
	 * Returns the last reported heading of el roboto.
	 *
	 * @return the heading
	 */
	protected int GetLastH()
		{
		return lastH;
		}

	/**
	 * Returns the last reported battery voltage of el roboto.
	 *
	 * @return the battery voltage
	 */
	protected double GetLastB()
		{
		return lastB;
		}

	/**
	 * Returns the status of the obstacle detector. If an obstacle 
	 * has been hit since the last call to <code>ClearObstacle</code>, 
	 * return true.
	 *
	 * @return the obstacle detector status.
	 */
	protected boolean GetObstacle()
		{
		return sawObstacle;
		}

	/**
	 * Resets the obstacle detector. Call this method to reset 
	 * the detection logic.
	 */
	protected void ClearObstacle()
		{
		sawObstacle = false;
		}

	/**
	 * Convert a (signed) byte [-127..128] to an (unsigned) 
	 * integer [0..255].
	 *
	 * @param b the (signed) byte
	 *
	 * @return the (unsigned) integer
	 */
	private int bToI(byte b)
		{
		if(b < 0) return ((int)b + 256);

		return ((int)b);
		}
	}