summaryrefslogtreecommitdiff
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, 0 insertions, 4008 deletions
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/__init__.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/__init__.py
deleted file mode 100644
index 01be052..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/__init__.py
+++ /dev/null
@@ -1,17 +0,0 @@
-# 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
deleted file mode 100644
index 99917f2..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/bluesock.py
+++ /dev/null
@@ -1,82 +0,0 @@
-# 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
deleted file mode 100644
index 240ba27..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/brick.py
+++ /dev/null
@@ -1,226 +0,0 @@
-# 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
deleted file mode 100644
index 50b7f2a..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/direct.py
+++ /dev/null
@@ -1,216 +0,0 @@
-# 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
deleted file mode 100644
index 3d14497..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/error.py
+++ /dev/null
@@ -1,87 +0,0 @@
-# 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
deleted file mode 100644
index 697655f..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/fantomglue.py
+++ /dev/null
@@ -1,211 +0,0 @@
-# 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
deleted file mode 100644
index f2ab92f..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/lightblueglue.py
+++ /dev/null
@@ -1,53 +0,0 @@
-# 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
deleted file mode 100644
index 4adc680..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/locator.py
+++ /dev/null
@@ -1,57 +0,0 @@
-# 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
deleted file mode 100644
index f29a5a5..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/motor.py
+++ /dev/null
@@ -1,431 +0,0 @@
-# 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
deleted file mode 100644
index b140dfe..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/pyusbglue.py
+++ /dev/null
@@ -1,82 +0,0 @@
-# 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
deleted file mode 100644
index 8f2a337..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py
+++ /dev/null
@@ -1,50 +0,0 @@
-# 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
deleted file mode 100644
index e8d9b7b..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py
+++ /dev/null
@@ -1,41 +0,0 @@
-# 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
deleted file mode 100644
index 5afd6c8..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py
+++ /dev/null
@@ -1,67 +0,0 @@
-# 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
deleted file mode 100644
index dbc730f..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py
+++ /dev/null
@@ -1,227 +0,0 @@
-# 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
deleted file mode 100644
index b3d792f..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py
+++ /dev/null
@@ -1,154 +0,0 @@
-# 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
deleted file mode 100644
index 3c152d3..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
+++ /dev/null
@@ -1,611 +0,0 @@
-# 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
deleted file mode 100644
index de6c7ee..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py
+++ /dev/null
@@ -1,815 +0,0 @@
-# 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
deleted file mode 100644
index a82d5d6..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/system.py
+++ /dev/null
@@ -1,297 +0,0 @@
-# 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
deleted file mode 100644
index 1fc5b3e..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/telegram.py
+++ /dev/null
@@ -1,118 +0,0 @@
-# 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
deleted file mode 100644
index 5ff2b36..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/usbsock.py
+++ /dev/null
@@ -1,133 +0,0 @@
-# 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
deleted file mode 100644
index 98ba6df..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/utils.py
+++ /dev/null
@@ -1,33 +0,0 @@
-# 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
-