JNA love NXT

I started working on something to bring some Ruby love to my Lego Mindstorm NXT. I know, there is already one or two projects that aims to do that, but the first one is dormant and the other one is at a fairly early stage.

The first thing that needs to be done before accomplishing anything useful on the NXT, is to connect to it via USB or (preferably) Bluetooth. This can be done in two ways : use the Lego Fantom API or use direct communication via a serial link.

Communicating via a serial link from Ruby involves using the ruby-serialport gem which seems to be no longer maintained. The other options to work with the serial port (besides using platform specific stuff) would be to use JRuby and the Java Communication API. But Java Comm only support Solaris and Linux. There is also RXTX but it is not that simple to get started with. If serial port communication was the only option, I would probably choose ruby-serialport to get things done (which ruby-nxt is using).

However, there is the option of using the Lego Fantom API. This API comes as a shared library that gets installed whenever you install the Lego Mindstorm software. It is available for Windows and Mac OS. This API is used by the Lego Mindstorm software so I assume it is well tested and will be maintained as long as there is a market for the NXT. It has the added benefit to completely abstract the actual communication link between the PC and the NXT. This mean that you no longer have to choose to work with USB or Bluetooth, the same code will work with both. While I would have likes to see the Fantom API available for Linux, this is not a big showstopper for me as I guess that most Mindstorm users use either Windows or Mac OS.

To use the Fantom API from Ruby, I need a way to map shared library functions to Ruby. This can be done using Ruby DLX, Ruby/DL or ruby-dl2. Again, none of these projects seems to be active at the moment. On the other hand, in Java land, there is JNA. Which is currently gathering some buzz and is being actively maintained. JNA allows to map shared library functions to Java interfaces is a really easy and concise way. Even if I want to ultimately use Ruby to talk to my robot, I don't care if my Ruby code must run under JRuby. So JNA is a good fit for me to get quickly started and not care to much about the low level communication stuff needed to connect to the NXT.

It is now time to start playing around and send some commands to my Lego robot.

So the first thing that need to be done, is to create the Java interface that will be used to map the Fantom API. Here is a minimalist version of it (it just contains the API function needed for this example):

Fantom.java

package treelaws.fantom;

import com.sun.jna.Native;
import com.sun.jna.Pointer;
import com.sun.jna.ptr.ByteByReference;
import com.sun.jna.ptr.IntByReference;
import com.sun.jna.win32.StdCallLibrary;
import java.nio.Buffer;

/**
 * Provides access to the Lego Mindstorm Fantom Library
 * @author Emmanuel Pirsch
 */
public interface Fantom extends StdCallLibrary {
    Fantom INSTANCE = (Fantom) Native.loadLibrary("fantom", Fantom.class);
    
    /**
     * 
     * @param searchBluetooth indicate if we also search bluetooth devices.
     * @param bluetoothSearchTimeout the timeout in second when searching bluetooth devices.
     * @param status OUT
     * @return
     */
    Pointer nFANTOM100_createNXTIterator(boolean searchBluetooth, int bluetoothSearchTimeout, Status status);
    /**
     * 
     * @param iNXTIterator
     * @param resourceName length must be at least 256.
     * @param status
     */
    void nFANTOM100_iNXTIterator_getName(Pointer iNXTIterator, byte[] resourceName, Status status);
    void nFANTOM100_iNXTIterator_advance(Pointer iNXTIterator, Status status);
    Pointer nFANTOM100_iNXTIterator_getNXT(Pointer iNXTIterator, Status status);
    void nFANTOM100_destroyNXTIterator(Pointer iNXTIterator, Status status);
    int nFANTOM100_iNXT_sendDirectCommand(Pointer iNXT, boolean requireResponse, Buffer commandBuffer, int commandBufferLength, byte[] responseBuffer, int responseBufferLength, Status status);
}

 

So now, I can easily connect to the NXT using something like :

