selfpos.java

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

package cie.mobile;

import name.lxm.robot.arch.*;

/**
 * This class is used by the robot to locate itself
 * using the motor encoder outputs
 * The intial positon when system start is (0, 0)
 * and the heading is 90 degree.
 */
public class SelfPos extends AbstractModule
{
	String[] ports_name = {"motor", "pos", "heading"};
	SimpleInPort motor_port = null;
	SimpleOutPort pos_port = null;
	SimpleOutPort heading_port = null;

	double cx=0.0, cy=0.0;
	double heading = Math.PI/2.0;
	double total_length = 0;
	
	public Port getPort(String name)
	{
		if(ports_name[0].equals(name))
			return motor_port;
		if(ports_name[1].equals(name))
			return pos_port;
		if(ports_name[2].equals(name))
			return heading_port;
		return null;
	}

	public void init(ModuleDoc md) throws Exception
	{
		super.init(md);
		motor_port = new SimpleInPort(this, ports_name[0]);
		pos_port = new SimpleOutPort(this, ports_name[1]);
		heading_port = new SimpleOutPort(this, ports_name[2]);
		motor_port.registerListener(this);
	}

	public void run()
	{
	}

	public void valueUpdated()
	{
		//recieved a new motor status
		String motor_status = (String) motor_port.getValue();
		String[] sp = motor_status.split(":");
		double p1, p2;
		p1 = Double.parseDouble(sp[0]);
		p2 = Double.parseDouble(sp[1]);
		//calculate the current position
		calNewPos(p1, p2);
	}

	/**
	 * @param p1 - the distance encoder, in cm
	 * @param p2 - the angular encoder, in rad
	 */
	private void calNewPos(double p1, double p2)
	{
		//calculate the current positon in the local coordinate system
		cx = cx + (p1 - total_length)*Math.cos(heading);
		cy = cy + (p1 - total_length)*Math.sin(heading);
		//change heading first
		heading = p2 + Math.PI/2.0; 
		//change the total_length
		total_length = p1;
		//set value
		heading_port.setValue(this, new Double(heading), 1000);
		System.out.println("debug->Heading=" + heading);
		pos_port.setValue(this, Double.toString(cx)+":"+Double.toString(cy), 1000);
		System.out.println("Debug->Cx:Cy=" + Double.toString(cx)+":"+Double.toString(cy));
	}
}