aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/armdebug/nxt-python-fantom/nxt
diff options
context:
space:
mode:
Diffstat (limited to 'AT91SAM7S256/armdebug/nxt-python-fantom/nxt')
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/__init__.py17
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/bluesock.py82
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/brick.py226
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/direct.py216
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/error.py87
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/fantomglue.py211
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/lightblueglue.py53
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/locator.py57
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/motor.py431
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/pyusbglue.py82
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py50
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py41
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py67
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py227
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py154
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py611
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py815
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/system.py297
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/telegram.py118
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/usbsock.py133
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/utils.py33
21 files changed, 4008 insertions, 0 deletions
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/__init__.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/__init__.py
new file mode 100644
index 0000000..01be052
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/__init__.py
@@ -0,0 +1,17 @@
+# nxt.__init__ module -- LEGO Mindstorms NXT python package
+# Copyright (C) 2006 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from .locator import find_one_brick
+from .motor import *
+from .sensor import *
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/bluesock.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/bluesock.py
new file mode 100644
index 0000000..99917f2
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/bluesock.py
@@ -0,0 +1,82 @@
+# nxt.bluesock module -- Bluetooth socket communication with LEGO Minstorms NXT
+# Copyright (C) 2006, 2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+try:
+ import fantomglue as bluetooth
+except ImportError:
+ try:
+ import bluetooth
+ except ImportError:
+ import lightblueglue as bluetooth
+
+import os
+from .brick import Brick
+
+class BlueSock(object):
+
+ bsize = 118 # Bluetooth socket block size
+ PORT = 1 # Standard NXT rfcomm port
+
+ def __init__(self, host):
+ self.host = host
+ self.sock = None
+ self.debug = False
+
+ def __str__(self):
+ return 'Bluetooth (%s)' % self.host
+
+ def connect(self):
+ if self.debug:
+ print 'Connecting via Bluetooth...'
+ sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
+ sock.connect((self.host, BlueSock.PORT))
+ self.sock = sock
+ if self.debug:
+ print 'Connected.'
+ return Brick(self)
+
+ def close(self):
+ if self.debug:
+ print 'Closing Bluetooth connection...'
+ self.sock.close()
+ if self.debug:
+ print 'Bluetooth connection closed.'
+
+ def send(self, data):
+ if self.debug:
+ print 'Send:',
+ print ':'.join('%02x' % ord(c) for c in data)
+ l0 = len(data) & 0xFF
+ l1 = (len(data) >> 8) & 0xFF
+ d = chr(l0) + chr(l1) + data
+ self.sock.send(d)
+
+ def recv(self):
+ data = self.sock.recv(2)
+ l0 = ord(data[0])
+ l1 = ord(data[1])
+ plen = l0 + (l1 << 8)
+ data = self.sock.recv(plen)
+ if self.debug:
+ print 'Recv:',
+ print ':'.join('%02x' % ord(c) for c in data)
+ return data
+
+def _check_brick(arg, value):
+ return arg is None or arg == value
+
+def find_bricks(host=None, name=None):
+ for h, n in bluetooth.discover_devices(lookup_names=True):
+ if _check_brick(host, h) and _check_brick(name, n):
+ yield BlueSock(h)
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/brick.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/brick.py
new file mode 100644
index 0000000..240ba27
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/brick.py
@@ -0,0 +1,226 @@
+# nxt.brick module -- Classes to represent LEGO Mindstorms NXT bricks
+# Copyright (C) 2006 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, rhn
+# Copyright (C) 2010 rhn, Marcus Wanner, zonedabone
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from time import sleep
+from threading import Lock
+from .error import FileNotFound, ModuleNotFound
+from .telegram import OPCODES, Telegram
+from .sensor import get_sensor
+
+def _make_poller(opcode, poll_func, parse_func):
+ def poll(self, *args, **kwargs):
+ ogram = poll_func(opcode, *args, **kwargs)
+ with self.lock:
+ self.sock.send(str(ogram))
+ igram = Telegram(opcode=opcode, pkt=self.sock.recv())
+ return parse_func(igram)
+ return poll
+
+class _Meta(type):
+ 'Metaclass which adds one method for each telegram opcode'
+
+ def __init__(cls, name, bases, dict):
+ super(_Meta, cls).__init__(name, bases, dict)
+ for opcode in OPCODES:
+ poll_func, parse_func = OPCODES[opcode]
+ m = _make_poller(opcode, poll_func, parse_func)
+ setattr(cls, poll_func.__name__, m)
+
+
+class FileFinder(object):
+ 'A generator to find files on a NXT brick.'
+
+ def __init__(self, brick, pattern):
+ self.brick = brick
+ self.pattern = pattern
+ self.handle = None
+
+ def _close(self):
+ if self.handle is not None:
+ self.brick.close(self.handle)
+ self.handle = None
+
+ def __del__(self):
+ self._close()
+
+ def __iter__(self):
+ results = []
+ self.handle, fname, size = self.brick.find_first(self.pattern)
+ results.append((fname, size))
+ while True:
+ try:
+ handle, fname, size = self.brick.find_next(self.handle)
+ results.append((fname, size))
+ except FileNotFound:
+ self._close()
+ break
+ for result in results:
+ yield result
+
+
+def File(brick, name, mode='r', size=None):
+ """Opens a file for reading/writing. Mode is 'r' or 'w'. If mode is 'w',
+ size must be provided.
+ """
+ if mode == 'w':
+ if size is not None:
+ return FileWriter(brick, name, size)
+ else:
+ return ValueError('Size not specified')
+ elif mode == 'r':
+ return FileReader(brick, name)
+ else:
+ return ValueError('Mode ' + str(mode) + ' not supported')
+
+
+class FileReader(object):
+ """Context manager to read a file on a NXT brick. Do use the iterator or
+ the read() method, but not both at the same time!
+ The iterator returns strings of an arbitrary (short) length.
+ """
+
+ def __init__(self, brick, fname):
+ self.brick = brick
+ self.handle, self.size = brick.open_read(fname)
+
+ def read(self, bytes=None):
+ if bytes is not None:
+ remaining = bytes
+ else:
+ remaining = self.size
+ bsize = self.brick.sock.bsize
+ data = []
+ while remaining > 0:
+ handle, bsize, buffer_ = self.brick.read(self.handle,
+ min(bsize, remaining))
+ remaining -= len(buffer_)
+ data.append(buffer_)
+ return ''.join(data)
+
+ def close(self):
+ if self.handle is not None:
+ self.brick.close(self.handle)
+ self.handle = None
+
+ def __del__(self):
+ self.close()
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, etp, value, tb):
+ self.close()
+
+ def __iter__(self):
+ rem = self.size
+ bsize = self.brick.sock.bsize
+ while rem > 0:
+ handle, bsize, data = self.brick.read(self.handle,
+ min(bsize, rem))
+ yield data
+ rem -= len(data)
+
+
+class FileWriter(object):
+ "Object to write to a file on a NXT brick"
+
+ def __init__(self, brick, fname, size):
+ self.brick = brick
+ self.handle = self.brick.open_write(fname, size)
+ self._position = 0
+ self.size = size
+
+ def __del__(self):
+ self.close()
+
+ def close(self):
+ if self.handle is not None:
+ self.brick.close(self.handle)
+ self.handle = None
+
+ def tell(self):
+ return self._position
+
+ def write(self, data):
+ remaining = len(data)
+ if remaining > self.size - self._position:
+ raise ValueError('Data will not fit into remaining space')
+ bsize = self.brick.sock.bsize
+ data_position = 0
+
+ while remaining > 0:
+ batch_size = min(bsize, remaining)
+ next_data_position = data_position + batch_size
+ buffer_ = data[data_position:next_data_position]
+
+ handle, size = self.brick.write(self.handle, buffer_)
+
+ self._position += batch_size
+ data_position = next_data_position
+ remaining -= batch_size
+
+
+class ModuleFinder(object):
+ 'Iterator to lookup modules on a NXT brick'
+
+ def __init__(self, brick, pattern):
+ self.brick = brick
+ self.pattern = pattern
+ self.handle = None
+
+ def _close(self):
+ if self.handle:
+ self.brick.close(self.handle)
+ self.handle = None
+
+ def __del__(self):
+ self._close()
+
+ def __iter__(self):
+ self.handle, mname, mid, msize, miomap_size = \
+ self.brick.request_first_module(self.pattern)
+ yield (mname, mid, msize, miomap_size)
+ while True:
+ try:
+ handle, mname, mid, msize, miomap_size = \
+ self.brick.request_next_module(
+ self.handle)
+ yield (mname, mid, msize, miomap_size)
+ except ModuleNotFound:
+ self._close()
+ break
+
+
+class Brick(object): #TODO: this begs to have explicit methods
+ 'Main object for NXT Control'
+
+ __metaclass__ = _Meta
+
+ def __init__(self, sock):
+ self.sock = sock
+ self.lock = Lock()
+
+ def play_tone_and_wait(self, frequency, duration):
+ self.play_tone(frequency, duration)
+ sleep(duration / 1000.0)
+
+ def __del__(self):
+ print "Deleting Brick"
+ self.sock.close()
+
+ find_files = FileFinder
+ find_modules = ModuleFinder
+ open_file = File
+ get_sensor = get_sensor
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/direct.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/direct.py
new file mode 100644
index 0000000..50b7f2a
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/direct.py
@@ -0,0 +1,216 @@
+# nxt.direct module -- LEGO Mindstorms NXT direct telegrams
+# Copyright (C) 2006, 2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+'Use for direct communication with the NXT ***EXTREMELY ADVANCED USERS ONLY***'
+
+def _create(opcode):
+ 'Create a simple direct telegram'
+ from telegram import Telegram
+ return Telegram(True, opcode)
+
+def start_program(opcode, fname):
+ tgram = _create(opcode)
+ tgram.add_filename(fname)
+ return tgram
+
+def _parse_simple(tgram):
+ tgram.check_status()
+
+def stop_program(opcode):
+ return _create(opcode)
+
+def play_sound_file(opcode, loop, fname):
+ tgram = _create(opcode)
+ tgram.add_u8(loop)
+ tgram.add_filename(fname)
+ return tgram
+
+def play_tone(opcode, frequency, duration):
+ 'Play a tone at frequency (Hz) for duration (ms)'
+ tgram = _create(opcode)
+ tgram.add_u16(frequency)
+ tgram.add_u16(duration)
+ return tgram
+
+def set_output_state(opcode, port, power, mode, regulation, turn_ratio,
+ run_state, tacho_limit):
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ tgram.add_s8(power)
+ tgram.add_u8(mode)
+ tgram.add_u8(regulation)
+ tgram.add_s8(turn_ratio)
+ tgram.add_u8(run_state)
+ tgram.add_u32(tacho_limit)
+ return tgram
+
+def set_input_mode(opcode, port, sensor_type, sensor_mode):
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ tgram.add_u8(sensor_type)
+ tgram.add_u8(sensor_mode)
+ return tgram
+
+def get_output_state(opcode, port):
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ return tgram
+
+def _parse_get_output_state(tgram):
+ tgram.check_status()
+ port = tgram.parse_u8()
+ power = tgram.parse_s8()
+ mode = tgram.parse_u8()
+ regulation = tgram.parse_u8()
+ turn_ratio = tgram.parse_s8()
+ run_state = tgram.parse_u8()
+ tacho_limit = tgram.parse_u32()
+ tacho_count = tgram.parse_s32()
+ block_tacho_count = tgram.parse_s32()
+ rotation_count = tgram.parse_s32()
+ return (port, power, mode, regulation, turn_ratio, run_state,
+ tacho_limit, tacho_count, block_tacho_count, rotation_count)
+
+def get_input_values(opcode, port):
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ return tgram
+
+def _parse_get_input_values(tgram):
+ tgram.check_status()
+ port = tgram.parse_u8()
+ valid = tgram.parse_u8()
+ calibrated = tgram.parse_u8()
+ sensor_type = tgram.parse_u8()
+ sensor_mode = tgram.parse_u8()
+ raw_ad_value = tgram.parse_u16()
+ normalized_ad_value = tgram.parse_u16()
+ scaled_value = tgram.parse_s16()
+ calibrated_value = tgram.parse_s16()
+ return (port, valid, calibrated, sensor_type, sensor_mode, raw_ad_value,
+ normalized_ad_value, scaled_value, calibrated_value)
+
+def reset_input_scaled_value(opcode, port):
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ return tgram
+
+def message_write(opcode, inbox, message):
+ tgram = _create(opcode)
+ tgram.add_u8(inbox)
+ tgram.add_u8(len(message) + 1)
+ tgram.add_string(len(message), message)
+ tgram.add_u8(0)
+ return tgram
+
+def reset_motor_position(opcode, port, relative):
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ tgram.add_u8(relative)
+ return tgram
+
+def get_battery_level(opcode):
+ return _create(opcode)
+
+def _parse_get_battery_level(tgram):
+ tgram.check_status()
+ millivolts = tgram.parse_u16()
+ return millivolts
+
+def stop_sound_playback(opcode):
+ return _create(opcode)
+
+def keep_alive(opcode):
+ return _create(opcode)
+
+def _parse_keep_alive(tgram):
+ tgram.check_status()
+ sleep_time = tgram.parse_u32()
+ return sleep_time
+
+def ls_get_status(opcode, port):
+ 'Get status of low-speed sensor (ultrasonic)'
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ return tgram
+
+def _parse_ls_get_status(tgram):
+ tgram.check_status()
+ n_bytes = tgram.parse_u8()
+ return n_bytes
+
+def ls_write(opcode, port, tx_data, rx_bytes):
+ 'Write a low-speed command to a sensor (ultrasonic)'
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ tgram.add_u8(len(tx_data))
+ tgram.add_u8(rx_bytes)
+ tgram.add_string(len(tx_data), tx_data)
+ return tgram
+
+def ls_read(opcode, port):
+ 'Read a low-speed sensor value (ultrasonic)'
+ tgram = _create(opcode)
+ tgram.add_u8(port)
+ return tgram
+
+def _parse_ls_read(tgram):
+ tgram.check_status()
+ n_bytes = tgram.parse_u8()
+ contents = tgram.parse_string()
+ return contents[:n_bytes]
+
+def get_current_program_name(opcode):
+ return _create(opcode)
+
+def _parse_get_current_program_name(tgram):
+ tgram.check_status()
+ fname = tgram.parse_string()
+ return fname
+
+def message_read(opcode, remote_inbox, local_inbox, remove):
+ tgram = _create(opcode)
+ tgram.add_u8(remote_inbox)
+ tgram.add_u8(local_inbox)
+ tgram.add_u8(remove)
+ return tgram
+
+def _parse_message_read(tgram):
+ tgram.check_status()
+ local_inbox = tgram.parse_u8()
+ n_bytes = tgram.parse_u8()
+ message = tgram.parse_string()
+ return (local_inbox, message[:n_bytes])
+
+OPCODES = {
+ 0x00: (start_program, _parse_simple),
+ 0x01: (stop_program, _parse_simple),
+ 0x02: (play_sound_file, _parse_simple),
+ 0x03: (play_tone, _parse_simple),
+ 0x04: (set_output_state, _parse_simple),
+ 0x05: (set_input_mode, _parse_simple),
+ 0x06: (get_output_state, _parse_get_output_state),
+ 0x07: (get_input_values, _parse_get_input_values),
+ 0x08: (reset_input_scaled_value, _parse_simple),
+ 0x09: (message_write, _parse_simple),
+ 0x0A: (reset_motor_position, _parse_simple),
+ 0x0B: (get_battery_level, _parse_get_battery_level),
+ 0x0C: (stop_sound_playback, _parse_simple),
+ 0x0D: (keep_alive, _parse_keep_alive),
+ 0x0E: (ls_get_status, _parse_ls_get_status),
+ 0x0F: (ls_write, _parse_simple),
+ 0x10: (ls_read, _parse_ls_read),
+ 0x11: (get_current_program_name, _parse_get_current_program_name),
+ 0x13: (message_read, _parse_message_read),
+}
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/error.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/error.py
new file mode 100644
index 0000000..3d14497
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/error.py
@@ -0,0 +1,87 @@
+# nxt.error module -- LEGO Mindstorms NXT error handling
+# Copyright (C) 2006, 2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+'Declarations for the errors'
+
+class ProtocolError(Exception):
+ pass
+
+class SysProtError(ProtocolError):
+ pass
+
+class FileExistsError(SysProtError):
+ pass
+
+class FileNotFound(SysProtError):
+ pass
+
+class ModuleNotFound(SysProtError):
+ pass
+
+class DirProtError(ProtocolError):
+ pass
+
+class I2CError(DirProtError):
+ pass
+
+class I2CPendingError(I2CError):
+ pass
+
+CODES = {
+ 0x00: None,
+ 0x20: I2CPendingError('Pending communication transaction in progress'),
+ 0x40: DirProtError('Specified mailbox queue is empty'),
+ 0x81: SysProtError('No more handles'),
+ 0x82: SysProtError('No space'),
+ 0x83: SysProtError('No more files'),
+ 0x84: SysProtError('End of file expected'),
+ 0x85: SysProtError('End of file'),
+ 0x86: SysProtError('Not a linear file'),
+ 0x87: FileNotFound('File not found'),
+ 0x88: SysProtError('Handle already closed'),
+ 0x89: SysProtError('No linear space'),
+ 0x8A: SysProtError('Undefined error'),
+ 0x8B: SysProtError('File is busy'),
+ 0x8C: SysProtError('No write buffers'),
+ 0x8D: SysProtError('Append not possible'),
+ 0x8E: SysProtError('File is full'),
+ 0x8F: FileExistsError('File exists'),
+ 0x90: ModuleNotFound('Module not found'),
+ 0x91: SysProtError('Out of bounds'),
+ 0x92: SysProtError('Illegal file name'),
+ 0x93: SysProtError('Illegal handle'),
+ 0xBD: DirProtError('Request failed (i.e. specified file not found)'),
+ 0xBE: DirProtError('Unknown command opcode'),
+ 0xBF: DirProtError('Insane packet'),
+ 0xC0: DirProtError('Data contains out-of-range values'),
+ 0xDD: DirProtError('Communication bus error'),
+ 0xDE: DirProtError('No free memory in communication buffer'),
+ 0xDF: DirProtError('Specified channel/connection is not valid'),
+ 0xE0: I2CError('Specified channel/connection not configured or busy'),
+ 0xEC: DirProtError('No active program'),
+ 0xED: DirProtError('Illegal size specified'),
+ 0xEE: DirProtError('Illegal mailbox queue ID specified'),
+ 0xEF: DirProtError('Attempted to access invalid field of a structure'),
+ 0xF0: DirProtError('Bad input or output specified'),
+ 0xFB: DirProtError('Insufficient memory available'),
+ 0xFF: DirProtError('Bad arguments'),
+}
+
+def check_status(status):
+ if status:
+ ex = CODES.get(status)
+ if ex:
+ raise ex
+ else:
+ raise ProtocolError, status
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/fantomglue.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/fantomglue.py
new file mode 100644
index 0000000..697655f
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/fantomglue.py
@@ -0,0 +1,211 @@
+# fantom.py module -- Glue code from NXT_Python to Fantom, allowing
+# NXT_Python to run on Mac without modification. Supports subset of
+# PyBluez/bluetooth.py used by NXT_Python.
+#
+# Copyright (C) 2011 Tat-Chee Wan
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+import pyfantom
+
+USB_BUFSIZE = 64
+
+RFCOMM=11 # lightblue const
+FANTOM_BT = RFCOMM # For compatibilty with lightblue
+FANTOM_USB = 0
+
+# Bluetooth Iterator
+def discover_devices(lookup_names=False): # parameter is ignored
+ pairs = []
+ for d in pyfantom.NXTIterator(True):
+ # h: host, n: name
+ h = d.get_resource_string()
+ nxt = d.get_nxt()
+ device_info = nxt.get_device_info()
+ n = device_info.name
+ del nxt
+ pairs.append((h, n))
+ return pairs
+
+class BluetoothSocket:
+
+ def __init__(self, proto = FANTOM_BT, _sock=None):
+ # We instantiate a NXT object only when we connect if none supplied
+ self._sock = _sock
+ self._proto = proto
+ self.debug = True
+
+ def __str__(self):
+ return 'FantomGlue BT (%s)' % self.device_name()
+
+ def device_name(self):
+ devinfo = self._sock.get_device_info()
+ return devinfo.name
+
+ def connect(self, addrport):
+ addr, port = addrport
+ if self._sock is None:
+ # Port is ignored
+ paired_addr = pair_bluetooth(addr)
+ if self.debug:
+ print "BT Paired Addr: ", paired_addr
+ self._sock = pyfantom.NXT(paired_addr)
+
+ def send(self, data):
+ return self._sock.write( data )
+
+ def recv(self, numbytes):
+ return self._sock.read( numbytes )
+
+ def close(self):
+ if self._sock is not None:
+ self._sock.close()
+ self._sock = None
+
+ #def __del__(self):
+ # """Destroy interface."""
+ # if self._sock is not None:
+ # del self._sock
+ # if self.debug:
+ # print "NXT object deleted"
+ # else:
+ # if self.debug:
+ # print "No NXT Object when calling __del__ for BluetoothSocket"
+
+class BluetoothError(IOError):
+ pass
+
+def _check_brick(arg, value):
+ return arg is None or arg == value
+
+
+def find_devices(lookup_names=False): # parameter is ignored
+ devicelist = []
+ for d in pyfantom.NXTIterator(False):
+ #name = d.get_name()
+ #addr = d.get_resource_string()
+ #print "NXT addr: ", addr
+ nxt = d.get_nxt()
+ devicelist.append(nxt)
+ return devicelist
+
+# FIXME: The semantics of find_devices is different from discover_devices
+#
+# # h: host, n: name
+# hostmatched = None
+# namematched = None
+# h = d.get_resource_string()
+# nxt = d.get_nxt()
+# device_info = nxt.get_device_info()
+# n = device_info.name
+# if _check_brick(host, h) and _check_brick(name, n):
+# yield nxt
+# else:
+# del nxt # Does not match criteria
+
+class USBSocket:
+
+ def __init__(self, device=None):
+ # We instantiate a NXT object only when we connect if none supplied
+ # FIXME: The addr is not passed in, so we can't actually create a NXT object later
+ #self.device = device
+ self._sock = device
+ self.debug = True
+
+ def __str__(self):
+ return 'FantomGlue USB (%s)' % self.device_name()
+
+ def device_name(self):
+ devinfo = self._sock.get_device_info()
+ return devinfo.name
+
+ def connect(self):
+ if self._sock is None:
+ # Port is ignored
+ if self.debug:
+ print "No NXT object assigned"
+ self._sock = pyfantom.NXT(addr) # FIXME: No address!
+ else:
+ if self.debug:
+ print "Using existing NXT object"
+
+ def send(self, data):
+ return self._sock.write( data )
+
+ def recv(self, numbytes):
+ return self._sock.read( numbytes )
+
+ #def poll_command(self, numbytes):
+ # I'm not sure if this is specific to Direct Command Processing
+ # Or if it refers to any data transmission
+ #print "Bytes Available:", self._sock.bytes_available()
+ #return self._sock.read_buffer( numbytes, 0 )
+
+ def close(self):
+ if self._sock is not None:
+ self._sock.close()
+ self._sock = None
+
+ #def __del__(self):
+ # """Destroy interface."""
+ # if self._sock is not None:
+ # del self._sock
+ # if self.debug:
+ # print "NXT object deleted"
+ # else:
+ # if self.debug:
+ # print "No NXT Object when calling __del__ for USBSocket"
+
+if __name__ == '__main__':
+ #get_info = False
+ get_info = True
+ write_read = True
+ for i in find_devices():
+ if get_info:
+ print " firmware version:", i.get_firmware_version()
+ print " get device info:", i.get_device_info()
+ rs = i.get_resource_string()
+ print " resource string:", rs
+ if write_read:
+ nxt = USBSocket(i)
+ nxt.connect()
+ import struct
+ # Write VERSION SYS_CMD.
+ # Query:
+ # SYS_CMD: 0x01
+ # VERSION: 0x88
+ cmd = struct.pack('2B', 0x01, 0x88)
+ r = nxt.send(cmd)
+ print "wrote", r
+ # Response:
+ # REPLY_CMD: 0x02
+ # VERSION: 0x88
+ # SUCCESS: 0x00
+ # PROTOCOL_VERSION minor
+ # PROTOCOL_VERSION major
+ # FIRMWARE_VERSION minor
+ # FIRMWARE_VERSION major
+ #rep = nxt.recv(USB_BUFSIZE) # Doesn't work if it exceeds buffer content length (exception)
+ #rep = nxt.recv(7) # Works, since reply is 7 bytes
+ rep = nxt.recv(5) # Works, read length < remaining buffer content length
+ print "read", struct.unpack('%dB' % len(rep), rep)
+ rep = nxt.recv(5) # Works, remaining buffer content length is non-zero
+ print "read", struct.unpack('%dB' % len(rep), rep)
+ rep = nxt.recv(1) # Doesn't work if no bytes are in read buffer
+ # Same thing, without response.
+ #cmd = struct.pack('2B', 0x81, 0x88)
+ #r = nxt.send(cmd)
+ #print "wrote", r
+ #rep = nxt.recv(USB_BUFSIZE)
+ #rep = nxt.recv(7)
+ #print "read", struct.unpack('%dB' % len(rep), rep)
+ nxt.close()
+ del nxt
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/lightblueglue.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/lightblueglue.py
new file mode 100644
index 0000000..f2ab92f
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/lightblueglue.py
@@ -0,0 +1,53 @@
+# bluetooth.py module -- Glue code from NXT_Python to Lightblue, allowing
+# NXT_Python to run on Mac without modification. Supports subset of
+# PyBluez/bluetooth.py used by NXT_Python.
+#
+# Copyright (C) 2007 Simon D. Levy
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+import lightblue
+
+RFCOMM=11
+
+def discover_devices(lookup_names=False): # parameter is ignored
+ pairs = []
+ d = lightblue.finddevices()
+ for p in d:
+ h = p[0]
+ n = p[1]
+ pairs.append((h, n))
+ return pairs
+
+class BluetoothSocket:
+
+ def __init__(self, proto = RFCOMM, _sock=None):
+ if _sock is None:
+ _sock = lightblue.socket(proto)
+ self._sock = _sock
+ self._proto = proto
+
+ def connect(self, addrport):
+ addr, port = addrport
+ self._sock.connect( (addr, port ))
+
+ def send(self, data):
+ return self._sock.send( data )
+
+ def recv(self, numbytes):
+ return self._sock.recv( numbytes )
+
+ def close(self):
+ return self._sock.close()
+
+class BluetoothError(IOError):
+ pass
+
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/locator.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/locator.py
new file mode 100644
index 0000000..4adc680
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/locator.py
@@ -0,0 +1,57 @@
+# nxt.locator module -- Locate LEGO Minstorms NXT bricks via USB or Bluetooth
+# Copyright (C) 2006, 2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+class BrickNotFoundError(Exception):
+ pass
+
+class NoBackendError(Exception):
+ pass
+
+def find_bricks(host=None, name=None, silent=False):
+ """Used by find_one_brick to look for bricks ***ADVANCED USERS ONLY***"""
+
+ try:
+ import usbsock
+ usb_available = True
+ socks = usbsock.find_bricks(host, name)
+ for s in socks:
+ yield s
+ except ImportError:
+ usb_available = False
+ import sys
+ if not silent: print >>sys.stderr, "USB module unavailable, not searching there"
+
+ try:
+ import bluesock
+ try:
+ socks = bluesock.find_bricks(host, name)
+ for s in socks:
+ yield s
+ except (bluesock.bluetooth.BluetoothError, IOError): #for cases such as no adapter, bluetooth throws IOError, not BluetoothError
+ pass
+ except ImportError:
+ import sys
+ if not silent: print >>sys.stderr, "Bluetooth module unavailable, not searching there"
+ if not usb_available:
+ raise NoBackendError("Neither USB nor Bluetooth could be used! Did you install PyUSB or PyBluez?")
+
+
+def find_one_brick(host=None, name=None, silent=False):
+ """Use to find one brick. After it returns a usbsock object or a bluesock
+object, it automatically connects to it. The host and name args limit
+the search to a given MAC or brick name. Set silent to True to stop
+nxt-python from printing anything during the search."""
+ for s in find_bricks(host, name, silent):
+ return s.connect()
+ raise BrickNotFoundError
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/motor.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/motor.py
new file mode 100644
index 0000000..f29a5a5
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/motor.py
@@ -0,0 +1,431 @@
+# nxt.motor module -- Class to control LEGO Mindstorms NXT motors
+# Copyright (C) 2006 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, rhn
+# Copyright (C) 2010 rhn
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+"""Use for motor control"""
+
+import time
+import warnings
+
+PORT_A = 0x00
+PORT_B = 0x01
+PORT_C = 0x02
+PORT_ALL = 0xFF
+
+MODE_IDLE = 0x00
+MODE_MOTOR_ON = 0x01
+MODE_BRAKE = 0x02
+MODE_REGULATED = 0x04
+
+REGULATION_IDLE = 0x00
+REGULATION_MOTOR_SPEED = 0x01
+REGULATION_MOTOR_SYNC = 0x02
+
+RUN_STATE_IDLE = 0x00
+RUN_STATE_RAMP_UP = 0x10
+RUN_STATE_RUNNING = 0x20
+RUN_STATE_RAMP_DOWN = 0x40
+
+LIMIT_RUN_FOREVER = 0
+
+class BlockedException(Exception):
+ pass
+
+class OutputState(object):
+ """An object holding the internal state of a motor, not including rotation
+ counters.
+ """
+ def __init__(self, values):
+ (self.power, self.mode, self.regulation,
+ self.turn_ratio, self.run_state, self.tacho_limit) = values
+
+ def to_list(self):
+ """Returns a list of properties that can be used with set_output_state.
+ """
+ return [self.power, self.mode, self.regulation,
+ self.turn_ratio, self.run_state, self.tacho_limit]
+
+ def __str__(self):
+ modes = []
+ if self.mode & MODE_MOTOR_ON:
+ modes.append('on')
+ if self.mode & MODE_BRAKE:
+ modes.append('brake')
+ if self.mode & MODE_REGULATED:
+ modes.append('regulated')
+ if not modes:
+ modes.append('idle')
+ mode = '&'.join(modes)
+ regulation = 'regulation: ' + \
+ ['idle', 'speed', 'sync'][self.regulation]
+ run_state = 'run state: ' + {0: 'idle', 0x10: 'ramp_up',
+ 0x20: 'running', 0x40: 'ramp_down'}[self.run_state]
+ return ', '.join([mode, regulation, str(self.turn_ratio), run_state] + [str(self.tacho_limit)])
+
+
+class TachoInfo:
+ """An object containing the information about the rotation of a motor"""
+ def __init__(self, values):
+ self.tacho_count, self.block_tacho_count, self.rotation_count = values
+
+ def get_target(self, tacho_limit, direction):
+ """Returns a TachoInfo object which corresponds to tacho state after
+ moving for tacho_limit ticks. Direction can be 1 (add) or -1 (subtract)
+ """
+ # TODO: adjust other fields
+ if abs(direction) != 1:
+ raise ValueError('Invalid direction')
+ new_tacho = self.tacho_count + direction * tacho_limit
+ return TachoInfo([new_tacho, None, None])
+
+ def is_greater(self, target, direction):
+ return direction * (self.tacho_count - target.tacho_count) > 0
+
+ def is_near(self, target, threshold):
+ difference = abs(target.tacho_count - self.tacho_count)
+ return difference < threshold
+
+ def __str__(self):
+ return str((self.tacho_count, self.block_tacho_count,
+ self.rotation_count))
+
+
+class SynchronizedTacho(object):
+ def __init__(self, leader_tacho, follower_tacho):
+ self.leader_tacho = leader_tacho
+ self.follower_tacho = follower_tacho
+
+ def get_target(self, tacho_limit, direction):
+ """This method will leave follower's target as None"""
+ leader_tacho = self.leader_tacho.get_target(tacho_limit, direction)
+ return SynchronizedTacho(leader_tacho, None)
+
+ def is_greater(self, other, direction):
+ return self.leader_tacho.is_greater(other.leader_tacho, direction)
+
+ def is_near(self, other, threshold):
+ return self.leader_tacho.is_near(other.leader_tacho, threshold)
+
+ def __str__(self):
+ if self.follower_tacho is not None:
+ t2 = str(self.follower_tacho.tacho_count)
+ else:
+ t2 = 'None'
+ t1 = str(self.leader_tacho.tacho_count)
+ return 'tacho: ' + t1 + ' ' + t2
+
+
+def get_tacho_and_state(values):
+ """A convenience function. values is the list of values from
+ get_output_state. Returns both OutputState and TachoInfo.
+ """
+ return OutputState(values[1:7]), TachoInfo(values[7:])
+
+
+class BaseMotor(object):
+ """Base class for motors"""
+ debug = 0
+ def _debug_out(self, message):
+ if self.debug:
+ print message
+
+ def turn(self, power, tacho_units, brake=True, timeout=1, emulate=True):
+ """Use this to turn a motor. The motor will not stop until it turns the
+ desired distance. Accuracy is much better over a USB connection than
+ with bluetooth...
+ power is a value between -127 and 128 (an absolute value greater than
+ 64 is recommended)
+ tacho_units is the number of degrees to turn the motor. values smaller
+ than 50 are not recommended and may have strange results.
+ brake is whether or not to hold the motor after the function exits
+ (either by reaching the distance or throwing an exception).
+ timeout is the number of seconds after which a BlockedException is
+ raised if the motor doesn't turn
+ emulate is a boolean value. If set to False, the motor is aware of the
+ tacho limit. If True, a run() function equivalent is used.
+ Warning: motors remember their positions and not using emulate
+ may lead to strange behavior, especially with synced motors
+ """
+
+ tacho_limit = tacho_units
+
+ if tacho_limit < 0:
+ raise ValueError, "tacho_units must be greater than 0!"
+
+ if self.method == 'bluetooth':
+ threshold = 70
+ elif self.method == 'usb':
+ threshold = 5
+ else:
+ threshold = 30 #compromise
+
+ tacho = self.get_tacho()
+ state = self._get_new_state()
+
+ # Update modifiers even if they aren't used, might have been changed
+ state.power = power
+ if not emulate:
+ state.tacho_limit = tacho_limit
+
+ self._debug_out('Updating motor information...')
+ self._set_state(state)
+
+ direction = 1 if power > 0 else -1
+ self._debug_out('tachocount: ' + str(tacho))
+ current_time = time.time()
+ tacho_target = tacho.get_target(tacho_limit, direction)
+
+ blocked = False
+ try:
+ while True:
+ time.sleep(self._eta(tacho, tacho_target, power) / 2)
+
+ if not blocked: # if still blocked, don't reset the counter
+ last_tacho = tacho
+ last_time = current_time
+
+ tacho = self.get_tacho()
+ current_time = time.time()
+ blocked = self._is_blocked(tacho, last_tacho, direction)
+ if blocked:
+ self._debug_out(('not advancing', last_tacho, tacho))
+ # the motor can be up to 80+ degrees in either direction from target when using bluetooth
+ if current_time - last_time > timeout:
+ if tacho.is_near(tacho_target, threshold):
+ break
+ else:
+ raise BlockedException("Blocked!")
+ else:
+ self._debug_out(('advancing', last_tacho, tacho))
+ if tacho.is_near(tacho_target, threshold) or tacho.is_greater(tacho_target, direction):
+ break
+ finally:
+ if brake:
+ self.brake()
+ else:
+ self.idle()
+
+
+class Motor(BaseMotor):
+ def __init__(self, brick, port):
+ self.brick = brick
+ self.port = port
+ self._read_state()
+ self.sync = 0
+ self.turn_ratio = 0
+ if str(type(self.brick.sock)) == "<class 'nxt.bluesock.BlueSock'>":
+ self.method = 'bluetooth'
+ elif str(type(self.brick.sock)) == "<class 'nxt.usbsock.USBSock'>":
+ self.method = 'usb'
+ else:
+ print "Warning: Socket object does not of a known type."
+ print "The name is: " + str(type(self.brick.sock))
+ print "Please report this problem to the developers!"
+ print "For now, turn() accuracy will not be optimal."
+ print "Continuing happily..."
+ self.method = None
+
+ def _set_state(self, state):
+ self._debug_out('Setting brick output state...')
+ list_state = [self.port] + state.to_list()
+ self.brick.set_output_state(*list_state)
+ self._debug_out(state)
+ self._state = state
+ self._debug_out('State set.')
+
+ def _read_state(self):
+ self._debug_out('Getting brick output state...')
+ values = self.brick.get_output_state(self.port)
+ self._debug_out('State got.')
+ self._state, tacho = get_tacho_and_state(values)
+ return self._state, tacho
+
+ #def get_tacho_and_state here would allow tacho manipulation
+
+ def _get_state(self):
+ """Returns a copy of the current motor state for manipulation."""
+ return OutputState(self._state.to_list())
+
+ def _get_new_state(self):
+ state = self._get_state()
+ if self.sync:
+ state.mode = MODE_MOTOR_ON | MODE_REGULATED
+ state.regulation = REGULATION_MOTOR_SYNC
+ state.turn_ratio = self.turn_ratio
+ else:
+ state.mode = MODE_MOTOR_ON | MODE_REGULATED
+ state.regulation = REGULATION_MOTOR_SPEED
+ state.run_state = RUN_STATE_RUNNING
+ state.tacho_limit = LIMIT_RUN_FOREVER
+ return state
+
+ def get_tacho(self):
+ return self._read_state()[1]
+
+ def reset_position(self, relative):
+ """Resets the counters. Relative can be True or False"""
+ self.brick.reset_motor_position(self.port, relative)
+
+ def run(self, power=100, regulated=False):
+ '''Tells the motor to run continuously. If regulated is True, then the
+ synchronization starts working.
+ '''
+ state = self._get_new_state()
+ state.power = power
+ if not regulated:
+ state.mode = MODE_MOTOR_ON
+ self._set_state(state)
+
+ def brake(self):
+ """Holds the motor in place"""
+ state = self._get_new_state()
+ state.power = 0
+ state.mode = MODE_MOTOR_ON | MODE_BRAKE | MODE_REGULATED
+ self._set_state(state)
+
+ def idle(self):
+ '''Tells the motor to stop whatever it's doing. It also desyncs it'''
+ state = self._get_new_state()
+ state.power = 0
+ state.mode = MODE_IDLE
+ state.regulation = REGULATION_IDLE
+ state.run_state = RUN_STATE_IDLE
+ self._set_state(state)
+
+ def weak_turn(self, power, tacho_units):
+ """Tries to turn a motor for the specified distance. This function
+ returns immediately, and it's not guaranteed that the motor turns that
+ distance. This is an interface to use tacho_limit without
+ REGULATION_MODE_SPEED
+ """
+ tacho_limit = tacho_units
+ tacho = self.get_tacho()
+ state = self._get_new_state()
+
+ # Update modifiers even if they aren't used, might have been changed
+ state.mode = MODE_MOTOR_ON
+ state.regulation = REGULATION_IDLE
+ state.power = power
+ state.tacho_limit = tacho_limit
+
+ self._debug_out('Updating motor information...')
+ self._set_state(state)
+
+ def _eta(self, current, target, power):
+ """Returns time in seconds. Do not trust it too much"""
+ tacho = abs(current.tacho_count - target.tacho_count)
+ return (float(tacho) / abs(power)) / 5
+
+ def _is_blocked(self, tacho, last_tacho, direction):
+ """Returns if any of the engines is blocked"""
+ return direction * (last_tacho.tacho_count - tacho.tacho_count) >= 0
+
+
+class SynchronizedMotors(BaseMotor):
+ """The object used to make two motors run in sync. Many objects may be
+ present at the same time but they can't be used at the same time.
+ Warning! Movement methods reset tacho counter.
+ THIS CODE IS EXPERIMENTAL!!!
+ """
+ def __init__(self, leader, follower, turn_ratio):
+ """Turn ratio can be >= 0 only! If you want to have it reversed,
+ change motor order.
+ """
+ if follower.brick != leader.brick:
+ raise ValueError('motors belong to different bricks')
+ self.leader = leader
+ self.follower = follower
+ self.method = self.leader.method #being from the same brick, they both have the same com method.
+
+ if turn_ratio < 0:
+ raise ValueError('Turn ratio <0. Change motor order instead!')
+
+ if self.leader.port == self.follower.port:
+ raise ValueError("The same motor passed twice")
+ elif self.leader.port > self.follower.port:
+ self.turn_ratio = turn_ratio
+ else:
+ self._debug_out('reversed')
+ self.turn_ratio = -turn_ratio
+
+ def _get_new_state(self):
+ return self.leader._get_new_state()
+
+ def _set_state(self, state):
+ self.leader._set_state(state)
+ self.follower._set_state(state)
+
+ def get_tacho(self):
+ leadertacho = self.leader.get_tacho()
+ followertacho = self.follower.get_tacho()
+ return SynchronizedTacho(leadertacho, followertacho)
+
+ def reset_position(self, relative):
+ """Resets the counters. Relative can be True or False"""
+ self.leader.reset_position(relative)
+ self.follower.reset_position(relative)
+
+ def _enable(self): # This works as expected. I'm not sure why.
+ #self._disable()
+ self.reset_position(True)
+ self.leader.sync = True
+ self.follower.sync = True
+ self.leader.turn_ratio = self.turn_ratio
+ self.follower.turn_ratio = self.turn_ratio
+
+ def _disable(self): # This works as expected. (tacho is reset ok)
+ self.leader.sync = False
+ self.follower.sync = False
+ #self.reset_position(True)
+ self.leader.idle()
+ self.follower.idle()
+ #self.reset_position(True)
+
+ def run(self, power=100):
+ """Warning! After calling this method, make sure to call idle. The
+ motors are reported to behave wildly otherwise.
+ """
+ self._enable()
+ self.leader.run(power, True)
+ self.follower.run(power, True)
+
+ def brake(self):
+ self._disable() # reset the counters
+ self._enable()
+ self.leader.brake() # brake both motors at the same time
+ self.follower.brake()
+ self._disable() # now brake as usual
+ self.leader.brake()
+ self.follower.brake()
+
+ def idle(self):
+ self._disable()
+
+ def turn(self, power, tacho_units, brake=True, timeout=1):
+ self._enable()
+ # non-emulation is a nightmare, tacho is being counted differently
+ try:
+ if power < 0:
+ self.leader, self.follower = self.follower, self.leader
+ BaseMotor.turn(self, power, tacho_units, brake, timeout, emulate=True)
+ finally:
+ if power < 0:
+ self.leader, self.follower = self.follower, self.leader
+
+ def _eta(self, tacho, target, power):
+ return self.leader._eta(tacho.leader_tacho, target.leader_tacho, power)
+
+ def _is_blocked(self, tacho, last_tacho, direction):
+ # no need to check both, they're synced
+ return self.leader._is_blocked(tacho.leader_tacho, last_tacho.leader_tacho, direction)
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/pyusbglue.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/pyusbglue.py
new file mode 100644
index 0000000..b140dfe
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/pyusbglue.py
@@ -0,0 +1,82 @@
+# pyusbglue.py module -- Glue code from NXT_Python to libusb for USB access.
+#
+# Copyright (C) 2011 Tat-Chee Wan
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+import usb
+
+USB_BUFSIZE = 64
+
+ID_VENDOR_LEGO = 0x0694
+ID_PRODUCT_NXT = 0x0002
+
+class USBSocket:
+ bsize = 60 # USB socket block size
+
+ def __init__(self, device):
+ self.device = device
+ self.handle = None
+ self.debug = False
+
+ def device_name(self):
+ return self.device.filename
+
+ def connect(self):
+ 'Use to connect to NXT.'
+ if self.debug:
+ print 'PyUSB Connecting...'
+ config = self.device.configurations[0]
+ iface = config.interfaces[0][0]
+ self.blk_out, self.blk_in = iface.endpoints
+ self.handle = self.device.open()
+ self.handle.setConfiguration(1)
+ self.handle.claimInterface(0)
+ self.handle.reset()
+ if self.debug:
+ print 'Connected.'
+
+ def close(self):
+ 'Use to close the connection.'
+ if self.debug:
+ print 'Closing USB connection...'
+ self.device = None
+ self.handle = None
+ self.blk_out = None
+ self.blk_in = None
+ if self.debug:
+ print 'USB connection closed.'
+
+ def send(self, data):
+ 'Use to send raw data over USB connection ***ADVANCED USERS ONLY***'
+ if self.debug:
+ print 'Send:',
+ print ':'.join('%02x' % ord(c) for c in data)
+ self.handle.bulkWrite(self.blk_out.address, data)
+
+ def recv(self, numbytes):
+ 'Use to recieve raw data over USB connection ***ADVANCED USERS ONLY***'
+ data = self.handle.bulkRead(self.blk_in.address, numbytes)
+ if self.debug:
+ print 'Recv:',
+ print ':'.join('%02x' % (c & 0xFF) for c in data)
+ # NOTE: bulkRead returns a tuple of ints ... make it sane
+ return ''.join(chr(d & 0xFF) for d in data)
+
+def find_devices(lookup_names=False): # parameter is ignored
+ devicelist = []
+ for bus in usb.busses():
+ for device in bus.devices:
+ if device.idVendor == ID_VENDOR_LEGO and device.idProduct == ID_PRODUCT_NXT:
+ devicelist.append(device)
+ return devicelist
+
+
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py
new file mode 100644
index 0000000..8f2a337
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py
@@ -0,0 +1,50 @@
+# nxt.sensor module -- Classes to read LEGO Mindstorms NXT sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from .common import *
+from .analog import BaseAnalogSensor
+from .digital import BaseDigitalSensor, find_class
+from .generic import Touch, Light, Sound, Ultrasonic, Color20
+import mindsensors
+MSSumoEyes = mindsensors.SumoEyes
+MSCompassv2 = mindsensors.Compassv2
+MSDIST = mindsensors.DIST
+MSRTC = mindsensors.RTC
+MSACCL = mindsensors.ACCL
+MSServo = mindsensors.Servo
+MSMTRMUX = mindsensors.MTRMUX
+MSLineLeader = mindsensors.LineLeader
+MSMMX = mindsensors.MMX
+MSPS2 = mindsensors.PS2
+MSHID = mindsensors.HID
+import hitechnic
+HTCompass = hitechnic.Compass
+HTAccelerometer = hitechnic.Accelerometer
+HTGyro = hitechnic.Gyro
+HTColorv2 = hitechnic.Colorv2
+HTEOPD = hitechnic.EOPD
+HTIRReceiver = hitechnic.IRReceiver
+HTIRSeekerv2 = hitechnic.IRSeekerv2
+HTPrototype = hitechnic.Prototype
+
+
+def get_sensor(brick, port):
+ """Tries to detect the sensor type and return the correct sensor
+object. Does not work for sensors with no identification information (such as
+all analog sensors or the MindSensors RTC.
+ """
+ base_sensor = BaseDigitalSensor(brick, port, False)
+ info = base_sensor.get_sensor_info()
+ return find_class(info)(brick, port, check_compatible=False)
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py
new file mode 100644
index 0000000..e8d9b7b
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py
@@ -0,0 +1,41 @@
+# nxt.sensor.analog module -- submodule for use with analog sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from common import *
+
+
+class RawReading: # can be converted to the old version
+ """A pseudo-structure holding the raw sensor values as returned by the NXT
+ brick.
+ """
+ def __init__(self, values):
+ (self.port, self.valid, self.calibrated, self.sensor_type, self.mode,
+ self.raw_ad_value, self.normalized_ad_value, self.scaled_value,
+ self.calibrated_value) = values
+
+ def __repr__(self):
+ return str((self.port, self.valid, self.calibrated, self.sensor_type, self.mode,
+ self.raw_ad_value, self.normalized_ad_value, self.scaled_value,
+ self.calibrated_value))
+
+
+class BaseAnalogSensor(Sensor):
+ """Object for analog sensors."""
+ def get_input_values(self):
+ """Returns the raw sensor values as returned by the NXT brick."""
+ return RawReading(self.brick.get_input_values(self.port))
+
+ def reset_input_scaled_value(self):
+ self.brick.reset_input_scaled_value()
+
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py
new file mode 100644
index 0000000..5afd6c8
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py
@@ -0,0 +1,67 @@
+# nxt.sensor.common module -- submodule with stuff useful in all sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+PORT_1 = 0x00
+PORT_2 = 0x01
+PORT_3 = 0x02
+PORT_4 = 0x03
+
+class Type(object):
+ 'Namespace for enumeration of the type of sensor'
+ # NOTE: just a namespace (enumeration)
+ NO_SENSOR = 0x00
+ SWITCH = 0x01 # Touch sensor
+ TEMPERATURE = 0x02
+ REFLECTION = 0x03
+ ANGLE = 0x04
+ LIGHT_ACTIVE = 0x05 # Light sensor (illuminated)
+ LIGHT_INACTIVE = 0x06 # Light sensor (ambient)
+ SOUND_DB = 0x07 # Sound sensor (unadjusted)
+ SOUND_DBA = 0x08 # Sound sensor (adjusted)
+ CUSTOM = 0x09
+ LOW_SPEED = 0x0A
+ LOW_SPEED_9V = 0x0B # Low-speed I2C (Ultrasonic sensor)
+ HIGH_SPEED = 0x0C #Possibly other mode for I2C; may be used by future sensors.
+ COLORFULL = 0x0D #NXT 2.0 color sensor in full color mode (color sensor mode)
+ COLORRED = 0x0E #NXT 2.0 color sensor with red light on (light sensor mode)
+ COLORGREEN = 0x0F #NXT 2.0 color sensor with green light on (light sensor mode)
+ COLORBLUE = 0x10 #NXT 2.0 color sensor in with blue light on (light sensor mode)
+ COLORNONE = 0x11 #NXT 2.0 color sensor in with light off (light sensor mode)
+ COLOREXIT = 0x12 #NXT 2.0 color sensor internal state (not sure what this is for yet)
+
+
+class Mode(object):
+ 'Namespace for enumeration of the mode of sensor'
+ # NOTE: just a namespace (enumeration)
+ RAW = 0x00
+ BOOLEAN = 0x20
+ TRANSITION_CNT = 0x40
+ PERIOD_COUNTER = 0x60
+ PCT_FULL_SCALE = 0x80
+ CELSIUS = 0xA0
+ FAHRENHEIT = 0xC0
+ ANGLE_STEPS = 0xE0
+ MASK = 0xE0
+ MASK_SLOPE = 0x1F # Why isn't this slope thing documented?
+
+
+class Sensor(object):
+ 'Main sensor object'
+
+ def __init__(self, brick, port):
+ self.brick = brick
+ self.port = port
+
+ def set_input_mode(self, type_, mode):
+ self.brick.set_input_mode(self.port, type_, mode)
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py
new file mode 100644
index 0000000..dbc730f
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py
@@ -0,0 +1,227 @@
+# nxt.sensor module -- Classes to read LEGO Mindstorms NXT sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010,2011 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from nxt.error import I2CError, I2CPendingError, DirProtError
+
+from common import *
+from time import sleep, time
+import struct
+
+
+class SensorInfo:
+ def __init__(self, version, product_id, sensor_type):
+ self.version = version
+ self.product_id = product_id
+ self.sensor_type = sensor_type
+
+ def clarifybinary(self, instr, label):
+ outstr = ''
+ outstr += (label + ': `' + instr + '`\n')
+ for char in instr:
+ outstr += (hex(ord(char))+', ')
+ outstr += ('\n')
+ return outstr
+
+ def __str__(self):
+ outstr = ''
+ outstr += (self.clarifybinary(str(self.version), 'Version'))
+ outstr += (self.clarifybinary(str(self.product_id), 'Product ID'))
+ outstr += (self.clarifybinary(str(self.sensor_type), 'Type'))
+ return outstr
+
+class BaseDigitalSensor(Sensor):
+ """Object for digital sensors. I2C_ADDRESS is the dictionary storing name
+ to i2c address mappings. It should be updated in every subclass. When
+ subclassing this class, make sure to call add_compatible_sensor to add
+ compatible sensor data.
+ """
+ I2C_DEV = 0x02
+ I2C_ADDRESS = {'version': (0x00, '8s'),
+ 'product_id': (0x08, '8s'),
+ 'sensor_type': (0x10, '8s'),
+# 'factory_zero': (0x11, 1), # is this really correct?
+ 'factory_scale_factor': (0x12, 'B'),
+ 'factory_scale_divisor': (0x13, 'B'),
+ }
+
+ def __init__(self, brick, port, check_compatible=True):
+ """Creates a BaseDigitalSensor. If check_compatible is True, queries
+ the sensor for its name, and if a wrong sensor class was used, prints
+ a warning.
+ """
+ super(BaseDigitalSensor, self).__init__(brick, port)
+ self.set_input_mode(Type.LOW_SPEED_9V, Mode.RAW)
+ self.last_poll = time()
+ self.poll_delay = 0.01
+ sleep(0.1) # Give I2C time to initialize
+ #Don't do type checking if this class has no compatible sensors listed.
+ try: self.compatible_sensors
+ except AttributeError: check_compatible = False
+ if check_compatible:
+ sensor = self.get_sensor_info()
+ if not sensor in self.compatible_sensors:
+ print ('WARNING: Wrong sensor class chosen for sensor ' +
+ str(sensor.product_id) + ' on port ' + str(port) + '. ' + """
+You may be using the wrong type of sensor or may have connected the cable
+incorrectly. If you are sure you're using the correct sensor class for the
+sensor, this message is likely in error and you should disregard it and file a
+bug report, including the output of get_sensor_info(). This message can be
+suppressed by passing "check_compatible=False" when creating the sensor object.""")
+
+ def _ls_get_status(self, n_bytes):
+ for n in range(10):
+ try:
+ b = self.brick.ls_get_status(self.port)
+ if b >= n_bytes:
+ return b
+ except I2CPendingError:
+ pass
+ raise I2CError, 'ls_get_status timeout'
+
+ def _i2c_command(self, address, value, format):
+ """Writes an i2c value to the given address. value must be a string. value is
+ a tuple of values corresponding to the given format.
+ """
+ value = struct.pack(format, *value)
+ msg = chr(self.I2C_DEV) + chr(address) + value
+ if self.last_poll+self.poll_delay > time():
+ diff = time() - self.last_poll
+ sleep(self.poll_delay - diff)
+ self.last_poll = time()
+ self.brick.ls_write(self.port, msg, 0)
+
+ def _i2c_query(self, address, format):
+ """Reads an i2c value from given address, and returns a value unpacked
+ according to the given format. Format is the same as in the struct
+ module. See http://docs.python.org/library/struct.html#format-strings
+ """
+ n_bytes = struct.calcsize(format)
+ msg = chr(self.I2C_DEV) + chr(address)
+ if self.last_poll+self.poll_delay > time():
+ diff = time() - self.last_poll
+ sleep(self.poll_delay - diff)
+ self.last_poll = time()
+ self.brick.ls_write(self.port, msg, n_bytes)
+ try:
+ self._ls_get_status(n_bytes)
+ finally:
+ #we should clear the buffer no matter what happens
+ data = self.brick.ls_read(self.port)
+ if len(data) < n_bytes:
+ raise I2CError, 'Read failure: Not enough bytes'
+ data = struct.unpack(format, data[-n_bytes:])
+ return data
+
+ def read_value(self, name):
+ """Reads a value from the sensor. Name must be a string found in
+ self.I2C_ADDRESS dictionary. Entries in self.I2C_ADDRESS are in the
+ name: (address, format) form, with format as in the struct module.
+ Be careful on unpacking single variables - struct module puts them in
+ tuples containing only one element.
+ """
+ address, fmt = self.I2C_ADDRESS[name]
+ for n in range(3):
+ try:
+ return self._i2c_query(address, fmt)
+ except DirProtError:
+ pass
+ raise I2CError, "read_value timeout"
+
+ def write_value(self, name, value):
+ """Writes value to the sensor. Name must be a string found in
+ self.I2C_ADDRESS dictionary. Entries in self.I2C_ADDRESS are in the
+ name: (address, format) form, with format as in the struct module.
+ value is a tuple of values corresponding to the format from
+ self.I2C_ADDRESS dictionary.
+ """
+ address, fmt = self.I2C_ADDRESS[name]
+ self._i2c_command(address, value, fmt)
+
+ def get_sensor_info(self):
+ version = self.read_value('version')[0].split('\0')[0]
+ product_id = self.read_value('product_id')[0].split('\0')[0]
+ sensor_type = self.read_value('sensor_type')[0].split('\0')[0]
+ return SensorInfo(version, product_id, sensor_type)
+
+ @classmethod
+ def add_compatible_sensor(cls, version, product_id, sensor_type):
+ """Adds an entry in the compatibility table for the sensor. If version
+ is None, then it's the default class for this model. If product_id is
+ None, then this is the default class for this vendor.
+ """
+ try:
+ cls.compatible_sensors
+ except AttributeError:
+ cls.compatible_sensors = []
+ finally:
+ cls.compatible_sensors.append(SCompatibility(version, product_id,
+ sensor_type))
+ add_mapping(cls, version, product_id, sensor_type)
+
+
+class SCompatibility(SensorInfo):
+ """An object that helps manage the sensor mappings"""
+ def __eq__(self, other):
+ if self.product_id is None:
+ return self.product_id == other.product_id
+ elif self.version is None:
+ return (self.product_id == other.product_id and
+ self.sensor_type == other.sensor_type)
+ else:
+ return (self.version == other.version and
+ self.product_id == other.product_id and
+ self.sensor_type == other.sensor_type)
+
+sensor_mappings = {}
+
+
+def add_mapping(cls, version, product_id, sensor_type):
+ "None means any other value"
+ if product_id not in sensor_mappings:
+ sensor_mappings[product_id] = {}
+ models = sensor_mappings[product_id]
+
+ if sensor_type is None:
+ if sensor_type in models:
+ raise ValueError('Already registered!')
+ models[sensor_type] = cls
+ return
+
+ if sensor_type not in models:
+ models[sensor_type] = {}
+ versions = models[sensor_type]
+
+ if version in versions:
+ raise ValueError('Already registered!')
+ else:
+ versions[version] = cls
+
+
+class SearchError(Exception):
+ pass
+
+
+def find_class(info):
+ """Returns an appropriate class for the given SensorInfo"""
+ dic = sensor_mappings
+ for val, msg in zip((info.product_id, info.sensor_type, info.version),
+ ('Vendor', 'Model', 'Version')):
+ if val in dic:
+ dic = dic[val]
+ elif None in dic:
+ dic = dic[None]
+ else:
+ raise SearchError(msg + ' not found')
+ return dic[info.sensor_type][None]
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py
new file mode 100644
index 0000000..b3d792f
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py
@@ -0,0 +1,154 @@
+# nxt.sensor.generic module -- Classes to read LEGO Mindstorms NXT sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 melducky, Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from .common import *
+from .digital import BaseDigitalSensor
+from .analog import BaseAnalogSensor
+
+
+class Touch(BaseAnalogSensor):
+ """The LEGO touch sensor"""
+
+ def __init__(self, brick, port):
+ super(Touch, self).__init__(brick, port)
+ self.set_input_mode(Type.SWITCH, Mode.BOOLEAN)
+
+ def is_pressed(self):
+ return bool(self.get_input_values().scaled_value)
+
+ get_sample = is_pressed
+
+
+class Light(BaseAnalogSensor):
+ """Object for light sensors. It automatically turns off light when it's not
+ used.
+ """
+ def __init__(self, brick, port, illuminated=True):
+ super(Light, self).__init__(brick, port)
+
+ def set_illuminated(self, active):
+ if active:
+ type_ = Type.LIGHT_ACTIVE
+ else:
+ type_ = Type.LIGHT_INACTIVE
+ self.set_input_mode(type_, Mode.RAW)
+
+ def get_lightness(self):
+ return self.get_input_values().scaled_value
+
+ get_sample = get_lightness
+
+
+class Sound(BaseAnalogSensor):
+ 'Object for sound sensors'
+
+ def __init__(self, brick, port, adjusted=True):
+ super(Sound, self).__init__(brick, port)
+ self.set_adjusted(adjusted)
+
+ def set_adjusted(self, active):
+ if active:
+ type_ = Type.SOUND_DBA
+ else:
+ type_ = Type.SOUND_DB
+ self.set_input_mode(type_, Mode.RAW)
+
+ def get_loudness(self):
+ return self.get_input_values().scaled_value
+
+ get_sample = get_loudness
+
+
+class Ultrasonic(BaseDigitalSensor):
+ """Object for ultrasonic sensors"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'measurement_units': (0x14, '7s'),
+ 'continuous_measurement_interval': (0x40, 'B'),
+ 'command': (0x41, 'B'),
+ 'measurement_byte_0': (0x42, 'B'),
+ 'measurements': (0x42, '8B'),
+ 'actual_scale_factor': (0x51, 'B'),
+ 'actual_scale_divisor': (0x52, 'B'),
+ })
+
+ class Commands:
+ 'These are for passing to command()'
+ OFF = 0x00
+ SINGLE_SHOT = 0x01
+ CONTINUOUS_MEASUREMENT = 0x02
+ EVENT_CAPTURE = 0x03 #Optimize results when other Ultrasonic sensors running
+ REQUEST_WARM_RESET = 0x04
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Ultrasonic, self).__init__(brick, port, check_compatible)
+ self.set_input_mode(Type.LOW_SPEED_9V, Mode.RAW)
+
+ def get_distance(self):
+ 'Function to get data from the ultrasonic sensor'
+ return self.read_value('measurement_byte_0')[0]
+
+ get_sample = get_distance
+
+ def get_measurement_units(self):
+ return self.read_value('measurement_units')[0].split('\0')[0]
+
+ def get_all_measurements(self):
+ "Returns all the past readings in measurement_byte_0 through 7"
+ return self.read_value('measurements')
+
+ def get_measurement_no(self, number):
+ "Returns measurement_byte_number"
+ if not 0 <= number < 8:
+ raise ValueError('Measurements are numbered 0 to 7, not ' + str(number))
+ base_address, format = self.I2C_ADDRESS['measurement_byte_0']
+ return self._i2c_query(base_address + number, format)[0]
+
+ def command(self, command):
+ self.write_value('command', (command, ))
+
+ def get_interval(self):
+ 'Get the sample interval for continuous measurement mode -- Unknown units'
+ return self.read_value('continuous_measurement_interval')
+
+ def set_interval(self, interval):
+ """Set the sample interval for continuous measurement mode.
+Unknown units; default is 1"""
+ self.write_value('continuous_measurement_interval', interval)
+
+Ultrasonic.add_compatible_sensor(None, 'LEGO', 'Sonar') #Tested with version 'V1.0'
+
+
+class Color20(BaseAnalogSensor):
+ def __init__(self, brick, port):
+ super(Color20, self).__init__(brick, port)
+ self.set_light_color(Type.COLORFULL)
+
+ def set_light_color(self, color):
+ """color should be one of the COLOR* Type namespace values, e.g. Type.COLORBLUE"""
+ self.set_input_mode(color, Mode.RAW)
+
+ def get_light_color(self):
+ """Returns one of the COLOR* Type namespace values, e.g. Type.COLORRED"""
+ return self.get_input_values().sensor_type
+
+ def get_reflected_light(self, color):
+ self.set_light_color(color)
+ return self.get_input_values().scaled_value
+
+ def get_color(self):
+ self.get_reflected_light(Type.COLORFULL)
+ return self.get_input_values().scaled_value
+
+ get_sample = get_color
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
new file mode 100644
index 0000000..3c152d3
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
@@ -0,0 +1,611 @@
+# nxt.sensor.hitechnic module -- Classes to read HiTechnic sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 rhn, Marcus Wanner, melducky, Samuel Leeman-Munk
+# Copyright (C) 2011 jerradgenson, Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from .common import *
+from .digital import BaseDigitalSensor
+from .analog import BaseAnalogSensor
+
+
+class Compass(BaseDigitalSensor):
+ """Hitechnic compass sensor."""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'mode': (0x41, 'B'),
+ 'heading': (0x42, 'B'),
+ 'adder' : (0x43, 'B'),
+ })
+
+ class Modes:
+ MEASUREMENT = 0x00
+ CALIBRATION = 0x43
+ CALIBRATION_FAILED = 0x02
+
+ def get_heading(self):
+ """Returns heading from North in degrees."""
+
+ two_degree_heading = self.read_value('heading')[0]
+ adder = self.read_value('adder')[0]
+ heading = two_degree_heading * 2 + adder
+
+ return heading
+
+ get_sample = get_heading
+
+ def get_relative_heading(self,target=0):
+ rheading = self.get_sample()-target
+ if rheading > 180:
+ rheading -= 360
+ elif rheading < -180:
+ rheading += 360
+ return rheading
+
+ def is_in_range(self,minval,maxval):
+ """This deserves a little explanation:
+if max > min, it's straightforward, but
+if min > max, it switches the values of max and min
+and returns true if heading is NOT between the new max and min
+ """
+ if minval > maxval:
+ (maxval,minval) = (minval,maxval)
+ inverted = True
+ else:
+ inverted = False
+ heading = self.get_sample()
+ in_range = (heading > minval) and (heading < maxval)
+ #an xor handles the reversal
+ #a faster, more compact way of saying
+ #if !reversed return in_range
+ #if reversed return !in_range
+ return bool(inverted) ^ bool(in_range)
+
+ def get_mode(self):
+ return self.read_value('mode')[0]
+
+ def set_mode(self, mode):
+ if mode != self.Modes.MEASUREMENT and \
+ mode != self.Modes.CALIBRATION:
+ raise ValueError('Invalid mode specified: ' + str(mode))
+ self.write_value('mode', (mode, ))
+
+Compass.add_compatible_sensor(None, 'HiTechnc', 'Compass ') #Tested with version '\xfdV1.23 '
+Compass.add_compatible_sensor(None, 'HITECHNC', 'Compass ') #Tested with version '\xfdV2.1 '
+
+
+class Accelerometer(BaseDigitalSensor):
+ 'Object for Accelerometer sensors. Thanks to Paulo Vieira.'
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'x_axis_high': (0x42, 'b'),
+ 'y_axis_high': (0x43, 'b'),
+ 'z_axis_high': (0x44, 'b'),
+ 'xyz_short': (0x42, '3b'),
+ 'all_data': (0x42, '3b3B')
+ })
+
+ class Acceleration:
+ def __init__(self, x, y, z):
+ self.x, self.y, self.z = x, y, z
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Accelerometer, self).__init__(brick, port, check_compatible)
+
+ def get_acceleration(self):
+ """Returns the acceleration along x, y, z axes. Units are unknown to me.
+ """
+ xh, yh, zh, xl, yl, zl = self.read_value('all_data')
+ x = xh << 2 + xl
+ y = yh << 2 + yl
+ z = zh << 2 + yl
+ return self.Acceleration(x, y, z)
+
+ get_sample = get_acceleration
+
+Accelerometer.add_compatible_sensor(None, 'HiTechnc', 'Accel. ')
+Accelerometer.add_compatible_sensor(None, 'HITECHNC', 'Accel. ') #Tested with version '\xfdV1.1 '
+
+
+class IRReceiver(BaseDigitalSensor):
+ """Object for HiTechnic IRReceiver sensors for use with LEGO Power Functions IR
+Remotes. Coded to HiTechnic's specs for the sensor but not tested. Please report
+whether this worked for you or not!
+ """
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'm1A': (0x42, 'b'),
+ 'm1B': (0x43, 'b'),
+ 'm2A': (0x44, 'b'),
+ 'm2B': (0x45, 'b'),
+ 'm3A': (0x46, 'b'),
+ 'm3B': (0x47, 'b'),
+ 'm4A': (0x48, 'b'),
+ 'm4B': (0x49, 'b'),
+ 'all_data': (0x42, '8b')
+ })
+
+ class SpeedReading:
+ def __init__(self, m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B):
+ self.m1A, self.m1B, self.m2A, self.m2B, self.m3A, self.m3B, self.m4A, self.m4B = m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B
+ self.channel_1 = (m1A, m1B)
+ self.channel_2 = (m2A, m2B)
+ self.channel_3 = (m3A, m3B)
+ self.channel_4 = (m4A, m4B)
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(IRReceiver, self).__init__(brick, port, check_compatible)
+
+ def get_speeds(self):
+ """Returns the motor speeds for motors A and B on channels 1-4.
+Values are -128, -100, -86, -72, -58, -44, -30, -16, 0, 16, 30, 44, 58, 72, 86
+and 100. -128 specifies motor brake mode. Note that no motors are actually
+being controlled here!
+ """
+ m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B = self.read_value('all_data')
+ return self.SpeedReading(m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B)
+
+ get_sample = get_speeds
+
+IRReceiver.add_compatible_sensor(None, 'HiTechnc', 'IRRecv ')
+IRReceiver.add_compatible_sensor(None, 'HITECHNC', 'IRRecv ')
+
+
+class IRSeekerv2(BaseDigitalSensor):
+ """Object for HiTechnic IRSeeker sensors. Coded to HiTechnic's specs for the sensor
+but not tested. Please report whether this worked for you or not!
+ """
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'dspmode': (0x41, 'B'),
+ 'DC_direction': (0x42, 'B'),
+ 'DC_sensor_1': (0x43, 'B'),
+ 'DC_sensor_2': (0x44, 'B'),
+ 'DC_sensor_3': (0x45, 'B'),
+ 'DC_sensor_4': (0x46, 'B'),
+ 'DC_sensor_5': (0x47, 'B'),
+ 'DC_sensor_mean': (0x48, 'B'),
+ 'all_DC': (0x42, '7B'),
+ 'AC_direction': (0x49, 'B'),
+ 'AC_sensor_1': (0x4A, 'B'),
+ 'AC_sensor_2': (0x4B, 'B'),
+ 'AC_sensor_3': (0x4C, 'B'),
+ 'AC_sensor_4': (0x4D, 'B'),
+ 'AC_sensor_5': (0x4E, 'B'),
+ 'all_AC': (0x49, '6B')
+ })
+ I2C_DEV = 0x10 #different from standard 0x02
+
+ class DSPModes:
+ #Modes for modulated (AC) data.
+ AC_DSP_1200Hz = 0x00
+ AC_DSP_600Hz = 0x01
+
+ class _data:
+ def get_dir_brightness(self, direction):
+ "Gets the brightness of a given direction (1-9)."
+ if direction%2 == 1: #if it's an odd number
+ exec("val = self.sensor_%d" % ((direction-1)/2+1))
+ else:
+ exec("val = (self.sensor_%d+self.sensor_%d)/2" % (direction/2, (direction/2)+1))
+ return val
+
+ class DCData(_data):
+ def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean):
+ self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5, self.sensor_mean = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean
+
+ class ACData(_data):
+ def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5):
+ self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5 = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5
+
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(IRSeekerv2, self).__init__(brick, port, check_compatible)
+
+ def get_dc_values(self):
+ """Returns the unmodulated (DC) values.
+ """
+ direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean = self.read_value('all_DC')
+ return self.DCData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean)
+
+ def get_ac_values(self):
+ """Returns the modulated (AC) values. 600Hz and 1200Hz modes can be selected
+between by using the set_dsp_mode() function.
+ """
+ direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5 = self.read_value('all_AC')
+ return self.ACData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5)
+
+ def get_dsp_mode(self):
+ return self.read_value('dspmode')[0]
+
+ def set_dsp_mode(self, mode):
+ self.write_value('dspmode', (mode, ))
+
+ get_sample = get_ac_values
+
+IRSeekerv2.add_compatible_sensor(None, 'HiTechnc', 'NewIRDir')
+IRSeekerv2.add_compatible_sensor(None, 'HITECHNC', 'NewIRDir')
+
+
+class EOPD(BaseAnalogSensor):
+ """Object for HiTechnic Electro-Optical Proximity Detection sensors.
+ """
+
+ # To be divided by processed value.
+ _SCALE_CONSTANT = 250
+
+ # Maximum distance the sensor can detect.
+ _MAX_DISTANCE = 1023
+
+ def __init__(self, brick, port):
+ super(EOPD, self).__init__(brick, port)
+ from math import sqrt
+ self.sqrt = sqrt
+
+ def set_range_long(self):
+ ''' Choose this mode to increase the sensitivity
+ of the EOPD sensor by approximately 4x. May
+ cause sensor overload.
+ '''
+
+ self.set_input_mode(Type.LIGHT_ACTIVE, Mode.RAW)
+
+ def set_range_short(self):
+ ''' Choose this mode to prevent the EOPD sensor from
+ being overloaded by white objects.
+ '''
+
+ self.set_input_mode(Type.LIGHT_INACTIVE, Mode.RAW)
+
+ def get_raw_value(self):
+ '''Unscaled value read from sensor.'''
+
+ return self._MAX_DISTANCE - self.get_input_values().raw_ad_value
+
+ def get_processed_value(self):
+ '''Derived from the square root of the raw value.'''
+
+ return self.sqrt(self.get_raw_value())
+
+ def get_scaled_value(self):
+ ''' Returns a value that will scale linearly as distance
+ from target changes. This is the method that should
+ generally be called to get EOPD sensor data.
+ '''
+
+ try:
+ result = self._SCALE_CONSTANT / self.get_processed_value()
+ return result
+
+ except ZeroDivisionError:
+ return self._SCALE_CONSTANT
+
+ get_sample = get_scaled_value
+
+
+class Colorv2(BaseDigitalSensor):
+ """Object for HiTechnic Color v2 Sensors. Coded to HiTechnic's specs for the sensor
+but not tested. Please report whether this worked for you or not!"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'mode': (0x41, 'B'),
+ 'number': (0x42, 'B'),
+ 'red': (0x43, 'B'),
+ 'green': (0x44, 'B'),
+ 'blue': (0x45, 'B'),
+ 'white': (0x46, 'B'),
+ 'index': (0x47, 'B'),
+ 'normred': (0x48, 'B'),
+ 'normgreen': (0x49, 'B'),
+ 'normblue': (0x4A, 'B'),
+ 'all_data': (0x42, '9B'),
+ 'rawred': (0x42, '<H'),
+ 'rawgreen': (0x44, '<H'),
+ 'rawblue': (0x46, '<H'),
+ 'rawwhite': (0x48, '<H'),
+ 'all_raw_data': (0x42, '<4H')
+ })
+
+ class Modes:
+ ACTIVE = 0x00 #get measurements using get_active_color
+ PASSIVE = 0x01 #get measurements using get_passive_color
+ RAW = 0x03 #get measurements using get_passive_color
+ BLACK_CALIBRATION = 0x42 #hold away from objects, results saved in EEPROM
+ WHITE_CALIBRATION = 0x43 #hold in front of white surface, results saved in EEPROM
+ LED_POWER_LOW = 0x4C #saved in EEPROM, must calibrate after using
+ LED_POWER_HIGH = 0x48 #saved in EEPROM, must calibrate after using
+ RANGE_NEAR = 0x4E #saved in EEPROM, only affects active mode
+ RANGE_FAR = 0x46 #saved in EEPROM, only affects active mode, more susceptable to noise
+ FREQ_50 = 0x35 #saved in EEPROM, use when local wall power is 50Hz
+ FREQ_60 = 0x36 #saved in EEPROM, use when local wall power is 60Hz
+
+ class ActiveData:
+ def __init__(self, number, red, green, blue, white, index, normred, normgreen, normblue):
+ self.number, self.red, self.green, self.blue, self.white, self.index, self.normred, self.normgreen, self.normblue = number, red, green, blue, white, index, normred, normgreen, normblue
+
+ class PassiveData:
+ #also holds raw mode data
+ def __init__(self, red, green, blue, white):
+ self.red, self.green, self.blue, self.white = red, green, blue, white
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Colorv2, self).__init__(brick, port, check_compatible)
+
+ def get_active_color(self):
+ """Returns color values when in active mode.
+ """
+ number, red, green, blue, white, index, normred, normgreen, normblue = self.read_value('all_data')
+ return self.ActiveData(number, red, green, blue, white, index, normred, normgreen, normblue)
+
+ get_sample = get_active_color
+
+ def get_passive_color(self):
+ """Returns color values when in passive or raw mode.
+ """
+ red, green, blue, white = self.read_value('all_raw_data')
+ return self.PassiveData(red, green, blue, white)
+
+ def get_mode(self):
+ return self.read_value('mode')[0]
+
+ def set_mode(self, mode):
+ self.write_value('mode', (mode, ))
+
+Colorv2.add_compatible_sensor(None, 'HiTechnc', 'ColorPD')
+Colorv2.add_compatible_sensor(None, 'HITECHNC', 'ColorPD')
+Colorv2.add_compatible_sensor(None, 'HiTechnc', 'ColorPD ')
+Colorv2.add_compatible_sensor(None, 'HITECHNC', 'ColorPD ')
+
+
+class Gyro(BaseAnalogSensor):
+ 'Object for gyro sensors'
+#This class is for the hitechnic gryo sensor. When the gryo is not
+#moving there will be a constant offset that will change with
+#temperature and other ambient factors. The calibrate() function
+#takes the currect value and uses it to offset subsequesnt ones.
+
+ def __init__(self, brick, port):
+ super(Gyro, self).__init__(brick, port)
+ self.set_input_mode(Type.ANGLE, Mode.RAW)
+ self.offset = 0
+
+ def get_rotation_speed(self):
+ return self.get_input_values().scaled_value - self.offset
+
+ def set_zero(self, value):
+ self.offset = value
+
+ def calibrate(self):
+ self.set_zero(self.get_rotation_speed())
+
+ get_sample = get_rotation_speed
+
+
+class Prototype(BaseDigitalSensor):
+ """Object for HiTechnic sensor prototype boards. Coded to HiTechnic's specs but not
+tested. Please report whether this worked for you or not!
+ """
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'A0': (0x42, '<H'),
+ 'A0': (0x44, '<H'),
+ 'A0': (0x46, '<H'),
+ 'A0': (0x48, '<H'),
+ 'A0': (0x4A, '<H'),
+ 'all_analog': (0x42, '<5H'),
+ 'digital_in': (0x4C, 'B'),
+ 'digital_out': (0x4D, 'B'),
+ 'digital_cont': (0x4E, 'B'),
+ 'sample_time': (0x4F, 'B'),
+ })
+
+ class Digital_Data():
+ """Container for 6 bits of digital data. Takes an integer or a list of six bools
+and can be converted into a list of bools or an integer."""
+ def __init__(self, pins):
+ if isinstance(pins, int):
+ self.dataint = pins
+ self.datalst = self.tolist(pins)
+ else:
+ self.dataint = self.toint(pins)
+ self.datalst = pins
+ self.d0, self.d1, self.d2, self.d3, self.d4, self.d5 = self.datalst
+
+ def tolist(self, val):
+ lst = []
+ for i in range(6):
+ lst.append(bool(val & 2**i))
+ return lst
+
+ def toint(self, lst):
+ val = 0
+ for i in range(6):
+ val += int(bool(lst[i])) * (2**i)
+ return val
+
+ def __int__(self):
+ return self.dataint
+
+ def __iter__(self):
+ return iter(self.datalst)
+
+ def __getitem__(self, i):
+ return self.datalst[i]
+
+ class Analog_Data():
+ def __init__(self, a0, a1, a2, a3, a4):
+ self.a0, self.a1, self.a2, self.a3, self.a4 = a0, a1, a2, a3, a4
+
+ def get_analog(self):
+ return Analog_Data(self.read_value('all_analog'))
+
+ def get_digital(self):
+ return Digital_Data(self.read_value('digital_in')[0])
+
+ def set_digital(self, pins):
+ """Can take a Digital_Data() object"""
+ self.write_value('digital_out', (int(pins), ))
+
+ def set_digital_modes(self, modes):
+ """Sets input/output mode of digital pins. Can take a Digital_Data() object."""
+ self.write_value('digital_cont', (int(modes), ))
+
+Prototype.add_compatible_sensor(None, 'HiTechnc', 'Proto ')
+
+
+class ServoCon(BaseDigitalSensor):
+ """Object for HiTechnic FIRST Servo Controllers. Coded to HiTechnic's specs for
+the sensor but not tested. Please report whether this worked for you or not!"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'status': (0x40, 'B'),
+ 'steptime': (0x41, 'B'),
+ 's1pos': (0x42, 'B'),
+ 's2pos': (0x43, 'B'),
+ 's3pos': (0x44, 'B'),
+ 'p4pos': (0x45, 'B'),
+ 'p5pos': (0x46, 'B'),
+ 'p6pos': (0x47, 'B'),
+ 'pwm': (0x46, 'B'),
+ })
+
+ class Status:
+ RUNNING = 0x00 #all motors stopped
+ STOPPED = 0x01 #motor(s) moving
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(ServoCon, self).__init__(brick, port, check_compatible)
+
+ def get_status(self):
+ """Returns the status of the motors. 0 for all stopped, 1 for
+some running.
+ """
+ return self.read_value('status')[0]
+
+ def set_step_time(self, time):
+ """Sets the step time (0-15).
+ """
+ self.write_value('steptime', (time, ))
+
+ def set_pos(self, num, pos):
+ """Sets the position of a server. num is the servo number (1-6),
+pos is the position (0-255).
+ """
+ self.write_value('s%dpos' % num, (pos, ))
+
+ def get_pwm(self):
+ """Gets the "PWM enable" value. The function of this value is
+nontrivial and can be found in the documentation for the sensor.
+ """
+ return self.read_value('pwm')[0]
+
+ def set_pwm(self, pwm):
+ """Sets the "PWM enable" value. The function of this value is
+nontrivial and can be found in the documentation for the sensor.
+ """
+ self.write_value('pwm', (pwm, ))
+
+ServoCon.add_compatible_sensor(None, 'HiTechnc', 'ServoCon')
+
+
+class MotorCon(BaseDigitalSensor):
+ """Object for HiTechnic FIRST Motor Controllers. Coded to HiTechnic's specs for
+the sensor but not tested. Please report whether this worked for you or not!"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'm1enctarget': (0x40, '>l'),
+ 'm1mode': (0x44, 'B'),
+ 'm1power': (0x45, 'b'),
+ 'm2power': (0x46, 'b'),
+ 'm2mode': (0x47, 'B'),
+ 'm2enctarget': (0x48, '>l'),
+ 'm1enccurrent': (0x4c, '>l'),
+ 'm2enccurrent': (0x50, '>l'),
+ 'batteryvoltage': (0x54, '2B'),
+ 'm1gearratio': (0x56, 'b'),
+ 'm1pid': (0x57, '3B'),
+ 'm2gearratio': (0x5a, 'b'),
+ 'm2pid': (0x5b, '3B'),
+ })
+
+ class PID_Data():
+ def __init__(self, p, i, d):
+ self.p, self.i, self.d = p, i, d
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(MotorCon, self).__init__(brick, port, check_compatible)
+
+ def set_enc_target(self, mot, val):
+ """Set the encoder target (-2147483648-2147483647) for a motor
+ """
+ self.write_value('m%denctarget'%mot, (val, ))
+
+ def get_enc_target(self, mot):
+ """Get the encoder target for a motor
+ """
+ return self.read_value('m%denctarget'%mot)[0]
+
+ def get_enc_current(self, mot):
+ """Get the current encoder value for a motor
+ """
+ return self.read_value('m%denccurrent'%mot)[0]
+
+ def set_mode(self, mot, mode):
+ """Set the mode for a motor. This value is a bit mask and you can
+find details about it in the sensor's documentation.
+ """
+ self.write_value('m%dmode'%mot, (mode, ))
+
+ def get_mode(self, mot):
+ """Get the mode for a motor. This value is a bit mask and you can
+find details about it in the sensor's documentation.
+ """
+ return self.read_value('m%dmode'%mot)[0]
+
+ def set_power(self, mot, power):
+ """Set the power (-100-100) for a motor
+ """
+ self.write_value('m%dpower'%mot, (power, ))
+
+ def get_power(self, mot):
+ """Get the power for a motor
+ """
+ return self.read_value('m%dpower'%mot)[0]
+
+ def set_gear_ratio(self, mot, ratio):
+ """Set the gear ratio for a motor
+ """
+ self.write_value('m%dgearratio'%mot, (ratio, ))
+
+ def get_gear_ratio(self, mot):
+ """Get the gear ratio for a motor
+ """
+ return self.read_value('m%dgearratio'%mot)[0]
+
+ def set_pid(self, mot, piddata):
+ """Set the PID coefficients for a motor. Takes data in
+MotorCon.PID_Data(p, i, d) format.
+ """
+ self.write_value('m%dpid'%mot, (piddata.p, piddata.i, piddata.d))
+
+ def get_pid(self, mot):
+ """Get the PID coefficients for a motor. Returns a PID_Data() object.
+ """
+ p, i, d = self.read_value('m%dpid'%mot)
+ return self.PID_Data(p, i, d)
+
+ def get_battery_voltage(self):
+ """Gets the battery voltage (in millivolts/20)
+ """
+ high, low = self.read_value('bateryvoltage')[0]
+ return high << 2 + low
+
+MotorCon.add_compatible_sensor(None, 'HiTechnc', 'MotorCon')
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py
new file mode 100644
index 0000000..de6c7ee
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py
@@ -0,0 +1,815 @@
+# nxt.sensor.mindsensors module -- Classes implementing Mindsensors sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 Marcus Wanner, MindSensors
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+
+from .common import *
+from .digital import BaseDigitalSensor, SensorInfo
+from .analog import BaseAnalogSensor
+
+
+class SumoEyes(BaseAnalogSensor):
+ """The class to control Mindsensors Sumo sensor. Warning: long range not
+ working for my sensor.
+ """
+ #range: 5-10cm
+ class Reading:
+ """Contains the reading of SumoEyes sensor. left and right can be True or
+ False. If True, then there is something there, if False, then it's empty
+ there.
+ """
+ def __init__(self, raw_reading):
+ self.raw = raw_reading
+ val = raw_reading.normalized_ad_value # FIXME: make it rely on raw_ad_value
+ right = 600 < val < 700
+ both = 700 <= val < 900
+ left = 300 < val < 400
+ self.left = left or both
+ self.right = right or both
+
+ def __str__(self):
+ return '(left: ' + str(self.left) + ', right: ' + str(self.right) + ')'
+
+ def __init__(self, brick, port, long_range=False):
+ super(SumoEyes, self).__init__(brick, port)
+ self.set_long_range(long_range)
+
+ def set_long_range(self, val):
+ """Sets if the sensor should operate in long range mode (12 inches) or
+ the short range mode (6 in). val should be True or False.
+ """
+ if val:
+ type_ = Type.LIGHT_INACTIVE
+ else:
+ type_ = Type.LIGHT_ACTIVE
+ self.set_input_mode(type_, Mode.RAW)
+
+ def get_sample(self):
+ """Returns the processed meaningful values of the sensor"""
+ return self.Reading(self.get_input_values())
+
+
+class Compassv2(BaseDigitalSensor):
+ """Class for the now-discontinued CMPS-Nx sensor. Also works with v1.1 sensors.
+Note that when using a v1.x sensor, some of the commands are not supported!
+To determine your sensor's version, use get_sensor_info().version"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command': (0x41, '<B'),
+ 'heading': (0x42, '<H'),
+ 'x_offset': (0x44, '<h'), #unsure about signedness for this one
+ 'y_offset': (0x46, '<h'), #and this one
+ 'x_range': (0x48, '<H'),
+ 'y_range': (0x4A, '<H'),
+ 'x_raw': (0x4C, '<H'), #and this one
+ 'y_raw': (0x4E, '<H'), #and this one
+ })
+
+ class Commands:
+ AUTO_TRIG_ON = 'A'
+ AUTO_TRIG_OFF = 'S'
+ MAP_HEADING_BYTE = 'B' # map heading to 0-255 range
+ MAP_HEADING_INTEGER = 'I' # map heading to 0-36000 (or 3600) range
+ SAMPLING_50_HZ = 'E' # set sampling frequency to 50 Hz
+ SAMPLING_60_HZ = 'U' # set sampling frequency to 60 Hz
+ SET_ADPA_MODE_ON = 'N' # set ADPA mode on
+ SET_ADPA_MODE_OFF = 'O' # set ADPA mode off
+ BEGIN_CALIBRATION = 'C' # begin calibration
+ DONE_CALIBRATION = 'D' # done with calibration
+ LOAD_USER_CALIBRATION = 'L' # load user calibration value
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Compassv2, self).__init__(brick, port, check_compatible)
+ self.command(self.Commands.MAP_HEADING_INTEGER)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_heading(self):
+ return self.read_value('heading')[0]
+
+ get_sample = get_heading
+
+Compassv2.add_compatible_sensor(None, 'mndsnsrs', 'CMPS')
+
+
+class DIST(BaseDigitalSensor):
+ """Class for the Distance Infrared Sensor"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command': (0x41, '<B'),
+ 'distance': (0x42, '<H'),
+ 'voltage': (0x44, '<H'),
+ 'type': (0x50, '<B'),
+ 'no_of_data_points': (0x51, '<B'),
+ 'min_distance': (0x52, '<H'),
+ 'max_distance': (0x54, '<H'),
+ })
+
+ class Commands:
+ TYPE_GP2D12 = '1' #GP2D12 sensor Module
+ TYPE_GP2D120 = '2' #Short range sensor Module
+ TYPE_GP2Y0A21YK = '3' #Medium range sensor Module
+ TYPE_GP2Y0A02YK = '4' #Long range sensor Module
+ TYPE_CUSTOM = '5' #Custom sensor Module
+ POWER_ON = 'E' #Sensor module power on
+ POWER_OFF = 'D' #Sensor module power offset
+ ADPA_ON = 'N' #ADPA mode on
+ ADPA_OFF = 'O' #ADPA mode off (default)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_distance(self):
+ return self.read_value('distance')[0]
+
+ get_sample = get_distance
+
+ def get_type(self):
+ return self.read_value('type')[0]
+
+ def get_voltage(self):
+ return self.read_value('voltage')[0]
+
+ def get_min_distance(self):
+ return self.read_value('min_distance')[0]
+
+ def get_max_distance(self):
+ return self.read_value('max_distance')[0]
+
+DIST.add_compatible_sensor(None, 'mndsnsrs', 'DIST')
+
+
+class RTC(BaseDigitalSensor):
+ """Class for the RealTime Clock sensor"""
+ #TODO: Create a function to set the clock
+ #Has no indentification
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'seconds': (0x00, '<B'),
+ 'minutes': (0x01, '<B'),
+ 'hours': (0x02, '<B'),
+ 'day': (0x03, '<B'),
+ 'date': (0x04, '<B'),
+ 'month': (0x05, '<B'),
+ 'year': (0x06, '<B'),
+ })
+ I2C_DEV = 0xD0
+
+ def __init__(self, brick, port, check_compatible=False): #check_compatible must remain false due to no identification!
+ super(RTC, self).__init__(brick, port, check_compatible)
+
+ def get_seconds(self):
+ gs = self.read_value('seconds')[0]
+ gs2 = gs & 0xf # bitmasks
+ gs3 = gs & 0x70
+ gs3 = gs3 >> 4
+ return str(gs3) + str(gs2)
+
+ def get_minutes(self):
+ gm = self.read_value('minutes')[0]
+ gm2 = gm & 0xf
+ gm3 = gm & 0x70
+ gm3 = gm3 >> 4
+ return str(gm3) + str(gm2)
+
+ def get_hours(self):
+ gh = self.read_value('hours')[0]
+ gh2 = gh & 0xf
+ gh3 = gh & 0x30
+ gh3 = gh3 >> 4
+ return str(gh3) + str(gh2)
+
+ def get_day(self):
+ gwd = self.read_value('day')[0]
+ gwd = gwd & 0x07
+ return gwd
+
+ def get_month(self):
+ gmo = self.read_value('month')[0]
+ gmo2 = gmo & 0xf
+ gmo3 = gmo & 0x10
+ gmo3 = gmo3 >> 4
+ return str(gmo3) + str(gmo2)
+
+ def get_year(self):
+ """Last two digits (10 for 2010)"""
+ gy = self.read_value('year')[0]
+ gy2 = gy & 0xf
+ gy3 = gy & 0xF0
+ gy3 = gy3 >> 4
+ return str(gy3) + str(gy2)
+
+ def get_date(self):
+ gd = self.read_value('date')[0]
+ gd2 = gd & 0xf
+ gd3 = gd & 0x60
+ gd3 = gd3 >> 4
+ return str(gd3) + str(gd2)
+
+ def hour_mode(self, mode):
+ """Writes mode bit and re-enters hours, which is required"""
+ if mode == 12 or 24:
+ hm = self.read_value('hours')[0]
+ hm2 = hm & 0x40
+ hm2 = hm2 >> 6
+ if mode == 12 and hm2 == 0: #12_HOUR = 1
+ hm3 = hm + 64
+ self.write_value('hours', (hm3, ))
+ elif mode == 24 and hm2 == 1: #24_HOUR = 0
+ hm3 = hm - 64
+ self.write_value('hours', (hm3, ))
+ else:
+ print 'That mode is already selected!'
+ else:
+ raise ValueError('Must be 12 or 24!')
+
+ def get_mer(self):
+ mer = self.read_value('hours')[0]
+ mer2 = mer & 0x40
+ mer2 = mer2 >> 6
+ if mer2 == 1:
+ mer3 = mer & 0x20
+ mer3 = mer3 >> 0x10
+ return mer3
+ else:
+ print 'Cannot get mer! In 24-hour mode!'
+
+ def get_sample(self):
+ """Returns a struct_time() tuple which can be processed by the time module."""
+ import time
+ return time.struct_time((
+ int(self.get_year())+2000,
+ int(self.get_month()),
+ int(self.get_date()),
+ int(self.get_hours()),
+ int(self.get_minutes()),
+ int(self.get_seconds()),
+ int(self.get_day()),
+ 0, #Should be the Julian Day, but computing that is hard.
+ 0 #No daylight savings time to worry about here.
+ ))
+
+
+class ACCL(BaseDigitalSensor):
+ """Class for Accelerometer sensor"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'sensitivity': (0x19, 'B'),
+ 'command': (0x41, 'B'),
+ 'x_tilt': (0x42, 'b'),
+ 'y_tilt': (0x43, 'b'),
+ 'z_tilt': (0x44, 'b'),
+ 'all_tilt': (0x42, '3b'),
+
+ 'x_accel': (0x45, '<h'),
+ 'y_accel': (0x47, '<h'),
+ 'z_accel': (0x49, '<h'),
+ 'all_accel': (0x45, '<3h'),
+
+ 'x_offset': (0x4B, '<h'),
+ 'x_range': (0x4D, '<h'),
+
+ 'y_offset': (0x4F, '<h'),
+ 'y_range': (0x51, '<h'),
+
+ 'z_offset': (0x53, '<h'),
+ 'z_range': (0x55, '<h'),
+ })
+
+ class Commands:
+ SENS_15G = '1' #that's 1.5...Alt. 2.5G (sensors older than V3.20)
+ SENS_2G = '2' #Alt .3.3G
+ SENS_4G = '3' #Alt. 6.7G
+ SENS_6G = '4' #Alt. 10G
+ X_CALIBRATION = 'X' #Acquire X point calibration
+ X_CAL_AND_END = 'x' #X point calibration and end calibration
+ Y_CALIBRATION = 'Y' #Acquire Y point calibration
+ Y_CAL_AND_END = 'y' #Y point calibration and end calibration
+ Z_CALIBRATION = 'Z' #Acquire Z point calibration
+ Z_CAL_AND_END = 'z' #Z point calibration and end calibration
+ CAL_RESET = 'R' #Reset to factory set calibration
+ ADPA_ON = 'N' #Set ADPA mode On
+ ADPA_OFF = 'O' #Set ADPA mode Off (default)
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(ACCL, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_sensitivity(self):
+ return chr(self.read_value('sensitivity')[0])
+
+ def get_tilt(self, axis):
+ xyz = str(axis) + '_tilt'
+ return self.read_value(xyz)[0]
+
+ def get_all_tilt(self):
+ return self.read_value('all_tilt')
+
+ def get_accel(self, axis):
+ xyz = str(axis) + '_accel'
+ return self.read_value(xyz)[0]
+
+ def get_all_accel(self):
+ return self.read_value('all_accel')
+
+ get_sample = get_all_accel
+
+ def get_offset(self, axis):
+ xyz = str(axis) + '_offset'
+ return self.read_value(xyz)[0]
+
+ def get_range(self, axis):
+ xyz = str(axis) + '_range'
+ return self.read_value(xyz)[0]
+
+ def set_offset(self, axis, value):
+ xyz = str(axis) + '_offset'
+ self.write_value(xyz, (value, ))
+
+ def set_range(self, axis, value):
+ xyz = str(axis) + '_range'
+ self.write_value(xyz, (value, ))
+
+ACCL.add_compatible_sensor(None, 'mndsnsrs', 'ACCL-NX') #Tested with version 'V3.20'
+
+
+class MTRMUX(BaseDigitalSensor):
+ """Class for RCX Motor Multiplexer sensor"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ 'direction_m1': (0x42, '<B'),
+ 'speed_m1': (0x43, '<B'),
+ 'direction_m2': (0x44, '<B'),
+ 'speed_m2': (0x45, '<B'),
+ 'direction_m3': (0x46, '<B'),
+ 'speed_m3': (0x47, '<B'),
+ 'direction_m4': (0x48, '<B'),
+ 'speed_m4': (0x49, '<B'),
+ })
+ I2C_DEV = 0xB4
+
+ class Commands:
+ FLOAT = 0x00
+ FORWARD = 0x01
+ REVERSE = 0x02
+ BRAKE = 0x03
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(MTRMUX, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ self.write_value('command', (command, ))
+
+ def set_direction(self, number, value):
+ addressname = 'direction_m' + str(number)
+ self.write_value(addressname, (value, ))
+
+ def set_speed(self, number, value):
+ addressname = 'speed_m' + str(number)
+ self.write_value(addressname, (value, ))
+
+ def get_direction(self, number):
+ addressname = 'direction_m' + str(number)
+ self.read_value(addressname)
+
+ def get_speed(self, number):
+ addressname = 'speed_m' + str(number)
+ self.read_value(addressname)
+
+MTRMUX.add_compatible_sensor(None, 'mndsnsrs', 'MTRMUX') #Tested with version 'V2.11'
+
+
+class LineLeader(BaseDigitalSensor):
+ """Class for Line Sensor Array"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command': (0x41, '<B'),
+ 'steering': (0x42, '<b'),
+ 'average': (0x43, '<B'),
+ 'result': (0x44, '<B'),
+ 'set_point': (0x45, '<B'),
+
+ 'kp': (0x46, '<B'),
+ 'ki': (0x47, '<B'),
+ 'kd': (0x48, '<B'),
+ 'kp_divisor':(0x61, '<B'),
+ 'ki_divisor':(0x62, '<B'),
+ 'kd_divisor':(0x63, '<B'),
+ #One byte for each sensor, so byte# = sensor#
+ 'calibrated_reading_byte1': (0x49, '<B'),
+ 'calibrated_reading_byte2': (0x4A, '<B'),
+ 'calibrated_reading_byte3': (0x4B, '<B'),
+ 'calibrated_reading_byte4': (0x4C, '<B'),
+ 'calibrated_reading_byte5': (0x4D, '<B'),
+ 'calibrated_reading_byte6': (0x4E, '<B'),
+ 'calibrated_reading_byte7': (0x4F, '<B'),
+ 'calibrated_reading_byte8': (0x50, '<B'),
+ 'all_calibrated_readings': (0x49, '<8B'),
+
+ 'w_read_limit':(0x51, '<H'),
+ 'b_read_limit':(0x59, '<B'),
+ 'w_cal_data1':(0x64, '<B'),
+ 'b_cal_data':(0x6C, '<B'),
+
+ 'uncal_sensor1_voltage_byte1':(0x74, '<B'),
+ 'uncal_sensor2_voltage_byte1':(0x76, '<B'),
+ 'uncal_sensor3_voltage_byte1':(0x78, '<B'),
+ 'uncal_sensor4_voltage_byte1':(0x7A, '<B'),
+ 'uncal_sensor5_voltage_byte1':(0x7C, '<B'),
+ 'uncal_sensor6_voltage_byte1':(0x7E, '<B'),
+ 'uncal_sensor7_voltage_byte1':(0x80, '<B'),
+ 'uncal_sensor8_voltage_byte1':(0x82, '<B'),
+ 'all_uncal_readings': (0x74, '<8B'),
+ })
+
+ class Commands:
+ CALIBRATE_WHITE = 'W'
+ CALIBRATE_BLACK = 'B'
+ SENSOR_SLEEP = 'D'
+ US_CONFIG = 'A'
+ EU_CONFIG = 'E'
+ UNI_CONFIG = 'U'
+ SENSOR_WAKE = 'P'
+ COLOR_INVERT = 'I'
+ COLOR_INVERT_REVERSE = 'R'
+ SNAPSHOT = 'S'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(LineLeader, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_steering(self):
+ 'Value to add to the left and subtract from the right motor\'s power.'
+ return self.read_value('steering')[0]
+
+ def get_average(self):
+ 'Weighted average; greater as line is closer to right edge. 0 for no line.'
+ return self.read_value('average')[0]
+
+ def get_result(self):
+ 'Bitmap, one bit for each sensor'
+ return self.read_value('result')[0]
+
+ def set_set_point(self, value):
+ 'Average value for steering to gravitate to. 10 (left) to 80 (right).'
+ self.write_value('set_point', (value, ))
+
+ def set_pid(self, pid, value):
+ addressname = 'k' + str(pid)
+ self.write_value(addressname, (value, ))
+
+ def set_pid_divisor(self, pid, value):
+ addressname = 'k' + str(pid) + '_divisor'
+ self.write_value(addressname, (value, ))
+
+ def get_reading(self, number):
+ addressname = 'calibrated_reading_byte' + str(number)
+ return self.read_value(addressname)[0]
+
+ def get_reading_all(self):
+ return self.read_value('all_calibrated_readings')
+
+ get_sample = get_reading_all
+
+ def get_uncal_reading(self, number):
+ addressname = 'uncal_sensor' + str(number) + '_voltage_byte1'
+ return self.read_value(addressname)[0]
+
+ def get_uncal_all(self):
+ return self.read_value('all_uncal_readings')
+
+LineLeader.add_compatible_sensor(None, 'mndsnsrs', 'LineLdr') #Tested with version 'V1.16'
+
+
+class Servo(BaseDigitalSensor):
+ """Class for Servo sensors"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+
+ 'servo_1_pos': (0x42, '<H'),
+ 'servo_2_pos': (0x44, '<H'),
+ 'servo_3_pos': (0x46, '<H'),
+ 'servo_4_pos': (0x48, '<H'),
+ 'servo_5_pos': (0x4A, '<H'),
+ 'servo_6_pos': (0x4C, '<H'),
+ 'servo_7_pos': (0x4E, '<H'),
+ 'servo_8_pos': (0x50, '<H'),
+
+ 'servo_1_speed': (0x52, '<B'),
+ 'servo_2_speed': (0x53, '<B'),
+ 'servo_3_speed': (0x54, '<B'),
+ 'servo_4_speed': (0x55, '<B'),
+ 'servo_5_speed': (0x56, '<B'),
+ 'servo_6_speed': (0x57, '<B'),
+ 'servo_7_speed': (0x58, '<B'),
+ 'servo_8_speed': (0x59, '<B'),
+
+ 'servo_1_quick': (0x5A, '<B'),
+ 'servo_2_quick': (0x5B, '<B'),
+ 'servo_3_quick': (0x5C, '<B'),
+ 'servo_4_quick': (0x5D, '<B'),
+ 'servo_5_quick': (0x5E, '<B'),
+ 'servo_6_quick': (0x5F, '<B'),
+ 'servo_7_quick': (0x60, '<B'),
+ 'servo_8_quick': (0x61, '<B'),
+ })
+ I2C_DEV = 0xB0
+
+ COMMANDVALUES = {'R': (0x52), #Resume macro execution
+ 'S': (0x53), #reset initial position and speed
+ 'I1': (0x4931), #store initial position motor 1
+ 'I2': (0x4932), #store initial position motor 2
+ 'I3': (0x4933), #etc...
+ 'I4': (0x4934),
+ 'I5': (0x4935),
+ 'I6': (0x4936),
+ 'I7': (0x4937),
+ 'I8': (0x4938),
+ 'H': (0x48), #Halt macro
+ 'Gx': (0x4778), #not going to work yet x = variable
+ 'EM': (0x454d), #Edit Macro
+ 'P': (0x50), #Pause Macro
+ }
+
+ class Commands:
+ RESUME_MACRO = 'R'
+ RESET_POS_SPEED = 'S'
+ STORE_MOTOR_POS_1 = 'I1'
+ STORE_MOTOR_POS_2 = 'I2'
+ STORE_MOTOR_POS_3 = 'I3'
+ STORE_MOTOR_POS_4 = 'I4'
+ STORE_MOTOR_POS_5 = 'I5'
+ STORE_MOTOR_POS_6 = 'I6'
+ STORE_MOTOR_POS_7 = 'I7'
+ STORE_MOTOR_POS_8 = 'I8'
+ HALT_MACRO = 'H'
+ X_TO_VAR = 'Gx' #not going to work yet
+ EDIT_MACRO = 'EM'
+ PAUSE_MACRO = 'P'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Servo, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = self.COMMANDVALUES[command]
+ self.write_value('command', (value, ))
+
+ def get_bat_level(self):
+ return self.read_value('command')[0]
+
+ def set_position(self, number, value):
+ addressname = 'servo_' + str(number) + '_pos'
+ self.write_value(addressname, (value, ))
+
+ def get_position(self, number):
+ return self.read_value('servo_' + str(number) + '_pos')[0]
+
+ def set_speed(self, number, value):
+ addressname = 'servo_' + str(number) + '_speed'
+ self.write_value(addressname, (value, ))
+
+ def get_speed(self, number):
+ return self.read_value('servo_' + str(number) + '_speed')[0]
+
+ def set_quick(self, number, value):
+ addressname = 'servo_' + str(number) + '_quick'
+ self.write_value(addressname, (value, ))
+
+Servo.add_compatible_sensor(None, 'mndsnsrs', 'NXTServo') #Tested with version 'V1.20'
+
+
+class MMX(BaseDigitalSensor):
+ """Class for MMX sensors"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ #Motor Writes
+ 'encoder_1_target': (0x42, '<l'),
+ 'speed_1': (0x46, '<B'),
+ 'seconds_to_run_1': (0x47, '<B'),
+ 'command_b_1': (0x48, '<B'),
+ 'command_a_1': (0x49, '<B'),
+
+ 'encoder_2_target': (0x4A, '<l'),
+ 'speed_2': (0x4E, '<B'),
+ 'seconds_to_run_2': (0x4F, '<B'),
+ 'command_b_2': (0x50, '<B'),
+ 'command_a_2': (0x51, '<B'),
+ #Motor reads
+ 'encoder_1_pos': (0x62, '<H'),
+ 'encoder_2_pos': (0x66, '<H'),
+ 'status_m1': (0x72, '<B'),
+ 'status_m2': (0x73, '<B'),
+ 'tasks_running_m1': (0x76, '<H'),
+ 'tasks_running_m2': (0x77, '<H'),
+ #PID Control
+ 'p_encoder': (0x7A, '<H'),
+ 'i_encoder': (0x7C, '<H'),
+ 'd_encoder': (0x7E, '<H'),
+ 'p_speed': (0x80, '<H'),
+ 'i_speed': (0x82, '<H'),
+ 'd_speed': (0x84, '<H'),
+ 'pass_count': (0x86, '<B'),
+ 'tolerance': (0x87, '<B'),
+ })
+ I2C_DEV = 0x06
+
+ class Commands:
+ RESET_PARAMS_ENCODERS = 'R'
+ ISSUE_SYNCED_COMMANDS = 'S'
+ MOTOR_1_FLOAT_STOP = 'a'
+ MOTOR_2_FLOAT_STOP = 'b'
+ BOTH_FLOAT_STOP = 'c'
+ MOTOR_1_BRAKE_STOP = 'A'
+ MOTOR_2_BRAKE_STOP = 'B'
+ BOTH_BRAKE_STOP = 'C'
+ MOTOR_1_ENC_RESET = 'r'
+ MOTOR_2_ENC_RESET = 's'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(MMX, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_bat_level(self):
+ return self.read_value('command')[0]
+
+ def set_encoder_target(self, motor_number, value):
+ addressname = 'encoder_' + str(motor_number) + '_target'
+ self.write_value(addressname, (value, ))
+
+ def set_speed(self, motor_number, value):
+ addressname = 'speed_' + str(motor_number)
+ self.write_value(addressname, (value, ))
+
+ def set_time_run(self, motor_number, seconds):
+ addressname = 'seconds_to_run_' + str(motor_number)
+ self.write_value(addressname, (seconds, ))
+
+ def command_b(self, motor_number, value):
+ addressname = 'command_b_' + str(motor_number)
+ self.write_value(addressname, (value, ))
+
+ def command_a(self, motor_number, bit_num, bit_val):
+ addressname = 'command_a_' + str(motor_number)
+ s = self.read_value(addressname)[0]
+ #I feel like there must be an easier way to write one bit...
+ val = bit_val << bit_num
+ if bit_val == 1:
+ value = val | s
+ self.write_value(addressname, (value, ))
+ return value #testing purposes
+ elif bit_val == 0:
+ val = 1
+ val = val << bit_num
+ val = val ^ 0xFF
+ value = val & s
+ self.write_value(addressname, (value, ))
+ return value
+
+ def get_encoder_pos(self, motor_number):
+ addressname = 'encoder_' +str(motor_number) +'_pos'
+ return self.read_value(addressname)[0]
+
+ def get_motor_status(self, motor_number, bit_num):
+ addressname = 'status_m' + str(motor_number)
+ s = self.read_value(addressname)[0]
+ x = 1
+ x = x << bit_num
+ value = x & s
+ value = value >> bit_num
+ return value
+
+ def get_tasks(self, motor_number):
+ addressname = 'tasks_running_m' + str(motor_number)
+ return self.read_value(addressname)[0]
+
+ def set_pid(self, pid, target, value):
+ addressname = str(pid) + '_' + str(target)
+ self.write_value(addressname, (value, ))
+
+ def set_pass_count(self, value):
+ self.write_value('pass_count', (value, ))
+
+ def set_tolerance(self, value):
+ self.write_value('tolerance', (value, ))
+
+MMX.add_compatible_sensor(None, 'mndsnsrs', 'NxTMMX') #Tested with version 'V1.01'
+
+
+class HID(BaseDigitalSensor):
+ """Class for Human Interface Device sensors.
+These are connected to a computer and look like a keyboard to it."""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ 'modifier' : (0x42, '<B'),
+ 'keyboard_data' : (0x43, '<B'),
+ })
+ I2C_DEV = 0x04
+
+ class Commands:
+ TRANSMIT = 'T'
+ ASCII_MODE = 'A'
+ DIRECT_MODE = 'D'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(HID, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def set_modifier(self, mod):
+ self.write_value('modifier', (mod, ))
+
+ def write_data(self, data):
+ data = ord(data)
+ self.write_value('keyboard_data', (data, ))
+
+HID.add_compatible_sensor(None, 'mndsnsrs', 'NXTHID') #Tested with version 'V1.02'
+
+
+class PS2(BaseDigitalSensor):
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ 'button_set_1': (0x42, '<B'),
+ 'button_set_2': (0x43, '<B'),
+ 'x_left_joystick': (0x44, '<b'),
+ 'y_left_joystick': (0x45, '<b'),
+ 'x_right_joystick': (0x46, '<b'),
+ 'y_right_joystick': (0x47, '<b'),
+ })
+
+ class ControllerState:
+ class Buttons:
+ left, down, right, up, square, cross, circle, triangle, r1, r2, r3, l1, l2, l3 = [0 for i in range(14)] #14 zeros
+ def __init__(self, buttons_1, buttons_2, left_x, left_y, right_x, right_y):
+ self.leftstick = (left_x, left_y)
+ self.rightstick = (right_x, right_y)
+ buttons_1 = ~buttons_1
+ buttons_2 = ~buttons_2
+ self.buttons = self.Buttons()
+ self.buttons.left = bool(buttons_1 & 0x80)
+ self.buttons.down = bool(buttons_1 & 0x40)
+ self.buttons.right = bool(buttons_1 & 0x20)
+ self.buttons.up = bool(buttons_1 & 0x10)
+ self.buttons.square = bool(buttons_2 & 0x80)
+ self.buttons.cross = bool(buttons_2 & 0x40)
+ self.buttons.circle = bool(buttons_2 & 0x20)
+ self.buttons.triangle = bool(buttons_2 & 0x10)
+ self.buttons.r1 = bool(buttons_2 & 0x08)
+ self.buttons.r2 = bool(buttons_2 & 0x02)
+ self.buttons.r3 = bool(buttons_1 & 0x04)
+ self.buttons.l1 = bool(buttons_2 & 0x04)
+ self.buttons.l2 = bool(buttons_2 & 0x01)
+ self.buttons.l3 = bool(buttons_1 & 0x02)
+
+ class Commands:
+ POWER_ON = 'E'
+ POWER_OFF = 'D'
+ DIGITAL_MODE = 'A'
+ ANALOG_MODE = 's'
+ ADPA_ON = 'N'
+ ADPA_OFF = 'O'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(PS2, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_joystick(self, xy, lr):
+ addressname = str(xy) + '_' + str(lr) + '_joystick'
+ return self.read_value(addressname)[0]
+
+ def get_buttons(self, setnum):
+ addressname = 'button_set_' + str(setnum)
+ return self.read_value(addressname)[0]
+
+ def get_sample(self):
+ return self.ControllerState(
+ get_buttons(0),
+ get_buttons(1),
+ get_joystick('x', 'l'),
+ get_joystick('y', 'l'),
+ get_joystick('x', 'r'),
+ get_joystick('y', 'r'))
+
+PS2.add_compatible_sensor(None, 'mndsnsrs', 'PSPNX') #Tested with version 'V2.00'
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/system.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/system.py
new file mode 100644
index 0000000..a82d5d6
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/system.py
@@ -0,0 +1,297 @@
+# nxt.system module -- LEGO Mindstorms NXT system telegrams
+# Copyright (C) 2006 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+'Use for communications regarding the NXT filesystem and such ***ADVANCED USERS ONLY***'
+
+def _create(opcode):
+ 'Create a simple system telegram'
+ from telegram import Telegram
+ return Telegram(False, opcode)
+
+def _create_with_file(opcode, fname):
+ tgram = _create(opcode)
+ tgram.add_filename(fname)
+ return tgram
+
+def _create_with_handle(opcode, handle):
+ tgram = _create(opcode)
+ tgram.add_u8(handle)
+ return tgram
+
+def open_read(opcode, fname):
+ return _create_with_file(opcode, fname)
+
+def _parse_open_read(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ n_bytes = tgram.parse_u32()
+ return (handle, n_bytes)
+
+def open_write(opcode, fname, n_bytes):
+ tgram = _create_with_file(opcode, fname)
+ tgram.add_u32(n_bytes)
+ return tgram
+
+def _parse_open_write(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ return handle
+
+def read(opcode, handle, n_bytes):
+ tgram = _create_with_handle(opcode, handle)
+ tgram.add_u16(n_bytes)
+ return tgram
+
+def _parse_read(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ n_bytes = tgram.parse_u16()
+ data = tgram.parse_string()
+ return (handle, n_bytes, data)
+
+def write(opcode, handle, data):
+ tgram = _create_with_handle(opcode, handle)
+ tgram.add_string(len(data), data)
+ return tgram
+
+def _parse_write(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ n_bytes = tgram.parse_u16()
+ return (handle, n_bytes)
+
+def close(opcode, handle):
+ return _create_with_handle(opcode, handle)
+
+def _parse_close(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ return handle
+
+def delete(opcode, fname):
+ return _create_with_file(opcode, fname)
+
+def _parse_delete(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ fname = tgram.parse_string()
+ return (handle, fname)
+
+def find_first(opcode, fname):
+ return _create_with_file(opcode, fname)
+
+def _parse_find(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ fname = tgram.parse_string(20)
+ n_bytes = tgram.parse_u32()
+ return (handle, fname, n_bytes)
+
+def find_next(opcode, handle):
+ return _create_with_handle(opcode, handle)
+
+def get_firmware_version(opcode):
+ return _create(opcode)
+
+def _parse_get_firmware_version(tgram):
+ tgram.check_status()
+ prot_minor = tgram.parse_u8()
+ prot_major = tgram.parse_u8()
+ prot_version = (prot_major, prot_minor)
+ fw_minor = tgram.parse_u8()
+ fw_major = tgram.parse_u8()
+ fw_version = (fw_major, fw_minor)
+ return (prot_version, fw_version)
+
+def open_write_linear(opcode, fname, n_bytes):
+ tgram = _create_with_file(opcode, fname)
+ tgram.add_u32(n_bytes)
+ return tgram
+
+def open_read_linear(opcode, fname):
+ return _create_with_file(opcode, fname)
+
+def _parse_open_read_linear(tgram):
+ tgram.check_status()
+ n_bytes = tgram.parse_u32()
+ return n_bytes
+
+def open_write_data(opcode, fname, n_bytes):
+ tgram = _create_with_file(opcode, fname)
+ tgram.add_u32(n_bytes)
+ return tgram
+
+def open_append_data(opcode, fname):
+ return _create_with_file(opcode, fname)
+
+def _parse_open_append_data(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ n_bytes = tgram.parse_u32()
+ return (handle, n_bytes)
+
+def request_first_module(opcode, mname):
+ return _create_with_file(opcode, mname)
+
+def _parse_request_module(tgram):
+ tgram.check_status()
+ handle = tgram.parse_u8()
+ mname = tgram.parse_string(20)
+ mod_id = tgram.parse_u32()
+ mod_size = tgram.parse_u32()
+ mod_iomap_size = tgram.parse_u16()
+ return (handle, mname, mod_id, mod_size, mod_iomap_size)
+
+def request_next_module(opcode, handle):
+ return _create_with_handle(opcode, handle)
+
+def close_module_handle(opcode, handle):
+ return _create_with_handle(opcode, handle)
+
+def read_io_map(opcode, mod_id, offset, n_bytes):
+ tgram = _create(opcode)
+ tgram.add_u32(mod_id)
+ tgram.add_u16(offset)
+ tgram.add_u16(n_bytes)
+ return tgram
+
+def _parse_read_io_map(tgram):
+ tgram.check_status()
+ mod_id = tgram.parse_u32()
+ n_bytes = tgram.parse_u16()
+ contents = tgram.parse_string()
+ return (mod_id, n_bytes, contents)
+
+def write_io_map(opcode, mod_id, offset, content):
+ tgram = _create(opcode)
+ tgram.add_u32(mod_id)
+ tgram.add_u16(offset)
+ tgram.add_u16(len(content))
+ tgram.add_string(len(content), content)
+ return tgram
+
+def _parse_write_io_map(tgram):
+ tgram.check_status()
+ mod_id = tgram.parse_u32()
+ n_bytes = tgram.parse_u16()
+ return (mod_id, n_bytes)
+
+def boot(opcode):
+ # Note: this command is USB only (no Bluetooth)
+ tgram = _create(opcode)
+ tgram.add_string(19, "Let's dance: SAMBA\0")
+ return tgram
+
+def _parse_boot(tgram):
+ tgram.check_status()
+ resp = tgram.parse_string()
+ # Resp should be 'Yes\0'
+ return resp
+
+def set_brick_name(opcode, bname):
+ tgram = _create(opcode)
+ if len(bname) > 15:
+ print "Warning! Brick name %s will be truncated to %s!" % (bname, bname[0:15])
+ bname = bname[0:15]
+ elif len(bname) < 15:
+ bname += '\x00' * (15-len(bname)) #fill the extra chars with nulls
+ tgram.add_string(len(bname), bname)
+ return tgram
+
+def _parse_set_brick_name(tgram):
+ tgram.check_status()
+
+def get_device_info(opcode):
+ return _create(opcode)
+
+def _parse_get_device_info(tgram):
+ tgram.check_status()
+ name = tgram.parse_string(15)
+ a0 = tgram.parse_u8()
+ a1 = tgram.parse_u8()
+ a2 = tgram.parse_u8()
+ a3 = tgram.parse_u8()
+ a4 = tgram.parse_u8()
+ a5 = tgram.parse_u8()
+ a6 = tgram.parse_u8()
+ # FIXME: what is a6 for?
+ address = '%02X:%02X:%02X:%02X:%02X:%02X' % (a0, a1, a2, a3, a4, a5)
+ signal_strength = tgram.parse_u32()
+ user_flash = tgram.parse_u32()
+ return (name, address, signal_strength, user_flash)
+
+def delete_user_flash(opcode):
+ return _create(opcode)
+
+def _parse_delete_user_flash(tgram):
+ tgram.check_status()
+
+def poll_command_length(opcode, buf_num):
+ tgram = _create(opcode)
+ tgram.add_u8(buf_num)
+ return tgram
+
+def _parse_poll_command_length(tgram):
+ buf_num = tgram.parse_u8()
+ tgram.check_status()
+ n_bytes = tgram.parse_u8()
+ return (buf_num, n_bytes)
+
+def poll_command(opcode, buf_num, n_bytes):
+ tgram = _create(opcode)
+ tgram.add_u8(buf_num)
+ tgram.add_u8(n_bytes)
+ return tgram
+
+def _parse_poll_command(tgram):
+ buf_num = tgram.parse_u8()
+ tgram.check_status()
+ n_bytes = tgram.parse_u8()
+ command = tgram.parse_string()
+ return (buf_num, n_bytes, command)
+
+def bluetooth_factory_reset(opcode):
+ # Note: this command is USB only (no Bluetooth)
+ return _create(opcode)
+
+def _parse_bluetooth_factory_reset(tgram):
+ tgram.check_status()
+
+OPCODES = {
+ 0x80: (open_read, _parse_open_read),
+ 0x81: (open_write, _parse_open_write),
+ 0x82: (read, _parse_read),
+ 0x83: (write, _parse_write),
+ 0x84: (close, _parse_close),
+ 0x85: (delete, _parse_delete),
+ 0x86: (find_first, _parse_find),
+ 0x87: (find_next, _parse_find),
+ 0x88: (get_firmware_version, _parse_get_firmware_version),
+ 0x89: (open_write_linear, _parse_open_write),
+ 0x8A: (open_read_linear, _parse_open_read_linear),
+ 0x8B: (open_write_data, _parse_open_write),
+ 0x8C: (open_append_data, _parse_open_append_data),
+ 0x90: (request_first_module, _parse_request_module),
+ 0x91: (request_next_module, _parse_request_module),
+ 0x92: (close_module_handle, _parse_close),
+ 0x94: (read_io_map, _parse_read_io_map),
+ 0x95: (write_io_map, _parse_write_io_map),
+ 0x97: (boot, _parse_boot),
+ 0x98: (set_brick_name, _parse_set_brick_name),
+ 0x9B: (get_device_info, _parse_get_device_info),
+ 0xA0: (delete_user_flash, _parse_delete_user_flash),
+ 0xA1: (poll_command_length, _parse_poll_command_length),
+ 0xA2: (poll_command, _parse_poll_command),
+ 0xA4: (bluetooth_factory_reset, _parse_bluetooth_factory_reset),
+}
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/telegram.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/telegram.py
new file mode 100644
index 0000000..1fc5b3e
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/telegram.py
@@ -0,0 +1,118 @@
+# nxt.telegram module -- LEGO Mindstorms NXT telegram formatting and parsing
+# Copyright (C) 2006 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+'Used by nxt.system for sending telegrams to the NXT'
+
+from cStringIO import StringIO
+from struct import pack, unpack
+import nxt.error
+
+class InvalidReplyError(Exception):
+ pass
+
+class InvalidOpcodeError(Exception):
+ pass
+
+class Telegram(object):
+
+ TYPE = 0 # type byte offset
+ CODE = 1 # code byte offset
+ DATA = 2 # data byte offset
+
+ TYPE_NOT_DIRECT = 0x01 # system vs. direct type
+ TYPE_REPLY = 0x02 # reply type (from NXT brick)
+ TYPE_REPLY_NOT_REQUIRED = 0x80 # reply not required flag
+
+ def __init__(self, direct=False, opcode=0, reply_req=True, pkt=None):
+ if pkt:
+ self.pkt = StringIO(pkt)
+ self.typ = self.parse_u8()
+ self.opcode = self.parse_u8()
+ if not self.is_reply():
+ raise InvalidReplyError
+ if self.opcode != opcode:
+ raise InvalidOpcodeError, self.opcode
+ else:
+ self.pkt = StringIO()
+ typ = 0
+ if not direct:
+ typ |= Telegram.TYPE_NOT_DIRECT
+ if not reply_req:
+ typ |= Telegram.TYPE_REPLY_NOT_REQUIRED
+ self.add_u8(typ)
+ self.add_u8(opcode)
+
+ def __str__(self):
+ return self.pkt.getvalue()
+
+ def is_reply(self):
+ return self.typ == Telegram.TYPE_REPLY
+
+ def add_string(self, n_bytes, v):
+ self.pkt.write(pack('%ds' % n_bytes, v))
+
+ def add_filename(self, fname):
+ self.pkt.write(pack('20s', fname))
+
+ def add_s8(self, v):
+ self.pkt.write(pack('<b', v))
+
+ def add_u8(self, v):
+ self.pkt.write(pack('<B', v))
+
+ def add_s16(self, v):
+ self.pkt.write(pack('<h', v))
+
+ def add_u16(self, v):
+ self.pkt.write(pack('<H', v))
+
+ def add_s32(self, v):
+ self.pkt.write(pack('<i', v))
+
+ def add_u32(self, v):
+ self.pkt.write(pack('<I', v))
+
+ def parse_string(self, n_bytes=0):
+ if n_bytes:
+ return unpack('%ss' % n_bytes,
+ self.pkt.read(n_bytes))[0]
+ else:
+ return self.pkt.read()
+
+ def parse_s8(self):
+ return unpack('<b', self.pkt.read(1))[0]
+
+ def parse_u8(self):
+ return unpack('<B', self.pkt.read(1))[0]
+
+ def parse_s16(self):
+ return unpack('<h', self.pkt.read(2))[0]
+
+ def parse_u16(self):
+ return unpack('<H', self.pkt.read(2))[0]
+
+ def parse_s32(self):
+ return unpack('<i', self.pkt.read(4))[0]
+
+ def parse_u32(self):
+ return unpack('<I', self.pkt.read(4))[0]
+
+ def check_status(self):
+ nxt.error.check_status(self.parse_u8())
+
+import nxt.direct
+import nxt.system
+
+OPCODES = dict(nxt.system.OPCODES)
+OPCODES.update(nxt.direct.OPCODES)
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/usbsock.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/usbsock.py
new file mode 100644
index 0000000..5ff2b36
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/usbsock.py
@@ -0,0 +1,133 @@
+# nxt.usbsock module -- USB socket communication with LEGO Minstorms NXT
+# Copyright (C) 2006, 2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+USB_BUFSIZE = 64
+
+try:
+ import fantomglue as usb
+except ImportError:
+ import pyusbglue as usb
+
+from nxt.brick import Brick
+
+class USBSock(object):
+ 'Object for USB connection to NXT'
+
+ bsize = 60 # USB socket block size
+
+ def __init__(self, device):
+ self.sock = usb.USBSocket(device)
+ self.debug = True
+
+ def __str__(self):
+ return 'USB (%s)' % (self.sock.device_name())
+
+ def connect(self):
+ 'Use to connect to NXT.'
+ if self.debug:
+ print 'Connecting via USB...'
+ self.sock.connect()
+ return Brick(self)
+
+ def close(self):
+ 'Use to close the connection.'
+ if self.debug:
+ print 'Closing USB connection...'
+ self.sock.close()
+ #self.sock = None
+ if self.debug:
+ print 'USB connection closed.'
+
+ def send(self, data):
+ 'Use to send raw data over USB connection ***ADVANCED USERS ONLY***'
+ if self.debug:
+ print 'Send:',
+ print ':'.join('%02x' % ord(c) for c in data)
+ self.sock.send(data)
+
+ def recv(self, num_bytes=USB_BUFSIZE):
+ 'Use to recieve raw data over USB connection ***ADVANCED USERS ONLY***'
+ data = self.sock.recv(num_bytes)
+ if self.debug:
+ print 'Recv:',
+ print ':'.join('%02x' % ord(c) for c in data)
+ return data
+
+ def __del__(self):
+ """Destroy interface."""
+ if self.sock is not None:
+ del self.sock
+ if self.debug:
+ print 'Deleted USBSocket.'
+
+
+def _check_brick(arg, value):
+ return arg is None or arg == value
+
+def find_bricks(host=None, name=None):
+ get_info = False
+ 'Use to look for NXTs connected by USB only. ***ADVANCED USERS ONLY***'
+ for d in usb.find_devices(lookup_names=True):
+ if get_info:
+ # pyfantom specific debug info
+ print " firmware version:", d.get_firmware_version()
+ print " get device info:", d.get_device_info()
+ rs = d.get_resource_string()
+ print " resource string:", rs
+ # FIXME: probably should check host and name
+ yield USBSock(d)
+
+if __name__ == '__main__':
+ write_read = True
+ socks = find_bricks()
+ for s in socks:
+ #llsocks = usb.find_devices()
+ #for ls in llsocks:
+ #s = USBSock(ls)
+ print s.sock
+ brick = s.connect()
+ if write_read:
+ import struct
+ # Write VERSION SYS_CMD.
+ # Query:
+ # SYS_CMD: 0x01
+ # VERSION: 0x88
+ cmd = struct.pack('2B', 0x01, 0x88)
+ brick.sock.send(cmd)
+ #s.send(cmd)
+ #s.sock.send(cmd)
+ print "wrote", len(cmd)
+ # Response:
+ # REPLY_CMD: 0x02
+ # VERSION: 0x88
+ # SUCCESS: 0x00
+ # PROTOCOL_VERSION minor
+ # PROTOCOL_VERSION major
+ # FIRMWARE_VERSION minor
+ # FIRMWARE_VERSION major
+ rep = brick.sock.recv(7)
+ #rep = s.recv(7)
+ #rep = s.sock.recv(7)
+ print "read", struct.unpack('%dB' % len(rep), rep)
+ # Same thing, without response.
+ #cmd = struct.pack('2B', 0x81, 0x88)
+ #brick.sock.send(cmd)
+ #print "wrote", cmd
+ #rep = brick.sock.recv()
+ #print "read", struct.unpack('%dB' % len(rep), rep)
+ #s.close()
+ #del s
+ #brick.sock.close()
+ del brick
+
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/utils.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/utils.py
new file mode 100644
index 0000000..98ba6df
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/utils.py
@@ -0,0 +1,33 @@
+# nxt.utils module -- Generic utilities to support other modules
+# Copyright (C) 2010 Vladimir Moskva
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from collections import defaultdict
+
+def parse_command_line_arguments(arguments):
+ keyword_parameters = defaultdict(lambda: None)
+ parameters = []
+ current_key = None
+
+ for argument in arguments[1:]:
+ if argument in ('-h', '--host'):
+ current_key = 'host'
+ else:
+ if current_key is not None:
+ if argument.startswith('-'):
+ raise Exception('Invalid arguments')
+ keyword_parameters[current_key] = argument
+ current_key = None
+ else:
+ parameters.append(argument)
+ return parameters, keyword_parameters
+