String name= "T-NXT";
Status status= new Status();
Pointer iNXTIterator= fantom.nFANTOM100_createNXTIterator(true, 30, status);
try {
    while(!Status.Statuses.NO_MORE_ITEMS_FOUND.equals(status.getStatus())) {
        byte[] resourceName= newResourceName();
        fantom.nFANTOM100_iNXTIterator_getName(iNXTIterator, resourceName, status);
        System.out.println(asString(resourceName));
        if (asString(resourceName).contains(name)) {
            Pointer iNXT= fantom.nFANTOM100_iNXTIterator_getNXT(iNXTIterator, status);
            if (Status.Statuses.SUCCESS.equals(status.getStatus())) {
                return iNXT;
            } else {
                throw new UnableToCreateNXTException();
            }
        }
        fantom.nFANTOM100_iNXTIterator_advance(iNXTIterator, status);
    }
} finally {
    if (iNXTIterator != null) {
        fantom.nFANTOM100_destroyNXTIterator(iNXTIterator, status);
    }
}

 

And then start a robot program (already uploaded to the NXT) with :

String filename= "scorpion.rxe"
Status status= new Status();
byte[] filenameBytes= filename.getBytes();
ByteBuffer command= ByteBuffer.allocate(filenameBytes.length+1+1);
command.put((byte)0x00);
command.put(filenameBytes);
command.put((byte)0x00);
fantom.nFANTOM100_iNXT_sendDirectCommand(nxtPointer, false, command, command.capacity(), null, 0, status);

 

And this is just the beginning... Over the next few weeks (as time permit), I will add support to the rest of the Fantom API and add some nice Java classes to wrap this functionality and completely hide the Fantom API. Once this is done, I will release this code and start working on the JRuby side of things.

For completion, here are the NXT, Status and FantomUtils classes (The NXT class is the starting point of the Fantom API wrapper):

NXT.java

package treelaws.mindstorm;

import com.sun.jna.Pointer;
import java.nio.ByteBuffer;
import treelaws.fantom.Fantom;
import treelaws.fantom.Status;

import static treelaws.fantom.FantomUtils.*;

/**
 * 
 * @author Emmanuel Pirsch
 */
public class NXT {
    private static Fantom fantom= Fantom.INSTANCE;
    
    private String name;
    private Pointer nxtPointer;

    public NXT(String name) {
        this.name= name;
        this.nxtPointer= connect(name);
    }
    
    public void startProgram(String filename) {
        Status status= new Status();
        byte[] filenameBytes= filename.getBytes();
        ByteBuffer command= ByteBuffer.allocate(filenameBytes.length+1+1);
        command.put((byte)0x00);
        command.put(filenameBytes);
        command.put((byte)0x00);
        fantom.nFANTOM100_iNXT_sendDirectCommand(nxtPointer, false, command, command.capacity(), null, 0, status);
        System.out.println(status.getStatus().toString());
    }
    
    public void stopProgram() {
        ByteBuffer command= ByteBuffer.allocate(1);
        command.put((byte)0x01);
        fantom.nFANTOM100_iNXT_sendDirectCommand(nxtPointer, false, command, 1, null, 0, new Status());
    }

    private Pointer connect(String name) {
        Status status= new Status();
        Pointer iNXTIterator= fantom.nFANTOM100_createNXTIterator(true, 30, status);
        try {
            while(!Status.Statuses.NO_MORE_ITEMS_FOUND.equals(status.getStatus())) {
                byte[] resourceName= newResourceName();
                fantom.nFANTOM100_iNXTIterator_getName(iNXTIterator, resourceName, status);
                System.out.println(asString(resourceName));
                if (asString(resourceName).contains(name)) {
                    Pointer iNXT= fantom.nFANTOM100_iNXTIterator_getNXT(iNXTIterator, status);
                    if (Status.Statuses.SUCCESS.equals(status.getStatus())) {
                        return iNXT;
                    } else {
                        throw new UnableToCreateNXTException();
                    }
                }
                fantom.nFANTOM100_iNXTIterator_advance(iNXTIterator, status);
            }
        } finally {
            if (iNXTIterator != null) {
                fantom.nFANTOM100_destroyNXTIterator(iNXTIterator, status);
            }
        }
        throw new NXTNotFoundException();
    }
}

