lmsutil.java

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

package cie.mobile.sick;

public class LMSUtil
{
    /** this mode start from angle 40 to 140, every 1 degree */
    public static final int MODE_101 = 1;

    /** this mode start from angle 40 to 140, every 0.5 degree */
    public static final int MODE_201 = 2;

    /** this mode start from angle 40 to 140, every 0.25 degree */
    public static final int MODE_401 = 3;

    /** this mode start from angle 0 to 180, every 1 degree */
    public static final int MODE_181 = 4;

    /** this mode start from angle 0 to 180, every 0.5 degree */
    public static final int MODE_361 = 5;

    private static int[] frag_size = {0, 101, 201, 401, 181, 361};
    private static int[] start_angle = {0, 40, 40, 40, 0, 0};
    private static int[] end_angle = {0, 140, 140, 140, 180, 180};

    /** command sending by PC to request if the sick is alive */
    private static byte[] statusCmd = { 0x02, 0x00, 0x01, 
	    0x00, 0x31, 0x15, 0x12 };

    /** command sending by PC to start transfering detected range data */
    private static byte[] startCmd = { 0x02, 0x00, 0x02, 
	    0x00, 0x20, 0x24, 0x34, 0x08 };

    /** command sending by PC to stop the transfer of detected range data */
    private static byte[] stopCmd = { 0x02, 0x00, 0x02, 0x00, 
	    0x20, 0x25, 0x35, 0x08 };

    /** command sending by PC to set the commnucation port baudrate to 9600 */
    private static byte[] baud9600Cmd = {0x02, 0x00, 0x02, 0x00, 
	    0x20, 0x42, 0x52, 0x08 };

    /** command sending by PC to set the commnucation port baudrate to 19200 */
    private static byte[] baud19200Cmd = {0x02, 0x00, 0x02, 0x00, 
	    0x20, 0x41, 0x51, 0x08 };

    /** command sending by PC to set the commnucation port baudrate to 38400 */
    private static byte[] baud38400Cmd = { 0x02, 0x00, 0x02, 0x00, 
	    0x20, 0x40, 0x50, 0x08 };

    /**
     * command sending by PC to set the resolution to MODE_101
     * 
     * @see MODE_101
     */
    private static byte[] reso100_1Cmd = { 0x02, 0x00, 0x05, 0x00, 
	    0x3B, 0x64, 0x00, 0x64, 0x00, 0x1D, 0x0F };

    /**
     * command sending by PC to set the resolution to MODE_201
     * 
     * @see MODE_201
     */
    private static byte[] reso100_05Cmd = { 0x02, 0x00, 0x05, 0x00, 
	    0x3B, 0x64, 0x00, 0x32, 0x00, (byte) 0xB1, 0x59 };

    /**
     * command sending by PC to set the resolution to MODE_401
     * 
     * @see MODE_401
     */
    private static byte[] reso100_025Cmd = { 0x02, 0x00, 0x05, 0x00, 
	    0x3B, 0x64, 0x00, 0x19, 0x00, (byte) 0xE7, 0x72 };

    /**
     * command sending by PC to set the resolution to MODE_181
     * 
     * @see MODE_181
     */
    private static byte[] reso180_1Cmd = { 0x02, 0x00, 0x05, 0x00, 
	    0x3B, (byte) 0xB4, 0x00, 0x64, 0x00, (byte) 0x97, 0x49 };

    /**
     * command sending by PC to set the resolution to MODE_361
     * 
     * @see MODE_361
     */
    private static byte[] reso180_05Cmd = { 0x02, 0x00, 0x05, 0x00, 
	    0x3B, (byte) 0xB4, 0x00, 0x32, 0x00, 0x3B, 0x1F };

    /**
     * Commands to set the resolution, in an array.
     */
    private static byte[][] resoCmd = {{}, reso100_1Cmd, reso100_05Cmd, 
	    reso100_025Cmd, reso180_1Cmd, reso180_05Cmd};
    
    /** command to start the measurement setting process */
    private static byte[] settingsModeCmd = { 0x02, 0x00, 0x0A, 
	    0x00, 0x20, 0x00, 0x53, 0x49, 0x43, 0x4B, 0x5F, 
	    0x4C, 0x4D, 0x53, (byte) 0xBE, (byte) 0xC5 };

    /** command send to change the range data unit to mm */
    private static byte[] mmModeCmd = { 0x02, 0x00, 0x21, 0x00, 
	    0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 
	    0x00, 0x00, 0x02, 0x02, 0x00, 0x00, 0x00, 0x00, 
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	    0x00, (byte) 0xEC, 0x7E };

