lms200.java

j2me实现的移动机器人代码（Java实现）

package cie.mobile.sick;

import name.lxm.comm.*;
import java.io.*;

public class LMS200
{
	public static final int STATE_READY = 0;
	public static final int STATE_WORKING = 1;
	public static final int STATE_MESS = 2;
	
	private String com;
	private int mode;
	private int baudrate;
	private RangeData rangeData = null;
	private CommSerialPort commPort;
	private byte[] buf = new byte[1024];
	private int[] sickdata = null;
	private int[] header = null;
	private int state = STATE_MESS;
	
	public LMS200(String com, int mode, int baudrate)
		throws IOException
	{
		this.com = com;
		this.mode = mode;
		this.baudrate = baudrate;
		init();
		System.out.println("LMS200 is ready now.");
	}

	/**
	 * This method is called to clear all the buffered sick message
	 * <i>Note: Do not use it in cycles</i>
	 * @return boolean if there's any data in the serial buffer
	 * @throws IOException
	 */
	private boolean clearSerialData() throws IOException 
	{
		int round = 0; 
		int size = 0;
		boolean b = false;
		while (true) {
			try {
				Thread.sleep(100);
			} catch (InterruptedException e) {
			}
			if ((size = commPort.readBytes(buf)) > 0) {
				b = true;
				round = 0;
			} else {
				round++;
			}
			if (round > 3)
				//I believe there's no more data left
				break;
		}
		return b;
	}

	
	private void init() throws IOException
	{
		//open serial port for communication
		try{
			commPort = CommPortFactory.getSerialPort(com, "LMS200");
			commPort.open();
		}catch(Exception e)
		{
			throw new IOException(e.toString());
		}
		
		//adjusting the baudrate
		if(baudrate != 9600)
		{
			//TODO...
			//till now, using the default one
		}
		//send stop command
		commPort.writeBytes(LMSUtil.getCmdStop());
		if(!clearSerialData())
			throw new IOException("No response from LMS.");
		//send resolution command
		commPort.writeBytes(LMSUtil.getCmdResolution(mode));
		//clear serial port
		if(!clearSerialData())
			throw new IOException("No response from LMS.");
		//initialize data buffer
		sickdata = new int[LMSUtil.getFragSize(mode)];
		//set header
		header = LMSUtil.getHeader(mode);
		//initialize the range data
		rangeData = new RangeData(LMSUtil.getFragSize(mode), 
				LMSUtil.getStartAngle(mode), 
				LMSUtil.getEndAngle(mode));
		//set state
		state = STATE_READY;
	}

	public LMS200(String com, String mode, String baudrate)
		throws IOException
	{
		this(com, LMSUtil.parseMode(mode), Integer.parseInt(baudrate));
	}

	/**
	 * <p>Extract a complete data frame from the range data stream</p>
	 * <i>Note: this is a blocking method</i>
	 * 
	 * @return the fragment containing a full range scan data
	 * @throws IOException
	 */
	public RangeData getDataFrame() throws IOException
	{
		int p = 0;
		int total = LMSUtil.getFragSize(mode);
		if(state != STATE_WORKING) return null;

		pass2Header();
		//start saving this frame
		while (p < total) {
			int c1 = commPort.readUnsignedByte();
			int c2 = commPort.readUnsignedByte();
			sickdata[p] = c2 * 256 + c1;
			p++;
		}
		rangeData.setData(sickdata);
		return rangeData;
	}

	/**
	 * looking for the begining of range data in the data stream
	 * 
	 * @throws IOException
	 */
	private void pass2Header() throws IOException 
	{
		int c;
		byte p = 0;
		int total = 0;
		while (true) {
    			c = commPort.readUnsignedByte();
			total++;
			if (c == header[p]) {
				p++;
			} else {
				p = 0;
			}
			if (p == 7) {
				break;
	    		}
        	}
	}

	public boolean isAlive()
	{
		return false;
	}

	/**
	 * start the LMS. After startup, a continuouse
	 * data flow containing the frame fragment whould
	 * be send from LMS to PC
	 *
	 * @throws SICKException, IOException 
	 */
	public void start() throws SICKException, IOException
	{
		if(state == STATE_READY)
		{
			commPort.writeBytes(LMSUtil.getCmdStart());
			state = STATE_WORKING;
			System.out.println("LMS is working now.");
		}
		else
		{
			//has not been initialized, although it is rare
			throw new SICKException("SICK is not ready!");
		}
	}

	/**
	 * make the sick stop working, ready for other commands
	 * 
	 * @throws SICKException, IOException
	 */
	public void stop() throws SICKException, IOException
	{
		if(state == STATE_WORKING)
		{
			commPort.writeBytes(LMSUtil.getCmdStop());
			clearSerialData();
			state = STATE_READY;
			System.out.println("LMS is idle now.");
		}
		else
		{
			throw new SICKException("SICK is not running.");
		}
	}
}