Status.java

package treelaws.fantom;

import com.sun.jna.Structure;
import java.util.EnumSet;
import java.util.HashMap;

/**
 *
 * @author Emmanuel Pirsch
 */
public class Status extends Structure {
    public int code;
    public byte[] filename= new byte[MAX_FILENAME_LENGTH];
    public int lineNumber;
    
    public Statuses getStatus() {
        return Statuses.fromCode(code);
    }
    
    static int MAX_FILENAME_LENGTH= 101;
    
    public enum Statuses {
        SUCCESS,
        FIRST(0),
        PAIRING_FAILED(5),
        BLUETOOTH_SEARCH_FAILED(6),
        SYSTEM_LIBRARY_NOT_FOUND(7),
        UNPAIRING_FAILED(8),
        INVALID_FILENAME(9),
        INVALID_ITERATOR_DEREFERENCE(10),
        LOCK_OPERATION_FAILED(11),
        SIZE_UNKNOWN(12),
        DUPLICATE_OPEN(13),
        EMPTY_FILE(14),
        FIRMWARE_DOWNLOAD_FAILED(15),
        PORT_NOT_FOUND(16),
        NO_MORE_ITEMS_FOUND(17),
        TOO_MANY_UNCONFIGURED_DEVICES(18),
        COMMAND_MISMATCH(19),
        ILLEGAL_OPERATION(20),
        BLUETOOTH_CACHE_UPDATE_FAILED(21),
        NON_NXT_DEVICE_SELECTED(22),
        RETRY_CONNECTION(23),
        POWER_CYCLE_NXT(24),
        FEATURE_NOT_IMPLEMENTED(99),
        FW_ILLEGAL_HANDLE(189),
        FW_ILLEGAL_FILENAME(190),
        FW_OUT_OF_BOUNT(191),
        FW_MODULE_NOT_FOUND(192),
        FW_FILE_EXISTS(193),
        FW_FILE_IS_FULL(194),
        FW_APPEND_NOT_POSSIBLE(195),
        FW_NO_WRITE_BUFFERS(196),
        FW_FILE_IS_BUSY(197),
        FW_UNDEFINED_ERROR(198),
        NO_LINEAR_SPACE(199),
        FW_HANDLE_ALREADY_CLOSED(200),
        FW_FILE_NOT_FOUND(201),
        FW_NOT_LINEAR_FILE(202),
        FW_END_OF_FILE(203),
        FW_END_OF_FILE_EXPECTED(204),
        FW_NO_MORE_FILES(205),
        FW_NO_SPACE(206),
        FW_NO_MORE_HANDLES(207),
        FW_UNKNOWN_ERROR_CODE(208),
        LAST(999);
        
        private int code;
        private static HashMap<Integer, Statuses> code_map= new HashMap<Integer, Statuses>();
        static {
            for(Statuses s : EnumSet.allOf(Statuses.class)) {
               code_map.put(s.code, s);
            }
        }
        Statuses() {
            this.code = 0;
        }
        Statuses(int code_offset) {
            this.code= -142000 - code_offset;
        }
        
        static Statuses fromCode(int code) {
            return code_map.get(code);
        }
    }
}

 

FantomUtils.java

package treelaws.fantom;

import com.sun.jna.Native;

/**
 *
 * @author Emmanuel Pirsch
 */
public class FantomUtils {
    static int DEFAULT_RESOURCE_NAME_ALLOCATION_SIZE = 256;
    
    public static final byte[] newResourceName() {
        return new byte[DEFAULT_RESOURCE_NAME_ALLOCATION_SIZE];
    }
    
    public static final String asString(byte[] s) {
        return Native.toString(s);
    }
}