From 2ae681a0a1a76f1e54bf77e3eb48b6d5b98a0ea2 Mon Sep 17 00:00:00 2001 From: Tat-Chee Wan (USM) Date: Thu, 19 May 2011 14:09:13 +0800 Subject: rewrite send_direct_command() send_direct_command() is rewritten to return the response string instead of the data read length. Added Battery Level Check test (direct command with required response) --- pyfantom.py | 67 +++++++++++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 56 insertions(+), 11 deletions(-) (limited to 'pyfantom.py') diff --git a/pyfantom.py b/pyfantom.py index 90fa389..decdc67 100644 --- a/pyfantom.py +++ b/pyfantom.py @@ -370,21 +370,23 @@ class NXT: assert ret <= length return data_buffer.raw[0:ret] - def send_direct_command(self, data, response=None, requireResponse=False): + def send_direct_command(self, data, responseLength=0, requireResponse=False): """Sends the specified direct command to this NXT.""" + """Note: responseLength MUST be set to the exact Data Length or else I/O Error will result.""" status = StatusVar(0) data_buffer = ctypes.create_string_buffer(data) + response_buffer = None if requireResponse: - assert response is not None - response_buffer = ctypes.create_string_buffer(response) - responseLen = len(response) - else: - response_buffer = None - responseLen = 0 + response_buffer = ctypes.create_string_buffer(responseLength) ret = dll.nFANTOM100_iNXT_sendDirectCommand(self.handle, requireResponse, data_buffer, len(data), - response_buffer, responseLen, cast(byref(status), POINTER(c_int))) + response_buffer, responseLength, cast(byref(status), POINTER(c_int))) Status.check(status) - return ret + if requireResponse: + if self.debug: + print "send_direct_command() response: ", struct.unpack('%dB' % len(response_buffer), response_buffer) + return response_buffer.raw[0:ret-1] # ret count includes Status byte, but Status byte is not included in message buffer + else: + return "" def poll_command_length(self, buffer=0): """Polls the data buffer on this NXT for the number of bytes available to be read.""" @@ -499,19 +501,57 @@ if __name__ == '__main__': # Play Tone using Send Direct Command print "send direct command (tone)..." cmd = struct.pack('5B', 0x03, 0x00, 0x08, 0x10, 0x00) - r = i.send_direct_command(cmd) + rep = i.send_direct_command(cmd) + print "direct command returned ", len(rep), " data bytes" time.sleep(1) # Play Tone using Send Raw Command print "send raw command (tone)..." cmd = struct.pack('6B', 0x80, 0x03, 0x00, 0x18, 0x10, 0x00) r = i.write(cmd) + def get_nxt_battery(i): + # Use Direct Command to read Battery level + # Write GETBATTERYLEVEL DIRECT_CMD. + # Query: + # DIRECT_CMD: 0x00 (response required) + # BATT_LEVEL: 0x0B + # Response (4 data bytes): + # REPLY_CMD: 0x02 (not included in reply string) + # BATT_LEVEL: 0x0B + # VOLTAGE milivolts + # VOLTAGE milivolts + print "send direct command (battery level)..." + cmd = struct.pack('1B', 0x0B) + rep = i.send_direct_command(cmd, 4, True) + print "direct command returned ", len(rep), " data bytes" + print "Battery Level", struct.unpack('%dB' % len(rep), rep) + + # Use Send Raw Command to read Battery level + # Write GETBATTERYLEVEL DIRECT_CMD. + # Query: + # DIRECT_CMD: 0x00 (response required) + # BATT_LEVEL: 0x0B + # Response (4 data bytes): + # REPLY_CMD: 0x02 (not included in reply string) + # BATT_LEVEL: 0x0B + # VOLTAGE milivolts + # VOLTAGE milivolts + print "send raw command (battery level)..." + cmd = struct.pack('2B', 0x00, 0x0B) + r = i.write(cmd) + print "wrote", r + rep = i.read(MAXBUFLEN) + print "Battery Level", struct.unpack('%dB' % len(rep), rep) + + + def write_read_nxt(i, readbuflen=MAXBUFLEN): # Test Variable Sized Reads of reply message # Write VERSION SYS_CMD. # Query: # SYS_CMD: 0x01 # VERSION: 0x88 + print "send raw command (version system command)..." cmd = struct.pack('2B', 0x01, 0x88) r = i.write(cmd) print "wrote", r @@ -536,8 +576,9 @@ if __name__ == '__main__': check_bt = False get_info = True play_tone = True + get_battery = True write_read = True - empty_readbuf = True + empty_readbuf = False print "Retrieving list of NXT objects" @@ -548,6 +589,8 @@ if __name__ == '__main__': get_nxt_info(i) if play_tone: play_tone_nxt(i) + if get_battery: + get_nxt_battery(i) if write_read: write_read_nxt(i) if empty_readbuf: @@ -564,6 +607,8 @@ if __name__ == '__main__': get_nxt_info(i) if play_tone: play_tone_nxt(i) + if get_battery: + get_nxt_battery(i) if write_read: write_read_nxt(i) if empty_readbuf: -- cgit v1.2.3