collidemodule.java

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

package cie.mobile;

import name.lxm.robot.arch.*;
import cie.mobile.sick.RangeData;
import cie.mobile.sick.SickDataException;

/**
 * This module is used to detect if the robot has the
 * potential risk of colliding with other object ahead,
 * if there is any, the robot will stop immediately util
 * the object in the way is removed.
 *
 * @author Richard Lee
 * @version 1.0
 */
public class CollideModule implements Runnable, Module
{
	private String module_name;
	private String[] port_names = {"map", "halt"};
	private SimpleInPort map_port = null;
	private SimpleOutPort halt_port = null;
	private ModuleDoc doc;
	private int layer;
	private int threadhold;
	private Thread t_this;
	private boolean bRun = false;
	private double[] valve = new double[181];
	private double R = 300.0; //Radius of robot is 300mm
	
	public CollideModule()
	{
	}
	
	public String getName()
	{
		return module_name;
	}
	
	public Port getPort(String name)
	{
		if(port_names[0].equals(name))
			return map_port;
		else if(port_names[1].equals(name))
			return halt_port;
		return null;
	}
	
	public ModuleDoc getModuleDoc()
	{
		return doc;
	}
	
	public void init(ModuleDoc cfg) throws Exception
	{
		doc = cfg;
		module_name = doc.getName();
		String s = doc.getParamValue("threadhold");
		if( s == null) throw new IllegalXMLFormatException("Module " + module_name + "needs a parameter - threadhold." );
		threadhold = Integer.parseInt(s);
		map_port = new SimpleInPort(this, port_names[0]);
		halt_port = new SimpleOutPort(this, port_names[1]);
		//initial threashold
		for(int i=0; i<=180; i++)
		{
			if(i<90)
			{
				valve[i] = (-1)*R/90.0*i+R+threadhold;
			}else
			{
				valve[i] = R/90.0*i+threadhold-R;
			}
		}
	}
	
	public void start() throws Exception
	{
		t_this = new Thread(this);
		bRun = true;
		t_this.start();
	}
	
	public void stop() throws Exception
	{
		bRun = false;
		int c = 0;
		while(t_this.isAlive())
		{
			try{
				Thread.sleep(50);
			}catch(InterruptedException e)
			{
			}
			c++;
			if(c > 5)
				break;
		}
		t_this = null;
	}
	
	public void destroy() throws Exception
	{
	}
	
	public int getLayer()
	{
		return doc.getLayer();
	}

	public void run()
	{
		RangeData data;
		int[] buf = null;
		while(bRun)
		{
			data = (RangeData) map_port.getValue();
			if(data == null)
			{
				try{
					Thread.sleep(50);
				}catch(InterruptedException e)
				{}
				continue;
			}
				
			if(buf == null)
			{
				buf = new int[data.getLength()];
			}
			if(data.getData(buf) == null)
			{
				System.out.println("WARNING: Buffer size is too small!");
				try{
					Thread.sleep(1000);
				}catch(InterruptedException e){}
				continue;
			}
			int r = checkMapData(buf);
			if(r==1)
			{
				halt_port.setValue(this, new Integer(r), 200);
				System.out.print("#");
			}
			try{
				Thread.sleep(100);
			}catch(InterruptedException e){}
		}
	}

	private int checkMapData(int[] data)
	{
		int c = 0;
		int i;
		for(i=0; i<data.length; i++)
		{
			if(data[i] < valve[i])
				c++;
		}
		double r = (double) c / (double) data.length;
		if(r > 0.1)
			return 1;
		return 0;
	}
}