Source Code for Module nxt.direct

  1  # nxt.direct module -- LEGO Mindstorms NXT direct telegrams 
  2  # Copyright (C) 2006-2007  Douglas P Lau 
  3  # Copyright (C) 2009  Marcus Wanner 
  4  # 
  5  # This program is free software: you can redistribute it and/or modify 
  6  # it under the terms of the GNU General Public License as published by 
  7  # the Free Software Foundation, either version 3 of the License, or 
  8  # (at your option) any later version. 
  9  # 
 10  # This program is distributed in the hope that it will be useful, 
 11  # but WITHOUT ANY WARRANTY; without even the implied warranty of 
 12  # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
 13  # GNU General Public License for more details. 
 14   
 15  'Use for direct communication with the NXT ***EXTREMELY ADVANCED USERS ONLY***' 
 16  import time 
 17   
 18 -def _create(opcode): 
 19          'Create a simple direct telegram' 
 20          from telegram import Telegram 
 21          return Telegram(True, opcode) 
 22   
 23 -def start_program(opcode, fname): 
 24          tgram = _create(opcode) 
 25          tgram.add_filename(fname) 
 26          return tgram 
 27   
 28 -def _parse_simple(tgram): 
 29          tgram.check_status() 
 30   
 31 -def stop_program(opcode): 
 32          return _create(opcode) 
 33   
 34 -def play_sound_file(opcode, loop, fname): 
 35          tgram = _create(opcode) 
 36          tgram.add_u8(loop) 
 37          tgram.add_filename(fname) 
 38          return tgram 
 39   
 40 -def play_tone(opcode, frequency, duration): 
 41          'Play a tone at frequency (Hz) for duration (ms)' 
 42          tgram = _create(opcode) 
 43          tgram.add_u16(frequency) 
 44          tgram.add_u16(duration) 
 45          return tgram 
 46   
 47 -def set_output_state(opcode, port, power, mode, regulation, turn_ratio, 
 48      run_state, tacho_limit): 
 49          tgram = _create(opcode) 
 50          tgram.add_u8(port) 
 51          tgram.add_s8(power) 
 52          tgram.add_u8(mode) 
 53          tgram.add_u8(regulation) 
 54          tgram.add_s8(turn_ratio) 
 55          tgram.add_u8(run_state) 
 56          tgram.add_u32(tacho_limit) 
 57          return tgram 
 58   
 59 -def set_input_mode(opcode, port, sensor_type, sensor_mode): 
 60          tgram = _create(opcode) 
 61          tgram.add_u8(port) 
 62          tgram.add_u8(sensor_type) 
 63          tgram.add_u8(sensor_mode) 
 64          return tgram 
 65   
 66 -def get_output_state(opcode, port): 
 67          tgram = _create(opcode) 
 68          tgram.add_u8(port) 
 69          return tgram 
 70   
 71 -def _parse_get_output_state(tgram): 
 72          tgram.check_status() 
 73          port = tgram.parse_u8() 
 74          power = tgram.parse_s8() 
 75          mode = tgram.parse_u8() 
 76          regulation = tgram.parse_u8() 
 77          turn_ratio = tgram.parse_s8() 
 78          run_state = tgram.parse_u8() 
 79          tacho_limit = tgram.parse_u32() 
 80          tacho_count = tgram.parse_s32() 
 81          block_tacho_count = tgram.parse_s32() 
 82          rotation_count = tgram.parse_s32() 
 83          return (port, power, mode, regulation, turn_ratio, run_state, 
 84                  tacho_limit, tacho_count, block_tacho_count, rotation_count) 
 85   
 86 -def get_input_values(opcode, port): 
 87          tgram = _create(opcode) 
 88          tgram.add_u8(port) 
 89          return tgram 
 90   
 91 -def _parse_get_input_values(tgram): 
 92          tgram.check_status() 
 93          port = tgram.parse_u8() 
 94          valid = tgram.parse_u8() 
 95          calibrated = tgram.parse_u8() 
 96          sensor_type = tgram.parse_u8() 
 97          sensor_mode = tgram.parse_u8() 
 98          raw_ad_value = tgram.parse_u16() 
 99          normalized_ad_value = tgram.parse_u16() 
