Lego Lab 16

Bluetooth communication

Date: 02-06/05/06 - January - 2011
Duration: 10 hours
Participants: Maria Soler

Goals for the lab session

• Primary goal: Communication between the two robots, including parsing and understanding of the commands.
• Secondary goal: Investigate communication the communication between the PC and the robot. This is something that we have not maneged to do in the previous labs.

The commands

The commands are implemented as classes that can be serialized and deserialized when they are transmitted. The structure and the methods are implemented in a command base class structure (Strategy design pattern). The specific parameters are implemented in the child classes, as shown in figure 1.

Figure 1: UML class diagram showing the different commands the system is featuring

public class Command
{
protected String commandID;
protected Hashtable parameters;
protected DataLogger dataLogger;
protected boolean logging;

public Command(DataLogger logger){...}
public Command(){...}

public void Deserialize(String frame)
{
//log if we are debugging
if(logging) this.dataLogger.writeLine("Parsing the command");

//parse the command ID
if(frame.indexOf("FETCH")> -1)
{
this.commandID = FetchCommand.ID;
}
else if(frame.indexOf("ACK")> -1)
{
this.commandID = ACKCommand.ID;
}
else if(frame.indexOf("DELIVER")> -1)
{
this.commandID = DeliverCommand.ID;
}
else
{
this.commandID = "";
}
if(logging) this.dataLogger.writeLine(commandID);

//Parse the parameters
this.parameters = new Hashtable();
int index = frame.indexOf(' ');
int start = index +1;
index = frame.indexOf(' ', start);
while(index > -1)
{
String parameter = frame.substring(start, index);
//put the parameter in the parameters Hashtable
parseParameter(parameter);

start = index + 1;
index = frame.indexOf(' ', start);
}
//add the last parameter, if any
if(frame.indexOf('=', start) > -1)
{
String parameter = frame.substring(start);
parseParameter(parameter);
}
if(logging) this.dataLogger.writeLine("Command parsed");
}
public String Serialize()
{
String frame = "";
frame += commandID;

Enumeration keys = parameters.keys();
while(keys.hasMoreElements())
{
String id = (String) keys.nextElement();
frame += " " + id + "=" + (String)parameters.get(id);
}
frame += "\n";
return frame;
}
[...]
}

The child classes just implement a specialized constructor and the getters for the specialized parameters. We can take the DeliverCommand as an example.

public class DeliverCommand extends Command {

public static String ID = "DELIVER";

public DeliverCommand(DataLogger logger)
{
super(logger);
commandID = ID;
}
public DeliverCommand() {...}

public DeliverCommand(DataLogger logger, float x, float y)
{
super(logger);
commandID = ID;

parameters.put("X", Float.toString(x));
parameters.put("Y", Float.toString(y));
}
public DeliverCommand(Command command)
{
if(command.getCommandID() .equals(ID) )
{
this.commandID = ID;
this.dataLogger = command.dataLogger;
this.parameters = command.parameters;
}
}
public float getX()
{
String x = (String) parameters.get("X");
return Float.parseFloat(x);
}
[...]
}

As the code shows, the parameters for the specific commands are not saved in a specific field, but the use the parameters Hashtable from the base class.

Code structure

All the communication shared code is placed in a separate java project so both robot 1 and 2 can access it. This way we avoid repeated code, which is much more difficult to maintain, even in a short project like this one.

Logging files

To be able to debug during development face, and also to monitor the robots behaviour during deployment, a logging mechanism has been implemented. The DataLogger class is inspired by the one used in lego lab 3 [1], but it is optimized to write lines instead of samples.

public class DataLogger
{
   private File f;
   private FileOutputStream fos;
   
   public DataLogger (String fileName) {...}

   public void logReceived(String line)
   {
writeLine("<< " + line);
   }
   public void logSent(String line)
   {
writeLine(">> " + line);
    }
   
   public void writeLine (String line) {...}
   public void close() {...}
}

A method is also implemented to generate the file names. Provided a prefix, it checks the NXT file system to generate the first prefixN.txt that doesn’t exist, where N is an integer from 0 to 100.

Then, if we want to generate the logs in robot1, we could call it robot1logN.txt. The method will see if robot1log0.txt, robot1log1.txt, …, exist, and give the next available name.

It is possible then to keep the logs from different executions of the program and analyse them later.

Communication between robots: FETCH and ACK

In this lab session only a test standalone program is implemented, to see how the communication should work. It will be later modified and integrated into the robots architecture.

The test program sends 10 FETCH commands and expects 10 ACKs back.

Robot 1: Sending the commands

Robot 1 starts the communication. The first thing done is to start a bluetooth channel with Robot 2. This was already done in lab 15 [2].

The program is very simple: send a command, wait for an acknowledge, and repeat that 10 times.

