2.27.2008

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);
    }
}

8 comments:

contrechoc said...

Thx a lot!
Because this works, so letting me send direct commands to the NXT using JAVA in Eclipse.
(I had to make and add a few exceptions: throws NXTNotFoundException, UnableToCreateNXTException)
My problem was, that i could connect very well using bluetooth with C# and C++ libs, but not with JAVA (icommand got to 90% :-(
and so i had to shift the lego FIRMWARE and lejos all the time.
(needing for balancing a fast prog on the brick)
Now i can expand your JAVA lib and keep the lejos firmware on the brick.
Nice!
For the time being i am exploring possiblities of C++, C# and JAVA, looked at ruby a bit, but that will be for later :-)

contrechoc said...
This comment has been removed by the author.
contrechoc said...
This comment has been removed by the author.
contrechoc said...

ok, for my students i wrote what i added and changed,
http://nxt-adventures.blogspot.com/2008/03/fantom-api-and-java.html
all credits to the author emmanuel!
(actually the timeout is the most important "change"- making 5 sec of the 30, because the connection waited 30 sec before doing something with the direct commands)

Emmanuel Pirsch said...

I'm aware of the timeout issue. I believe it's to enable the Fantom API to have time to find the bluetooth devices. I've also reduced it to 5 seconds to make it more bearable.

I think we can use the "nFANTOM100_createNXT" method if we have the full identifying String (this is the name returned by nFANTOM100_iNXTIterator_getName). This should try to connect directly to the NXT brick without any timeout.

The following line would be the definition for the JNA mapping:
Pointer nFANTOM100_createNXT(String resourceString, Status status, boolean checkFirmwareVersion);

I've added support for most of the NXT commands and found out some issues when using the turning ratio with setOutputState (basically, you have to issue a resetMotorPosition if you don't want your NXt to act funny).

The issue with the connection, is that the NXT can connect to multiple device only when it is the Master. When using the Fantom API, the NXT is acting as a Slave and cannot issue commends over bluetooth to other devices.

I've created a Google Code project and will put my code in it as soon as I shape it up a little.

contrechoc said...

Yep, thx for the suggestion, this method using the bricks name (fantom.nFANTOM100_createNXT)works and connects instantly. I made some sketchy classes for the light sensor and some I2C sensors (sonar and mindsensors clock) all worked!
great!

contrechoc said...

Hi, I had a big problem using 3 I2C sensors (at the same time) and the fantom lib (readings came in the wrong order) finally this was resolved: i had to do a careful check on which bytes were send and taken care of. The bytes which are directly returned.
The next problem is probably not to be resolved: setting sensor registers cannot be done (LSWRITE a value to a register) because of the master slave relation between the PC and the NXT?
So putting the NXTCAM in tracking mode or setting the time in the clocksensor is not possible, from the PC using fantom lib.
For more advanced use this limits all the bluetooth libs (C++, C#).
But maybe there is a work around?

wow power leveling said...
This comment has been removed by a blog administrator.

AdSense Links