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.
Download
WheelCommander.java - Java source
WheelCommander.rjp - RoboJDE project file
Source Code
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 WheelCommander {
I2CMaster i2c;
int slave_address;
Display display;
/*
* WheelCommander constructor
*/
WheelCommander()
{
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 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;
}
}
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 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 {
WheelCommander wc = new WheelCommander();
// 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("Digtal Supply Voltage = " + voltage);
voltage = wc.getServoVoltage();
wc.Log("Servo Supply Voltage = " + voltage);
// 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();
}
}
} catch (Throwable t) {
t.printStackTrace(); // Display a stack trace for any error
}
}
}
©2004-2008 Noetic Design, Inc. All Rights Reserved. Nubotics and WheelWatcher are trademarks of Noetic Design, Inc.