public class BTConnectTest {

private static String filePrefix = "senderLog";
public static void main(String[] args) throws Exception {

String fileName = Utils.getFileName(filePrefix);

     DataLogger logger = new DataLogger(fileName);       
     BTConnection btc = connectBluetooth(logger);

DataInputStream dis = btc.openDataInputStream();
DataOutputStream dos = btc.openDataOutputStream();

for(int i = 0; i<10;i++)

{
FetchCommand fetchCommand = new FetchCommand(3*i, (float)(25.46 * (float)i/10.0), (float)2.6, (float)6.75, i*2);
//"FETCH ID=3 X=25.46 Y=2.6 H=6.75 P=3\n";
String command = fetchCommand.Serialize();
try {
Utils.writeUpperLineToLCD("Sending command");
Utils.send(dos, command);
logger.logSent(command);
} catch (IOException ioe) {
Utils.writeUpperLineToLCD("Write Exception");
}
try {
String response = Utils.receive(dis);
logger.logReceived(response);
Utils.writeUpperLineToLCD(response);
} catch (IOException ioe) {
Utils.writeUpperLineToLCD("Read Exception");
}
}
[...] //Close all connections
}
}

The Utils class has some methods that can be used both in Robot1 program and Robot 2 program, so no coding needs repetition.

Robot 2: Receiving the commands

The testing program for Robot 2 waits for a bluetooth connection and then for 10 commands, which are acknowledged. The commands are just deserialized and printed on the log, but the values are not checked or interpreted. That will be done in the integration session.

for(int i = 0; i<10; i++)
{

String commandString = Utils.receive(dis);

if(commandString.length() > 0)
{
logger.logReceived(commandString);

Utils.writeUpperLineToLCD("Parsing the command");

Command command = new Command();
command.Deserialize(commandString);

Utils.writeUpperLineToLCD("Command parsed");

ACKCommand ack = new ACKCommand();

Utils.send(dos, ack.Serialize());
logger.logSent(ack);
}
else
{
logger.logReceived("null");
}
}

Communication between PC and Robot2: DELIVER and ACK

After having problems with PC to robot communication in lab 13 [3] we nearly gave up on this, but decided to use some more hours with a new approach and try again. This time we paired the robot with the computer and let the computer initiate the communication. This idea worked at first try. We changed a bit of the example code to search for a specific bluetooth device and not for “any”, like in the example from Lejos [4].

Code used in the first try (that didn’t work):
NXTConnector conn = new NXTConnector();
// Connect to any NXT over Bluetooth
boolean connected = conn.connectTo("btspp://");
[...]

Code used in the new approach (that actually works):
NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
NXTInfo[] nxtInfos = nxtComm.search(“Robot2”,NXTCommFactory.BLUETOOTH);
NXTConnector conn = new NXTConnector();
// Connect to our known NXT over Bluetooth
boolean connected = conn.connectTo(nxtInfo, NXTComm.PACKET);
[...]

The test program in the computer reads a set of coordinates (x,y) from the terminal and sends a DELIVER command to Robot2.

The only problem we have with the above program is that the PC keeps the connection open and the robot don’t. Therefore the computer program gives a communication error when trying to transmit for a second time. This was solved by using the same strategy for both parts of the system.

The strategy chosen is to close the communication channels after each command and open them again. In this way it will work in the setup for the robot 2 collector, since the robot first needs to receive a command where to fetch the object, then stop communication, and then wait for a command saying where to deliver the object.

Since the code is similar to the communication between the robots, we are not showing any code snippet, but the whole code can be find in [5] and [6]

Conclusion

The communication between both robots and PC works fine, and there is no problems when parsing and serializing commands and responses.

The only outstanding problem is performance. The connection is very slow to establish. But since performance is not a goal for this system, we will not consider it as a big problem. If it was, some changes should be considered. In the case that 2 commands should be sent from the same robot or same PC one after the other, the program could be optimized by not closing the connection. But in this system, one command is sent from the PC and one is sent from Robot 1, so there is not much we can do about this performance issue.

References

[1] Lab session 3.  http://www.legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson3.dir/Lesson.html
[2] Lego Lab 15: http://josemariakim.blogspot.com/2011/01/lego-lab-15.html
[3] Lego Lab 13: http://josemariakim.blogspot.com/2011/01/lego-lab-13.html
[4] Lejos Bluetooth Tutorial http://lejos.sourceforge.net/nxt/nxj/tutorial/Communications/Communications.htm
[5] PC client to communicate with the robot via bluetooth. http://code.google.com/p/josemariakim/source/browse/trunk/FP_pcProgram/src/pcProgram/PCProgram.java?r=80
[6] PC client to communicate with the robot via bluetooth (II). http://code.google.com/p/josemariakim/source/browse/trunk/?r=80#trunk%2FFP_bluetoothPC_NXT%2Fsrc