Example Source Code:
RoboJDE Java Test Program
For the WheelCommander WC-132


Description

This example shows a layered approach to controlling a WheelCommander. At the lowest level, the Intellibrain I2CMaster class is used to send and receive byte arrays. These byte arrays in turn are filled and emptied with methods such as buildByteCmd(), buildWordCmd(), extractByteParam(), and extractWordParam(). At the next level, methods such as doSetWordCmd() and doGetWordCmd() actually perform the command specified in the cmdChar parameter. These methods are then used by the next level, the specific command handlers supported by the WheelCommander. These include commands such as doSync(), doVelocity(), getVelocity(), and so forth.

Finally, the main routine uses these to establish and confirm communication with the WheelCommander, demonstrate a ramping of velocity, then drive the robot in a square.

TODO: refactor WheelCommander.java to use the methods in WC.java, and remove the local copies of its member functions.
 

Download

WC.java - Java source
WheelCommander.java - Java source
WheelCommander.rjp - RoboJDE project file

 

Source Code


WC.java


/*
 * 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
        }
    }
}

WheelCommander.java


/*
 * 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;
import com.ridgesoft.io.I2CMaster;

/**
 * This class provides an example of the Nubotics WheelCommander
 * differential drive controller attached to the IntelliBrain.
 */
public class WCtest {
I2CMaster i2c;
int slave_address;
Display display;
final static float DVolt = 14.0;
final static float SVolt = 14.0;
final static float VDivTol = 0.2;
final static float ADVolt0 = 1.25;
final static float ADVolt1 = 2.5;
final static float ADVolt2 = 3.75;
final static float ADVolt3 = 5.0;
final static float ADVoltTol = 0.25;
int num_failures = 0;

/*
 * WheelCommander constructor
 */

WCtest()
{
        display = IntelliBrain.getLcdDisplay(); // Get the LCD display
        display.print(0, "WheelCommander Test"); // Clear first line
        display.print(1, ""); // Clear second line
       Log("Connecting to WheelCommander at address 0x20");
slave_address = (byte)0x20;
        i2c = IntelliBrain.getI2CMaster();
try
{
i2c.setFrequency(400000);
Log("I2C Frequency set to 400KHz");
}
catch (Exception e)
{
Log("Error setting I2C frequency; defaulting to 100KHz");
}
}

void Log(String string)
{
System.out.println(string);
        display.print(1, string);
    }

/*
 * Support Methods for Packing and Unpacking buffers
 */
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;
}

String dumpbytes(byte[] bytes)
{
String ret = "";
for (int i = 0; i < bytes.length; i++)
ret += Integer.toHexString(bytes[i] & 0x0ff) + "h ";
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);
//Log("dword cmd: " + dumpbytes(ret));
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])
{
short ret = (short)(((int)buf[1] & 0x0ff) + (((int)buf[2] << 8) & 0x00ff00));
//Log("buf[1] = " + buf[1] + "; buf[2] = " + buf[2] + "; short = " + ret);
return ret;
}
else
throw new Exception("bad command return");
}

short extractWordXParam(char cmd, byte val, byte[] buf) throws Exception
{
if (((byte)cmd == buf[0]) && (val == buf[1]))
{
return (short)(((int)buf[2] & 0x0ff) + (((int)buf[3] << 8) & 0x00ff00));
}
else
{
Log("extractWordXParam: missmatch; cmd = " + cmd + ", buf[0] = " + (char)buf[0] + "; val = " + val + ", buf[1] = " + buf[1] );
throw new Exception("bad command return");
}
}

int extractDWordParam(char cmd, byte[] buf) throws Exception
{
if ((byte)cmd == buf[0])
{
return ((int)buf[1] & 0x0ff) + (((int)buf[2] << 8) & 0x00ff00) + (((int)buf[3] << 16) & 0x0ff0000) + (((int)buf[4] << 24) & 0xff000000);
}
else
throw new Exception("bad command return");
}

/*
 * Support Methods for Executing Commands
 */
