feelforce.java

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

package cie.mobile;

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

public class FeelForce implements Runnable, Module
{
	String module_name;
	ModuleDoc doc;
	String[] port_names = {"map", "force"};
	SimpleInPort map_port;
	SimpleOutPort force_port;
	Thread t_this;
	boolean bRun =  false;
	int threashold_far;
	int threashold_near;
	double[] force = new double[2];
	
	public FeelForce()
	{
	}

	public String getName()
	{
		return module_name;
	}
	
	public Port getPort(String name)
	{
		if(port_names[0].equals(name))
			return map_port;
		if(port_names[1].equals(name))
			return force_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("threashold_far");
		if(s == null)
		{
			String ss = "Module " + module_name + 
				" need the parameter threashold_far.";
			throw new IllegalXMLFormatException(ss);
		}
		threashold_far = Integer.parseInt(s);
		s = doc.getParamValue("threashold_near");
		if(s == null)
		{
			String ss = "Module " + module_name + 
				" need the parameter threashold_near.";
			throw new IllegalXMLFormatException(ss);
		}
		threashold_near = Integer.parseInt(s);
		map_port = new SimpleInPort(this, port_names[0]);
		force_port = new SimpleOutPort(this, port_names[1]);
	}
	
	public void start() throws Exception
	{
		t_this = new Thread(this);
		bRun = true;
		t_this.start();
	}
	
	public void stop() throws Exception
	{
		bRun = false;
		int i = 0;
		while(t_this.isAlive() && i<5)
		{
			try{
				Thread.sleep(50);
			}catch(InterruptedException e)
			{}
			i++;
		}

		if(t_this.isAlive())
		{
			throw new Exception("Cannot stop the module " + module_name);
		}
	}
	public void destroy() throws Exception
	{
	}
	
	public int getLayer()
	{
		return doc.getLayer();
	}

	public void run()
	{
		int[] buf = null;
		double[] angle = null;
		while(bRun)
		{
			RangeData data = (RangeData) map_port.getValue();
			if(data == null)
			{
				try{
					Thread.sleep(50);
				}catch(InterruptedException e){}
				continue;
			}
			if(buf == null)
			{
				int c = data.getLength();
				buf = new int[c];
				angle = new double[c];
				int sa = data.getStartAngle();
				int ea = data.getEndAngle();
				double reso = (double) (ea-sa)/(c-1);
				for(int i=0; i<c; i++)
				{
					angle[i] = sa + i * reso;
				}
			}
			if(data.getData(buf) != null)
			{
				calForce(buf, angle);
				force_port.setValue(this, force, 1000);
				System.out.println("fx=" + force[0] + ";fy=" + force[1]);
			}
			try{
				Thread.sleep(50);
			}catch(InterruptedException e){}
		}
	}

	private void calForce(int[] buf, double[] angle)
	{
		double sigma_f_x = 0, sigma_f_y = 0;
		double f;
		double d2r = Math.PI / 180.0;
		
		for(int i=0; i<buf.length; i++)
		{
			//classify the data
			if(buf[i] > threashold_far)
			{
				//neglect
			}
			else 
			{
				//calculate
				double ratio = (double) buf[i] / threashold_near;
				f = 1.0/Math.pow(ratio, 4);
				double ang = angle[i]*d2r;
				sigma_f_x += (-1.0)*f* Math.cos(ang);
				sigma_f_y += f * Math.sin(ang);
			}
		}
		force[0] = sigma_f_x;
		force[1] = -1.0*sigma_f_y;
	}
}