Welcome, guest | Sign In | My Account | Store | Cart

The new Lego Mindstorms NXT brick has an on-board Bluetooth transceiver that can connect to a serial port service on a PC. The NXT uses a simple format to pass raw bytes between connected Bluetooth devices. This interface allows the NXT brick to be controlled from Python. Robot programming in Python anyone?

Python, 212 lines
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
'''
Read and write data to a Lego Mindstorm NXT brick using serial bluetooth
connection.  You'll need to modify __init__ for unix style serial port 
identification in order to use this on Linux.

Blue enables raw byte transfer
TypeBlue utilizes NXT mailbox number for type identification.

Usage:
1. Enable a bluetooth serial port to accept connection requests from NXT.
2. Find and connect computer from NXT bluetooth menu.  Note serial port
   number; store in comport_num.

3. From python try this code, note the try finally and make sure the connection
   is established so that you are not waiting all the time for timeouts!  It is
   a real pain getting the comport back from a dropped connection.
   
import blueNXT
try:
    b = blueNXT.TypeBlue(comport_num)
    b.put('Hello NXT!')
    b.putall(False, True, 1, 'two')
    b.get()
finally:
    b.close()

4.  Write an interface to remote control your robots and share!
'''
__author__ = 'Justin Shaw'
import sys
import serial
import struct
import time

class Blue:
    '''
    A bluetooth connection to a Lego NXT brick
    '''
    huh = struct.pack('h', 2432) # don't really know what this is

    def __init__(self, comport=9, filename=None, mode='r', timeout=10):
        '''
        comport - integer com number for serial port
        filename and mode are for debug
        '''
        if filename is None:
            self.s = serial.Serial('COM%d' % comport, timeout=timeout)
        else:
            self.s = open(filename, mode)

    def get(self):
        '''
        Return payload, payload
        
        Get next message from NXT, return un-molested payload i.e. bytes.
        Use get_int() for integers and get_bool() for booleans
        '''
        sz = self.s.read(2)
        payload = None
        box = None
        if len(sz) == 2:
            sz = struct.unpack('h', sz)[0]
            # print 'sz', sz
            if 0 < sz < 1000:
                msg = self.s.read(sz)
                # print 'msg', msg
                dat = msg[:4]
                # for c in dat:
                #     print ord(c)
                # print struct.unpack('h', msg[:2])
                box = ord(dat[2]) + 1
                payload = msg[4:-1]
        return payload, box

    def put(self, payload, box=1):
        '''
        Send a raw message to NXT
        payload -- bytes to send
        box -- 1 to 10, which mail box on NXT to place message in
        '''
        # sz    msg----> 0
        # 0123456789 ... n
        payload += chr(0)
        pl_sz = len(payload)
        sz = pl_sz + 4
        header = struct.pack('h2sbb', sz, self.huh, box - 1, pl_sz)
        out = struct.pack('6s%ds' % pl_sz, header, payload)
        # print 'out', out
        dat = out[2:6]
        # for c in dat:
        #     print ord(c)
        # print
        # self.s.write('\x11\x00\x80\t\x00\r<0123456789>\x00')
        self.s.write(out)

    def __del__(self):
        try:
            self.close()
        except:
            pass
    def close(self):
        self.s.close()
        
class TypeBlue(Blue):
    '''
    Use mailbox number for type information:
    1 -- string
    2 -- int
    3 -- bool

    else -- string
    '''
    
    def get(self):
        '''
        Get a message off port.  Determine type from box number:
        1 -- string
        2 -- int
        3 -- bool
        '''
        msg, box = Blue.get(self)
        if box == 2:
            out = struct.unpack('i', msg)[0]
        elif box == 3:
            out = not not(ord(msg))
        else:
            out = msg
        return out
    
    def put(self, val):
        '''
        Put a message on port.  Use box to indicate type:
        1 -- string
        2 -- int
        3 -- bool
        '''
        if type(val) == type(''):
            msg = val
            box = 1
        elif type(val) == type(0):
            msg = struct.pack('i', val)
            box = 2
        elif type(val) == type(False):
            msg = struct.pack('b', not not val)
            box = 3
        return Blue.put(self, msg, box)

    def putall(self, *vals):
        '''
        Send several values to NXT
        '''
        for v in vals:
            self.put(v)

def Blue__test__():
    '''
    Test that the formats are consistant by reading and writing
    to a file.  No real bluetooth required.
    '''
    # read
    b = Blue(filename='text.dat')
    target = '<0123456789>'
    for i in range(10):
        msg, box = b.get()
        assert msg == target, '%s != %s' % (msg, target)

    # write
    b = Blue(filename='junk', mode='wb')
    b.put(target, 2)
    b = Blue(filename='junk')
    got, box = b.get()
    assert box == 2
    assert got == target, '%s != %s' % (got, target)
    b = Blue(filename='num.dat')

    # type
    b = TypeBlue(filename='junk', mode='wb')
    b.put(target)
    b.put(1)
    b.put(False)
    b = TypeBlue(filename='junk')
    got = b.get()
    assert got == target
    got = b.get()
    assert got == 1
    got = b.get()
    assert got == False
    
def tblue():
    '''
    Real bluetooth test.
    '''
    try:
        b = TypeBlue('COM10')
        for i in range(20):
            ##  only uncomment these if you have the NXT code sending data!
            # print b.get()
            # print b.get()
            # print b.get()
            # b.put(42)
            # b.put(False)
            b.put('HERE % d' % i)
            b.put(i)
            if i < 10:
                b.put(False)
            else:
                b.put(True)
            time.sleep(.25)
    finally:
        del b
# tblue()
# Blue__test__()

Although type information is not contained in the messages, the NXT has 10 distinct "mailboxes" whose box number is referenced in the message. The class TypeBlue uses the first three boxes as a type indicator: box 1 for byte data (strings), box 2 for integers, and box 3 for Booleans. This turns out to be too simlplistic to be useful, but you get the idea.

I have written a version that uses threads to constantly poll the serial port. That fragile version likes to get hung up so I have not included it here.

I point out in the doc strings, but it deserves another mention, that hung connections can be a real pain when you are debugging. Be sure to remember the try finally and ensure the Bluetooth connection is live.