/*
 * WheelCommander Test
 *
 * Copyright 2005 by Noetic Design, Inc.
 * www.nubotics.com
 *
 * Noetic Design grants you the right to use, modify, make derivative works and
 * redistribute this source file provided you do not remove this copyright notice.
 */

import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.io.Display;

/**
 * This class provides an example of the Nubotics WheelCommander
 * differential drive controller attached to the IntelliBrain.
 */
public class WheelCommander {
	int slave_address;

	byte[] buildCmd(char cmd)
	{
		byte[] ret = new byte[1];
		ret[0] = (byte)cmd;
		return ret;
	}
	byte[] buildByteCmd(char cmd, byte param)
	{
		byte[] ret = new byte[2];
		ret[0] = (byte)cmd;
		ret[1] = param;
		return ret;
	}
	byte[] buildWordCmd(char cmd, short param)
	{
		byte[] ret = new byte[3];
        ret[0] = (byte)cmd;
		ret[1] = (byte)(param & 0x0ff);
		ret[2] = (byte)((param >> 8) & 0x0ff);
		return ret;
	}
	byte[] buildDWordCmd(char cmd, int param)
	{
		byte[] ret = new byte[5];
        ret[0] = (byte)cmd;
		ret[1] = (byte)(param & 0x0ff);
		ret[2] = (byte)((param >> 8) & 0x0ff);
		ret[3] = (byte)((param >> 16) & 0x0ff);
		ret[4] = (byte)((param >> 24) & 0x0ff);
		return ret;
	}		
	byte extractByteParam(char cmd, byte[] buf) throws Exception
	{
		if ((byte)cmd == buf[0])
			return buf[1];
        else
			throw new Exception("bad command return");
	}
	short extractWordParam(char cmd, byte[] buf) throws Exception
	{
		if ((byte)cmd == buf[0])
		{
			return buf[1] + (short)buf[2] << 8;
		}
		else
			throw new Exception("bad command return");
	}		
	int extractDWordParam(char cmd, byte[] buf) throws Exception
	{
		if ((byte)cmd == buf[0])
		{
			return buf[1] + (int)buf[2] << 8 + (int)buf[3] << 16 + (int)buf[4] << 24;
		}
		else
			throw new Exception("bad command return");
	}		

	WheelCommander()
	{
		System.out.println("Starting WheelCommander");
		slave_address = (byte)0x20;
	}

    public static void main(String args[]) {
        try {
			System.out.println("Connecting to  WheelCommander at address 0x20");
            Display display = IntelliBrain.getLcdDisplay();	// Get the LCD display
            display.print(0, "");							// Clear first line
            display.print(1, "");							// Clear second line

            // Loop forever.
            while (true) {
//				display.print(0, "Light:  " + rangeFinder.readLightSensor());
                Thread.sleep(100);
//				float distance = rangeFinder.getDistanceInches();	// Read the distance measured by the range finder
//              if (distance < 0.0f)
//                  display.print(1, "Range:  ---");				// < 0.0 indicates no data, check connections
//              else
//                  display.print(1, "Range:  " +  (int)(distance + 0.5) + '"'); // display the distance
				Thread.sleep(500);
            }
        } catch (Throwable t) {
            t.printStackTrace();							// Display a stack trace for any error
        }
    }
}