100          scaled_value = tgram.parse_s16() 
101          calibrated_value = tgram.parse_s16() 
102          return (port, valid, calibrated, sensor_type, sensor_mode, raw_ad_value, 
103                  normalized_ad_value, scaled_value, calibrated_value) 
104   
105 -def reset_input_scaled_value(opcode, port): 
106          tgram = _create(opcode) 
107          tgram.add_u8(port) 
108          return tgram 
109   
110 -def message_write(opcode, inbox, message): 
111          tgram = _create(opcode) 
112          tgram.add_u8(inbox) 
113          tgram.add_u8(len(message) + 1) 
114          tgram.add_string(len(message), message) 
115          tgram.add_u8(0) 
116          return tgram 
117   
118 -def reset_motor_position(opcode, port, relative): 
119          tgram = _create(opcode) 
120          tgram.add_u8(port) 
121          tgram.add_u8(relative) 
122          return tgram 
123   
124 -def get_battery_level(opcode): 
125          return _create(opcode) 
126   
127 -def _parse_get_battery_level(tgram): 
128          tgram.check_status() 
129          millivolts = tgram.parse_u16() 
130          return millivolts 
131   
132 -def stop_sound_playback(opcode): 
133          return _create(opcode) 
134   
135 -def keep_alive(opcode): 
136          return _create(opcode) 
137   
138 -def _parse_keep_alive(tgram): 
139          tgram.check_status() 
140          sleep_time = tgram.parse_u32() 
141          return sleep_time 
142   
143 -def ls_get_status(opcode, port): 
144          'Get status of low-speed sensor (ultrasonic)' 
145          tgram = _create(opcode) 
146          tgram.add_u8(port) 
147          return tgram 
148   
149 -def _parse_ls_get_status(tgram): 
150          tgram.check_status() 
151          n_bytes = tgram.parse_u8() 
152          return n_bytes 
153   
154 -def ls_write(opcode, port, tx_data, rx_bytes): 
155          'Write a low-speed command to a sensor (ultrasonic)' 
156          time.sleep(0.005) 
157          tgram = _create(opcode) 
158          tgram.add_u8(port) 
159          tgram.add_u8(len(tx_data)) 
160          tgram.add_u8(rx_bytes) 
161          tgram.add_string(len(tx_data), tx_data) 
162          return tgram 
163   
164 -def ls_read(opcode, port): 
165          'Read a low-speed sensor value (ultrasonic)' 
166          tgram = _create(opcode) 
167          tgram.add_u8(port) 
168          return tgram 
169   
170 -def _parse_ls_read(tgram): 
171          tgram.check_status() 
172          n_bytes = tgram.parse_u8() 
173          contents = tgram.parse_string() 
174          return contents[:n_bytes] 
175   
176 -def get_current_program_name(opcode): 
177          return _create(opcode) 
178   
179 -def _parse_get_current_program_name(tgram): 
180          tgram.check_status() 
181          fname = tgram.parse_string() 
182          return fname 
183   
184 -def message_read(opcode, remote_inbox, local_inbox, remove): 
185          tgram = _create(opcode) 
186          tgram.add_u8(remote_inbox) 
187          tgram.add_u8(local_inbox) 
188          tgram.add_u8(remove) 
189          return tgram 
190   
191 -def _parse_message_read(tgram): 
192          tgram.check_status() 
193          local_inbox = tgram.parse_u8() 
194          n_bytes = tgram.parse_u8() 
195          message = tgram.parse_string() 
196          return (local_inbox, message[:n_bytes]) 
197   
198  OPCODES = { 
199          0x00: (start_program, _parse_simple), 
200          0x01: (stop_program, _parse_simple), 
201          0x02: (play_sound_file, _parse_simple), 
202          0x03: (play_tone, _parse_simple), 
203          0x04: (set_output_state, _parse_simple), 
204          0x05: (set_input_mode, _parse_simple), 
205          0x06: (get_output_state, _parse_get_output_state), 
206          0x07: (get_input_values, _parse_get_input_values), 
207          0x08: (reset_input_scaled_value, _parse_simple), 
208          0x09: (message_write, _parse_simple), 
209          0x0A: (reset_motor_position, _parse_simple), 
210          0x0B: (get_battery_level, _parse_get_battery_level), 
211          0x0C: (stop_sound_playback, _parse_simple), 
212          0x0D: (keep_alive, _parse_keep_alive), 
213          0x0E: (ls_get_status, _parse_ls_get_status), 
214          0x0F: (ls_write, _parse_simple), 
215          0x10: (ls_read, _parse_ls_read), 
216          0x11: (get_current_program_name, _parse_get_current_program_name), 
217          0x13: (message_read, _parse_message_read), 
218  } 
219