motormodule.java

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

package cie.mobile.pmac;

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

public class MotorModule implements Module, Runnable
{
	private String module_name;
	private static String PORTNAME_CMD = "command";
	private static String PORTNAME_HALT = "halt";
	private static String PORTNAME_MOTOR = "motor";
	
	private SimpleInPort portCmd = null;
	private SimpleInPort portHalt = null;
	private SimpleOutPort portMotor = null;
	private ModuleDoc md = null;
	private String comName = null;
	private Thread t_this = null;
	private boolean bRun = false;
	private boolean bHalt = false;
	private Pmac pMac = null;

	private double scale_move = 1.0;
	private double scale_turn = 1.0;

	public MotorModule()
	{
		portCmd = new SimpleInPort(this, PORTNAME_CMD);
		portHalt =  new SimpleInPort(this, PORTNAME_HALT);
		portMotor = new SimpleOutPort(this, PORTNAME_MOTOR);
	}
	
	public String getName()
	{
		return module_name;
	}
	
	public Port getPort(String name)
	{
		if(PORTNAME_CMD.equals(name))
			return portCmd;
		if(PORTNAME_HALT.equals(name))
			return portHalt;
		if(PORTNAME_MOTOR.equals(name))
			return portMotor;
		return null;
	}
	
	public ModuleDoc getModuleDoc()
	{
		return md;
	}
	
	public void init(ModuleDoc cfg) throws Exception
	{
		this.md = cfg;
		comName = cfg.getParamValue("com");
		module_name = cfg.getName();
		pMac = Pmac.getTheMobilePmac();
		pMac.init(comName);
		String sm = cfg.getParamValue("scale_move");
		String sr = cfg.getParamValue("scale_turn");
		if(sm == null || sr == null)
			throw new IllegalXMLFormatException("Parameter scale_move and scale_turn is required.");
		scale_move = Double.parseDouble(sm);
		scale_turn = Double.parseDouble(sr);
	}
	
	public void start() throws Exception
	{
		bRun = true;
		t_this = new Thread(this);
		t_this.start();
	}
	
	public void stop() throws Exception
	{
		bRun = false;
	}
	
	public void destroy() throws Exception
	{
	}
	
	public void run()
	{
		String cmd;
		while(bRun)
		{
			//get currrent position
			try{
				//get the angle
				//double p1 = pMac.getAxisPosition(1);
				//p1 = p1 / scale_turn;
				//get the moving distance
				//double p2 = pMac.getAxisPosition(2);
				//p2 = p2 / scale_move;
				//portMotor.setValue(this, Double.toString(p2)+":"+Double.toString(p1), 1000);
				//check halt port
				if(portHalt.getValue2() != null)
				{
					if(!bHalt)
					{
						//get halt command
						pMac.sendCommandLine("#2j/#1j/");
						bHalt = true;
					}
				}else if((cmd = (String) portCmd.getValue()) != null)
				{
					//get other command
					System.out.println("Get: " + cmd);
					processCmd(cmd);
				}
				try{
					Thread.sleep(100);
				}catch(InterruptedException e)
				{}
			}catch(Exception e)
			{
				e.printStackTrace();
			}
		}
	}

	public int getLayer()
	{
		return md.getLayer();
	}

	private void processCmd(String cmd) throws IOException, CommPortTimedOutException
	{
		String[] args = cmd.split(" ");
		int c = args.length;
		if("FORWARD".equals(args[0]))
		{
			if(c == 1)
				forward();
			if(c >= 2)
				forward(Double.parseDouble(args[1]));
			return;
		}
		if("BACKWARD".equals(args[0]))
		{
			if(c == 1)
				backward();
			if(c >= 2)
				backward(Double.parseDouble(args[1]));
			return;
		}
		if("TURNRIGHT".equals(args[0]))
		{
			if(c == 1)
				turn(1);
			if(c >= 2)
				turn(Math.abs(Double.parseDouble(args[1])));
			return;
		}
		if("TURNLEFT".equals(args[0]))
		{
			if(c == 1)
				turn(-1);
			if(c >= 2)
				turn(-1*Math.abs(Double.parseDouble(args[1])));
			return;
		}
		if("TURN".equals(args[0]))
		{
			if(c >= 2)
			{
				double d = Double.parseDouble(args[1]);
				turn(d);
			}
		}
		
		if("HALT".equals(args[0]))
		{
			haltMobile();
			return;
		}

		if("OFORWARD".equals(args[0]))
		{
			if(c >= 2)
			{
				int sp = Integer.parseInt(args[1]);
				ol_move(sp);
			}
			else
				ol_move(20);
			return;
		}

		if("ONLYTURN".equals(args[0]))
		{
			if(c>=2)
			{
				double d = Double.parseDouble(args[1]);
				onlyTurn(d);
			}
			return;
		}
	}

	private void forward() throws IOException, CommPortTimedOutException
	{
		pMac.sendCommandLine("#2j+#1j/");
	}

	private void forward(double v) throws IOException, CommPortTimedOutException
	{
		long p = (long) (v * scale_move);
		pMac.sendCommandLine("#1j/#2j:"+Long.toString(p));	
	}
	
	private void backward() throws IOException, CommPortTimedOutException
	{
		pMac.sendCommandLine("#1j/#2j-");
	}

	private void backward(double v) throws IOException, CommPortTimedOutException
	{
		long p = (long) (v * scale_move);
		pMac.sendCommandLine("#1j/#2j:-"+Long.toString(p));
	}

	public void turn(int i) throws IOException, CommPortTimedOutException
	{
		if(i>0)
			pMac.sendCommandLine("#1j+");
		else
			pMac.sendCommandLine("#1j-");
	}

	public void onlyTurn(double rela_p) throws IOException, CommPortTimedOutException
	{
		long p = (long) (rela_p * scale_turn);
		pMac.sendCommandLine("#1j:"+Long.toString(p)+"#2j/");
	}
	
	public void turn(double rela_p) throws IOException, CommPortTimedOutException
	{
		long p = (long) (rela_p * scale_turn);
		pMac.sendCommandLine("#1j:"+Long.toString(p));
	}

	public void haltMobile() throws IOException, CommPortTimedOutException
	{
		pMac.sendCommandLine("#1j/#2j/");
	}

	private void ol_move(int rate) throws IOException, CommPortTimedOutException
	{
		pMac.sendCommandLine("#2O"+Integer.toString(rate));
	}
}