    /** command sending to change the range data unit to cm */
    private static byte[] cmModeCmd = { 0x02, 0x00, 0x21, 0x00, 
	    0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	    0x00, 0x00, 0x02, 0x02, 0x00, 0x00, 0x00, 0x00, 
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	    0x00, (byte) 0xE8, 0x72 };

    /** this header from SICK means a new fragment of range data */
    private static int[] header_101 = {0x02, 0x80, 0xCE, 0x00, 
	    0xB0, 0x65, 0x40 };
    /** this header from SICK means a new fragment of range data */
    private static int[] header_201 = {0x02, 0x80, 0x96, 0x01, 
	    0xB0, 0xC9, 0x40};
    /** this header from SICK means a new fragment of range data */
    private static int[] header_401 = {0x02, 0x80, 0x26, 0x03, 
	    0xB0, 0x91, 0x41};
    /** this header from SICK means a new fragment of range data */
    private static int[] header_181 = {0x02, 0x80, 0x6E, 0x01, 
	    0xB0, 0xB5, 0x40};
    /** this header from SICK means a new fragment of range data */
    private static int[] header_361 = {0x02, 0x80, 0xD6, 0x02, 
	    0xB0, 0x69, 0x41};
	    
    /** these headers from SICK means a new fragment of range data */
    private static int[][] header = {{}, header_101, header_201, 
	    header_401, header_181, header_361};
    
    /**
     * Get the status command
     * 
     * @return a byte array to be sent from PC to SICK
     */
    public static byte[] getCmdStatus()
    {
	    return statusCmd;
    }

    /**
     * Get the start command which will leads to a flow of range data
     * comming from SICK to PC
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdStart()
    {
	    return startCmd;
    }
    
    /**
     * Get the stop command which will leads to the end of 
     * the flow of range data
     * comming from SICK to PC
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdStop()
    {
	    return stopCmd;
    }

    /**
     * Get the set baud to be 9600 command
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdBaud9600()
    {
	    return baud9600Cmd;
    }

    /**
     * Get the set baud to be 19200 command
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdBaud19200()
    {
	    return baud19200Cmd;
    }

    /**
     * Get the set baud to be 38400 command
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdBaud38400()
    {
	    return baud38400Cmd;
    }

    /**
     * Get the start seting mode command, use this command
     * before you want to set the data unit to be mm or cm
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdSetting()
    {
	    return settingsModeCmd;
    }

    /**
     * Get the set range data unit to be mm command
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdMM()
    {
	    return mmModeCmd;
    }

    /**
     * Get the set range data unit to be cm command
     * 
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdCM()
    {
	    return cmModeCmd;
    }

    /**
     * Get the set resolution command
     * 
     * @param mode int - the current SICK mode
     * @return a byte array to be send from PC to SICK
     */
    public static byte[] getCmdResolution(int mode)
    {
	    return resoCmd[mode];
    }

    /**
     * Get the range data fragment's size
     *
     * @param mode -  the current mode of SICK
     * @return int size of the fragment
     */
    public static int getFragSize(int mode)
    {
	    return frag_size[mode];
    }

    /**
     * Get the start angle of the range data
     * 
     * @param mode int - current mode of SICK
     * @return start angle, in degree
     */
    public static int getStartAngle(int mode)
    {
	    return start_angle[mode];
    }

    /**
     * Get the end angle of the range data
     * 
     * @param mode int - current mode of SICK
     * @return start angle, in degree
     */
    public static int getEndAngle(int mode)
    {
	    return end_angle[mode];
    }

    /**
     * An convenient mothod to refelct the string sick mode to
     * the integer form of sick mode
     *
     * @param smode the mode described in string
     * @return int the mode described by an integer
     */
    public static int parseMode(String smode)
    {
	    if("MODE_101".equals(smode)) return 1;
	    if("MODE_201".equals(smode)) return 2;
	    if("MODE_401".equals(smode)) return 3;
	    if("MODE_181".equals(smode)) return 4;
	    if("MODE_361".equals(smode)) return 5;
	    return 0;
    }

    /**
     * get the header of returned range data
     * 
     * @param mode int the current mode of the SICK
     * @return an integer array to match the fragment
     */
    public static int[] getHeader(int mode)
    {
	    return header[mode];
    }

    private LMSUtil()
    {
    }
}