boolean doCmd(char cmdChar, char expected)
{
byte[] cmd = buildCmd(cmdChar);
byte[] ret = new byte[1];
String error = "";
try
{
i2c.transfer(slave_address, cmd, null); // write the command; read the status seperately, as there could be a long delay while the unit does not respond
}
catch (Exception e)
{
Log("error sending command " + cmdChar);
return false;
}

for (int i = 1; i < 10; i++)
{
try
{
if (i > 1)
Thread.sleep(10);
i2c.transfer(slave_address, null, ret);
if (ret[0] == (byte)expected)
{
//if (i > 1)
// Log("doCmd took " + i + " tries to get result of cmd " + cmdChar + " = " + (char)ret[0]);
return true;
}
else
{
Log("bad doCmd return; expected " + expected + ", got " + (char)ret[0]);
throw new Exception("bad cmd return = " + ret[0]);
}
}
catch (Exception e)
{
error = "doCmd Exception: " + e.toString();
}
}
return false;
}

boolean doSetByteByteCmd(char cmdChar, byte val1, byte val2)
{
byte[] cmd = new byte[3];
cmd[0] = (byte)cmd;
cmd[1] = val1;
cmd[2] = val2;
byte[] ret = new byte[1];
try
{
i2c.transfer(slave_address, cmd, ret);
if (ret[0] == 'a')
return true;
else
{
Log("bad doSetByteByteCmd return; expected 'a', got '" + (char)ret[0] + "'");
throw new Exception("bad cmd return = " + ret[0]);
}
}
catch (Exception e)
{
Log("doSetByteByteCmd Exception: " + e.toString());
return false;
}
}

boolean doSetWordCmd(char cmdChar, short value)
{
byte[] cmd = buildWordCmd(cmdChar, value);
byte[] ret = new byte[1];
try
{
i2c.transfer(slave_address, cmd, ret);
if (ret[0] == 'a')
return true;
else
{
Log("bad doSetWordCmd return; expected 'a', got '" + (char)ret[0] + "'");
throw new Exception("bad cmd return = " + ret[0]);
}
}
catch (Exception e)
{
Log("doSetWordCmd Exception: " + e.toString());
return false;
}
}

boolean doSetDWordCmd(char cmdChar, int value)
{
byte[] cmd = buildDWordCmd(cmdChar, value);
byte[] ret = new byte[1];
try
{
            ret[0] = 0;
i2c.transfer(slave_address, cmd, ret);
if (ret[0] == 'a')
return true;
else
{
Log("bad doSetDWordCmd return; expected 'a', got '" + (char)ret[0] + "'");
throw new Exception("bad cmd return = " + ret[0]);
}
}
catch (Exception e)
{
Log("doSetDWordCmd Exception: " + e.toString());
return false;
}
}

short doGetWordCmd(char cmdChar)
{
byte[] cmd = buildCmd(cmdChar);
byte[] ret = new byte[3];
try
{
i2c.transfer(slave_address, cmd, ret);
return extractWordParam(cmdChar, ret);
}
catch (Exception e)
{
Log("doGetWordCmd Exception: " + e.toString());
return 0;
}
}

int doGetDWordCmd(char cmdChar)
{
byte[] cmd = buildCmd(cmdChar);
byte[] ret = new byte[5];
try
{
i2c.transfer(slave_address, cmd, ret);
return extractDWordParam(cmdChar, ret);
}
catch (Exception e)
{
Log("doGetDWordCmd Exception: " + e.toString());
return 0;
}
}

byte doGetByteByteCmd(char cmdChar, byte val)
{
byte[] cmd = buildByteCmd(cmdChar, val);
byte[] ret = new byte[3];
try
{
ret[0] = 'X';
i2c.transfer(slave_address, cmd, null);
i2c.transfer(slave_address, null, ret);
if (ret[0] == cmdChar)
{
if (ret[1] == val)
return ret[2];
}
throw new Exception("bad cmd return = " + ret[0] + "; val = " + ret[1]);
return 0;
}
catch (Exception e)
{
Log("doGetByteByteCmd Exception: " + e.toString() + "; first ret byte: " + Integer.toHexString(ret[1]) + "; second ret byte: " + Integer.toHexString(ret[2]));
return 0;
}
}

short doGetWordByteCmd(char cmdChar, byte val)
{
byte[] cmd = buildByteCmd(cmdChar, val);
byte[] ret = new byte[3];
try
{
ret[0] = 'X';
i2c.transfer(slave_address, cmd, null);
i2c.transfer(slave_address, null, ret);
//i2c.transfer(slave_address, cmd, ret);
return extractWordParam(cmdChar, ret);
}
catch (Exception e)
{
Log("doGetWordByteCmd Exception: " + e.toString() + "; first ret byte: " + (char)ret[0]);
return 0;
}
}

