aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor
diff options
context:
space:
mode:
authorTat-Chee Wan (USM)2011-06-03 12:11:27 +0800
committerTat-Chee Wan (USM)2011-06-03 12:11:27 +0800
commit724f0cab2008109102428c6a7393f1ec85281574 (patch)
tree7484bad0c0c44ae0042a5c9885f59391bd164f14 /AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor
parentb45fb85344d2299d3948191cbbf0bae18dc5a946 (diff)
parenta79dc637d60b33032e951f5338fdd9b6a47a5b46 (diff)
Merge branch 'master' of ssh://svc.cs.usm.my/~/gitrepo-bare/armdebug
Diffstat (limited to 'AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor')
-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
7 files changed, 0 insertions, 1965 deletions
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'