short doGetWordByteXCmd(char cmdChar, byte val)
{
byte[] cmd = buildByteCmd(cmdChar, val);
byte[] ret = new byte[4];
try
{
ret[0] = 'X';
ret[1] = 'Y';
//Log("doing get word cmd with byte param: " + cmdChar + ", param = " + val);
i2c.transfer(slave_address, cmd, null);
i2c.transfer(slave_address, null, ret);
return extractWordXParam(cmdChar, val, ret);
}
catch (Exception e)
{
Log("doGetWordByteXCmd Exception: " + e.toString() + "; first ret byte: " + (char)ret[0]);
return 0;
}
}

/*
 * WheelCommander Commands
 */
boolean doSync()
{
return doCmd('.', '.');
}

boolean doEcho(byte value)
{
byte[] cmd = buildByteCmd('E', value);
byte[] ret = new byte[2];
try
{
i2c.transfer(slave_address, cmd, ret);
byte swapped = (byte)(((value >> 4) & 0x0f) | ((value << 4) & 0xf0));
if (extractByteParam('E', ret) == swapped)
return true;
else
return false;
}
catch (Exception e)
{
Log("doEcho Exception: " + e.toString());
return false;
}
}

String doName()
{
byte[] cmd = buildCmd('N');
byte[] ret = new byte[5];
try
{
i2c.transfer(slave_address, cmd, ret);
return new String(ret, 0, 5);
}
catch (Exception e)
{
Log("doName Exception: " + e.toString());
return null;
}
}

boolean doSetConstant(byte address, byte value)
{
short both = (short)(address + (value << 8));
return doSetWordCmd('F', both);
}

byte doGetConstant(byte address) throws Exception
{
short both = doGetWordByteCmd('F', address);
if ((byte)(both & 0x0ff) == address)
{
return (byte)((both >> 8) & 0x0ff);
}
else
throw new Exception("doGetConstant: bad address returned");
}

boolean doVelocity(short velocity)
{
return doSetWordCmd('V', velocity);
}

short getVelocity()
{
return doGetWordCmd('V');
}

boolean doAcceleration(short accel)
{
return doSetWordCmd('A', accel);
}

short getAcceleration()
{
return doGetWordCmd('A');
}

boolean doRotationRate(short rate)
{
return doSetWordCmd('Y', rate);
}

short getRotationRate()
{
return doGetWordCmd('Y');
}

boolean doAngle(short angle)
{
return doSetWordCmd('W', angle);
}

short getAngle()
{
return doGetWordCmd('W');
}

boolean doDistance(int distance)
{
return doSetDWordCmd('X', distance);
}

int getDistance()
{
return doGetDWordCmd('D');
}

boolean doGo()
{
return doCmd('G', 'a');
}

boolean doCoast()
{
return doCmd('C', 'a');
}

boolean doBrake()
{
return doCmd('B', 'a');
}

boolean doReset()
{
return doCmd('R', 'a');
}

float getADVoltage(byte channel)
{
short ADval = doGetWordByteXCmd('H', channel);
      return (float)(ADval * 5.0 / 1024.0);
}

float getDigitalVoltage()
{
float ADval = getADVoltage((byte)6); 
      return (float)(ADval * 40.01 / 10);
}

float getServoVoltage()
{
float ADval = getADVoltage((byte)7); 
      return (float)(ADval * 40.01 / 10);
}

boolean getDigitalInput(int bit)
{
return doGetByteByteCmd('J', bit);
}

boolean setDigitalOutput(int bit, boolean level)
{
return doSetByteByteCmd('J', bit, level);
}

boolean WaitDone() throws Exception
{
short status;
short prev_status = 0;

for (int i = 0; i < 150; i++)
{
status = doGetWordCmd('S');
status &= 0x0fff;
if (status != prev_status)
{
Log("status = " + Integer.toHexString(status & 0x0ffff));
prev_status = status;
}
if ((status & 0x10) != 0) // dst done is true now
return true;
Thread.sleep(200);
}
return false; // timeout
}


//********************************************
// Main Entry Point
//
//********************************************
    public static void main(String args[]) {

        try {
WCtest wc = new WCtest();
int i;

// connect to the WheelCommander
wc.Log("Trying sync:");
if (wc.doSync())
{
wc.Log("Got sync; trying echo:");

// confirm communications
if (wc.doEcho((byte)0x5b))
{
wc.Log("Echo worked.  Connected!");

// check device type
String name = wc.doName();
wc.Log("Device name and version: " + name);

// clear operating conditions
wc.Log("Resetting motion.");
wc.doReset();

// display useful info
byte mode = wc.doGetConstant((byte)2);
wc.Log("Current mode = " + Integer.toHexString(mode & 0x0ff));
float voltage = wc.getDigitalVoltage();
wc.Log("Digital Supply Voltage = " + voltage);
if ((voltage < (DVolt - VDivTol)) || (voltage > (DVolt + VDivTol)))
{
failures++;
wc.Log("***** DVolt test failed.");
}
voltage = wc.getServoVoltage();
if ((voltage < (SVolt - VDivTol)) || (voltage > (SVolt + VDivTol)))
{
failures++;
wc.Log("***** SVolt test failed.");
}
wc.Log("Servo Supply Voltage = " + voltage);

// test digital I/O
if (!setDigitalOutput(0, 0)) // connects to DIO2
failures++;
if (!setDigitalOutput(1, 0)) // connects to DIO3
failures++;
if (getDigitalInput(2, 0)) // should be zero
failures++;
if (getDigitalInput(3, 0)) // should be zero
failures++;

if (!setDigitalOutput(0, 1)) // connects to DIO2
failures++;
if (!getDigitalInput(2, 0)) // should be one
failures++;
if (getDigitalInput(3, 0)) // should be zero
failures++;

if (!setDigitalOutput(0, 0)) // connects to DIO2
failures++;
if (!setDigitalOutput(1, 1)) // connects to DIO3
failures++;
if (getDigitalInput(2, 0)) // should be zero
failures++;
if (!getDigitalInput(3, 0)) // should be one
failures++;

// test analog I/O
voltage = getADVoltage(0);
if ((voltage < (ADVolt0 -ADVoltTol)) || (voltage > (ADVolt0 + ADVoltTol)))
failures++;
voltage = getADVoltage(1);
if ((voltage < (ADVolt1 -ADVoltTol)) || (voltage > (ADVolt1 + ADVoltTol)))
failures++;
voltage = getADVoltage(2);
if ((voltage < (ADVolt2 -ADVoltTol)) || (voltage > (ADVolt2 + ADVoltTol)))
failures++;
voltage = getADVoltage(3);
if ((voltage < (ADVolt3 -ADVoltTol)) || (voltage > (ADVolt3 + ADVoltTol)))
failures++;

// demonstrate velocity control
for (short vel = 20; vel < 70; vel += 5)
{
wc.Log("Setting velocity to " + vel);
wc.doVelocity(vel);
wc.doGo();
//Thread.sleep(1);
wc.Log("vel = " + wc.getVelocity() + ", dist = " + wc.getDistance());
}
wc.Log("Coasting.");
wc.doCoast();

// demonstrate position and angle control
wc.doAcceleration((short)50);
wc.doVelocity((short)50);
for (int i = 0; i < 10; i++)
{
wc.Log("Turning 90 degrees");
wc.doAngle((short)90);
wc.Log("Set angle; setting distance");
wc.doDistance(0);
wc.Log("Set distance; starting go");
wc.doGo();
wc.Log("Going...");
if (!wc.WaitDone())
wc.Log("Timeout waiting for completion");
wc.doCoast();
wc.Log("dist = " + wc.getDistance() + ", ang = " + wc.getAngle());
wc.Log("Going 12 inches");
wc.doDistance(120);
wc.doAngle((short)0);
wc.doGo();
if (!wc.WaitDone())
wc.Log("Timeout waiting for completion");
wc.doCoast();
wc.Log("dist = " + wc.getDistance() + ", ang = " + wc.getAngle());
}
wc.doCoast();
}
}
else
num_failures++;
        } catch (Throwable t) {
            t.printStackTrace(); // Display a stack trace for any error
num_failures++;
        }
if (num_failures)
wc.Log("FAILED");
else
wc.Log("PASSED");
    }
}

 

 © 2004-2013 Noetic Design, Inc. All Rights Reserved. Nubotics, Unicoder, WheelCommander, and WheelWatcher are trademarks of Noetic Design, Inc.