aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor
diff options
context:
space:
mode:
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, 1965 insertions, 0 deletions
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py
new file mode 100644
index 0000000..8f2a337
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/__init__.py
@@ -0,0 +1,50 @@
+# nxt.sensor module -- Classes to read LEGO Mindstorms NXT sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from .common import *
+from .analog import BaseAnalogSensor
+from .digital import BaseDigitalSensor, find_class
+from .generic import Touch, Light, Sound, Ultrasonic, Color20
+import mindsensors
+MSSumoEyes = mindsensors.SumoEyes
+MSCompassv2 = mindsensors.Compassv2
+MSDIST = mindsensors.DIST
+MSRTC = mindsensors.RTC
+MSACCL = mindsensors.ACCL
+MSServo = mindsensors.Servo
+MSMTRMUX = mindsensors.MTRMUX
+MSLineLeader = mindsensors.LineLeader
+MSMMX = mindsensors.MMX
+MSPS2 = mindsensors.PS2
+MSHID = mindsensors.HID
+import hitechnic
+HTCompass = hitechnic.Compass
+HTAccelerometer = hitechnic.Accelerometer
+HTGyro = hitechnic.Gyro
+HTColorv2 = hitechnic.Colorv2
+HTEOPD = hitechnic.EOPD
+HTIRReceiver = hitechnic.IRReceiver
+HTIRSeekerv2 = hitechnic.IRSeekerv2
+HTPrototype = hitechnic.Prototype
+
+
+def get_sensor(brick, port):
+ """Tries to detect the sensor type and return the correct sensor
+object. Does not work for sensors with no identification information (such as
+all analog sensors or the MindSensors RTC.
+ """
+ base_sensor = BaseDigitalSensor(brick, port, False)
+ info = base_sensor.get_sensor_info()
+ return find_class(info)(brick, port, check_compatible=False)
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py
new file mode 100644
index 0000000..e8d9b7b
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/analog.py
@@ -0,0 +1,41 @@
+# nxt.sensor.analog module -- submodule for use with analog sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from common import *
+
+
+class RawReading: # can be converted to the old version
+ """A pseudo-structure holding the raw sensor values as returned by the NXT
+ brick.
+ """
+ def __init__(self, values):
+ (self.port, self.valid, self.calibrated, self.sensor_type, self.mode,
+ self.raw_ad_value, self.normalized_ad_value, self.scaled_value,
+ self.calibrated_value) = values
+
+ def __repr__(self):
+ return str((self.port, self.valid, self.calibrated, self.sensor_type, self.mode,
+ self.raw_ad_value, self.normalized_ad_value, self.scaled_value,
+ self.calibrated_value))
+
+
+class BaseAnalogSensor(Sensor):
+ """Object for analog sensors."""
+ def get_input_values(self):
+ """Returns the raw sensor values as returned by the NXT brick."""
+ return RawReading(self.brick.get_input_values(self.port))
+
+ def reset_input_scaled_value(self):
+ self.brick.reset_input_scaled_value()
+
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py
new file mode 100644
index 0000000..5afd6c8
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/common.py
@@ -0,0 +1,67 @@
+# nxt.sensor.common module -- submodule with stuff useful in all sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+PORT_1 = 0x00
+PORT_2 = 0x01
+PORT_3 = 0x02
+PORT_4 = 0x03
+
+class Type(object):
+ 'Namespace for enumeration of the type of sensor'
+ # NOTE: just a namespace (enumeration)
+ NO_SENSOR = 0x00
+ SWITCH = 0x01 # Touch sensor
+ TEMPERATURE = 0x02
+ REFLECTION = 0x03
+ ANGLE = 0x04
+ LIGHT_ACTIVE = 0x05 # Light sensor (illuminated)
+ LIGHT_INACTIVE = 0x06 # Light sensor (ambient)
+ SOUND_DB = 0x07 # Sound sensor (unadjusted)
+ SOUND_DBA = 0x08 # Sound sensor (adjusted)
+ CUSTOM = 0x09
+ LOW_SPEED = 0x0A
+ LOW_SPEED_9V = 0x0B # Low-speed I2C (Ultrasonic sensor)
+ HIGH_SPEED = 0x0C #Possibly other mode for I2C; may be used by future sensors.
+ COLORFULL = 0x0D #NXT 2.0 color sensor in full color mode (color sensor mode)
+ COLORRED = 0x0E #NXT 2.0 color sensor with red light on (light sensor mode)
+ COLORGREEN = 0x0F #NXT 2.0 color sensor with green light on (light sensor mode)
+ COLORBLUE = 0x10 #NXT 2.0 color sensor in with blue light on (light sensor mode)
+ COLORNONE = 0x11 #NXT 2.0 color sensor in with light off (light sensor mode)
+ COLOREXIT = 0x12 #NXT 2.0 color sensor internal state (not sure what this is for yet)
+
+
+class Mode(object):
+ 'Namespace for enumeration of the mode of sensor'
+ # NOTE: just a namespace (enumeration)
+ RAW = 0x00
+ BOOLEAN = 0x20
+ TRANSITION_CNT = 0x40
+ PERIOD_COUNTER = 0x60
+ PCT_FULL_SCALE = 0x80
+ CELSIUS = 0xA0
+ FAHRENHEIT = 0xC0
+ ANGLE_STEPS = 0xE0
+ MASK = 0xE0
+ MASK_SLOPE = 0x1F # Why isn't this slope thing documented?
+
+
+class Sensor(object):
+ 'Main sensor object'
+
+ def __init__(self, brick, port):
+ self.brick = brick
+ self.port = port
+
+ def set_input_mode(self, type_, mode):
+ self.brick.set_input_mode(self.port, type_, mode)
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py
new file mode 100644
index 0000000..dbc730f
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/digital.py
@@ -0,0 +1,227 @@
+# nxt.sensor module -- Classes to read LEGO Mindstorms NXT sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010,2011 Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from nxt.error import I2CError, I2CPendingError, DirProtError
+
+from common import *
+from time import sleep, time
+import struct
+
+
+class SensorInfo:
+ def __init__(self, version, product_id, sensor_type):
+ self.version = version
+ self.product_id = product_id
+ self.sensor_type = sensor_type
+
+ def clarifybinary(self, instr, label):
+ outstr = ''
+ outstr += (label + ': `' + instr + '`\n')
+ for char in instr:
+ outstr += (hex(ord(char))+', ')
+ outstr += ('\n')
+ return outstr
+
+ def __str__(self):
+ outstr = ''
+ outstr += (self.clarifybinary(str(self.version), 'Version'))
+ outstr += (self.clarifybinary(str(self.product_id), 'Product ID'))
+ outstr += (self.clarifybinary(str(self.sensor_type), 'Type'))
+ return outstr
+
+class BaseDigitalSensor(Sensor):
+ """Object for digital sensors. I2C_ADDRESS is the dictionary storing name
+ to i2c address mappings. It should be updated in every subclass. When
+ subclassing this class, make sure to call add_compatible_sensor to add
+ compatible sensor data.
+ """
+ I2C_DEV = 0x02
+ I2C_ADDRESS = {'version': (0x00, '8s'),
+ 'product_id': (0x08, '8s'),
+ 'sensor_type': (0x10, '8s'),
+# 'factory_zero': (0x11, 1), # is this really correct?
+ 'factory_scale_factor': (0x12, 'B'),
+ 'factory_scale_divisor': (0x13, 'B'),
+ }
+
+ def __init__(self, brick, port, check_compatible=True):
+ """Creates a BaseDigitalSensor. If check_compatible is True, queries
+ the sensor for its name, and if a wrong sensor class was used, prints
+ a warning.
+ """
+ super(BaseDigitalSensor, self).__init__(brick, port)
+ self.set_input_mode(Type.LOW_SPEED_9V, Mode.RAW)
+ self.last_poll = time()
+ self.poll_delay = 0.01
+ sleep(0.1) # Give I2C time to initialize
+ #Don't do type checking if this class has no compatible sensors listed.
+ try: self.compatible_sensors
+ except AttributeError: check_compatible = False
+ if check_compatible:
+ sensor = self.get_sensor_info()
+ if not sensor in self.compatible_sensors:
+ print ('WARNING: Wrong sensor class chosen for sensor ' +
+ str(sensor.product_id) + ' on port ' + str(port) + '. ' + """
+You may be using the wrong type of sensor or may have connected the cable
+incorrectly. If you are sure you're using the correct sensor class for the
+sensor, this message is likely in error and you should disregard it and file a
+bug report, including the output of get_sensor_info(). This message can be
+suppressed by passing "check_compatible=False" when creating the sensor object.""")
+
+ def _ls_get_status(self, n_bytes):
+ for n in range(10):
+ try:
+ b = self.brick.ls_get_status(self.port)
+ if b >= n_bytes:
+ return b
+ except I2CPendingError:
+ pass
+ raise I2CError, 'ls_get_status timeout'
+
+ def _i2c_command(self, address, value, format):
+ """Writes an i2c value to the given address. value must be a string. value is
+ a tuple of values corresponding to the given format.
+ """
+ value = struct.pack(format, *value)
+ msg = chr(self.I2C_DEV) + chr(address) + value
+ if self.last_poll+self.poll_delay > time():
+ diff = time() - self.last_poll
+ sleep(self.poll_delay - diff)
+ self.last_poll = time()
+ self.brick.ls_write(self.port, msg, 0)
+
+ def _i2c_query(self, address, format):
+ """Reads an i2c value from given address, and returns a value unpacked
+ according to the given format. Format is the same as in the struct
+ module. See http://docs.python.org/library/struct.html#format-strings
+ """
+ n_bytes = struct.calcsize(format)
+ msg = chr(self.I2C_DEV) + chr(address)
+ if self.last_poll+self.poll_delay > time():
+ diff = time() - self.last_poll
+ sleep(self.poll_delay - diff)
+ self.last_poll = time()
+ self.brick.ls_write(self.port, msg, n_bytes)
+ try:
+ self._ls_get_status(n_bytes)
+ finally:
+ #we should clear the buffer no matter what happens
+ data = self.brick.ls_read(self.port)
+ if len(data) < n_bytes:
+ raise I2CError, 'Read failure: Not enough bytes'
+ data = struct.unpack(format, data[-n_bytes:])
+ return data
+
+ def read_value(self, name):
+ """Reads a value from the sensor. Name must be a string found in
+ self.I2C_ADDRESS dictionary. Entries in self.I2C_ADDRESS are in the
+ name: (address, format) form, with format as in the struct module.
+ Be careful on unpacking single variables - struct module puts them in
+ tuples containing only one element.
+ """
+ address, fmt = self.I2C_ADDRESS[name]
+ for n in range(3):
+ try:
+ return self._i2c_query(address, fmt)
+ except DirProtError:
+ pass
+ raise I2CError, "read_value timeout"
+
+ def write_value(self, name, value):
+ """Writes value to the sensor. Name must be a string found in
+ self.I2C_ADDRESS dictionary. Entries in self.I2C_ADDRESS are in the
+ name: (address, format) form, with format as in the struct module.
+ value is a tuple of values corresponding to the format from
+ self.I2C_ADDRESS dictionary.
+ """
+ address, fmt = self.I2C_ADDRESS[name]
+ self._i2c_command(address, value, fmt)
+
+ def get_sensor_info(self):
+ version = self.read_value('version')[0].split('\0')[0]
+ product_id = self.read_value('product_id')[0].split('\0')[0]
+ sensor_type = self.read_value('sensor_type')[0].split('\0')[0]
+ return SensorInfo(version, product_id, sensor_type)
+
+ @classmethod
+ def add_compatible_sensor(cls, version, product_id, sensor_type):
+ """Adds an entry in the compatibility table for the sensor. If version
+ is None, then it's the default class for this model. If product_id is
+ None, then this is the default class for this vendor.
+ """
+ try:
+ cls.compatible_sensors
+ except AttributeError:
+ cls.compatible_sensors = []
+ finally:
+ cls.compatible_sensors.append(SCompatibility(version, product_id,
+ sensor_type))
+ add_mapping(cls, version, product_id, sensor_type)
+
+
+class SCompatibility(SensorInfo):
+ """An object that helps manage the sensor mappings"""
+ def __eq__(self, other):
+ if self.product_id is None:
+ return self.product_id == other.product_id
+ elif self.version is None:
+ return (self.product_id == other.product_id and
+ self.sensor_type == other.sensor_type)
+ else:
+ return (self.version == other.version and
+ self.product_id == other.product_id and
+ self.sensor_type == other.sensor_type)
+
+sensor_mappings = {}
+
+
+def add_mapping(cls, version, product_id, sensor_type):
+ "None means any other value"
+ if product_id not in sensor_mappings:
+ sensor_mappings[product_id] = {}
+ models = sensor_mappings[product_id]
+
+ if sensor_type is None:
+ if sensor_type in models:
+ raise ValueError('Already registered!')
+ models[sensor_type] = cls
+ return
+
+ if sensor_type not in models:
+ models[sensor_type] = {}
+ versions = models[sensor_type]
+
+ if version in versions:
+ raise ValueError('Already registered!')
+ else:
+ versions[version] = cls
+
+
+class SearchError(Exception):
+ pass
+
+
+def find_class(info):
+ """Returns an appropriate class for the given SensorInfo"""
+ dic = sensor_mappings
+ for val, msg in zip((info.product_id, info.sensor_type, info.version),
+ ('Vendor', 'Model', 'Version')):
+ if val in dic:
+ dic = dic[val]
+ elif None in dic:
+ dic = dic[None]
+ else:
+ raise SearchError(msg + ' not found')
+ return dic[info.sensor_type][None]
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py
new file mode 100644
index 0000000..b3d792f
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/generic.py
@@ -0,0 +1,154 @@
+# nxt.sensor.generic module -- Classes to read LEGO Mindstorms NXT sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 melducky, Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from .common import *
+from .digital import BaseDigitalSensor
+from .analog import BaseAnalogSensor
+
+
+class Touch(BaseAnalogSensor):
+ """The LEGO touch sensor"""
+
+ def __init__(self, brick, port):
+ super(Touch, self).__init__(brick, port)
+ self.set_input_mode(Type.SWITCH, Mode.BOOLEAN)
+
+ def is_pressed(self):
+ return bool(self.get_input_values().scaled_value)
+
+ get_sample = is_pressed
+
+
+class Light(BaseAnalogSensor):
+ """Object for light sensors. It automatically turns off light when it's not
+ used.
+ """
+ def __init__(self, brick, port, illuminated=True):
+ super(Light, self).__init__(brick, port)
+
+ def set_illuminated(self, active):
+ if active:
+ type_ = Type.LIGHT_ACTIVE
+ else:
+ type_ = Type.LIGHT_INACTIVE
+ self.set_input_mode(type_, Mode.RAW)
+
+ def get_lightness(self):
+ return self.get_input_values().scaled_value
+
+ get_sample = get_lightness
+
+
+class Sound(BaseAnalogSensor):
+ 'Object for sound sensors'
+
+ def __init__(self, brick, port, adjusted=True):
+ super(Sound, self).__init__(brick, port)
+ self.set_adjusted(adjusted)
+
+ def set_adjusted(self, active):
+ if active:
+ type_ = Type.SOUND_DBA
+ else:
+ type_ = Type.SOUND_DB
+ self.set_input_mode(type_, Mode.RAW)
+
+ def get_loudness(self):
+ return self.get_input_values().scaled_value
+
+ get_sample = get_loudness
+
+
+class Ultrasonic(BaseDigitalSensor):
+ """Object for ultrasonic sensors"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'measurement_units': (0x14, '7s'),
+ 'continuous_measurement_interval': (0x40, 'B'),
+ 'command': (0x41, 'B'),
+ 'measurement_byte_0': (0x42, 'B'),
+ 'measurements': (0x42, '8B'),
+ 'actual_scale_factor': (0x51, 'B'),
+ 'actual_scale_divisor': (0x52, 'B'),
+ })
+
+ class Commands:
+ 'These are for passing to command()'
+ OFF = 0x00
+ SINGLE_SHOT = 0x01
+ CONTINUOUS_MEASUREMENT = 0x02
+ EVENT_CAPTURE = 0x03 #Optimize results when other Ultrasonic sensors running
+ REQUEST_WARM_RESET = 0x04
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Ultrasonic, self).__init__(brick, port, check_compatible)
+ self.set_input_mode(Type.LOW_SPEED_9V, Mode.RAW)
+
+ def get_distance(self):
+ 'Function to get data from the ultrasonic sensor'
+ return self.read_value('measurement_byte_0')[0]
+
+ get_sample = get_distance
+
+ def get_measurement_units(self):
+ return self.read_value('measurement_units')[0].split('\0')[0]
+
+ def get_all_measurements(self):
+ "Returns all the past readings in measurement_byte_0 through 7"
+ return self.read_value('measurements')
+
+ def get_measurement_no(self, number):
+ "Returns measurement_byte_number"
+ if not 0 <= number < 8:
+ raise ValueError('Measurements are numbered 0 to 7, not ' + str(number))
+ base_address, format = self.I2C_ADDRESS['measurement_byte_0']
+ return self._i2c_query(base_address + number, format)[0]
+
+ def command(self, command):
+ self.write_value('command', (command, ))
+
+ def get_interval(self):
+ 'Get the sample interval for continuous measurement mode -- Unknown units'
+ return self.read_value('continuous_measurement_interval')
+
+ def set_interval(self, interval):
+ """Set the sample interval for continuous measurement mode.
+Unknown units; default is 1"""
+ self.write_value('continuous_measurement_interval', interval)
+
+Ultrasonic.add_compatible_sensor(None, 'LEGO', 'Sonar') #Tested with version 'V1.0'
+
+
+class Color20(BaseAnalogSensor):
+ def __init__(self, brick, port):
+ super(Color20, self).__init__(brick, port)
+ self.set_light_color(Type.COLORFULL)
+
+ def set_light_color(self, color):
+ """color should be one of the COLOR* Type namespace values, e.g. Type.COLORBLUE"""
+ self.set_input_mode(color, Mode.RAW)
+
+ def get_light_color(self):
+ """Returns one of the COLOR* Type namespace values, e.g. Type.COLORRED"""
+ return self.get_input_values().sensor_type
+
+ def get_reflected_light(self, color):
+ self.set_light_color(color)
+ return self.get_input_values().scaled_value
+
+ def get_color(self):
+ self.get_reflected_light(Type.COLORFULL)
+ return self.get_input_values().scaled_value
+
+ get_sample = get_color
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
new file mode 100644
index 0000000..3c152d3
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
@@ -0,0 +1,611 @@
+# nxt.sensor.hitechnic module -- Classes to read HiTechnic sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 rhn, Marcus Wanner, melducky, Samuel Leeman-Munk
+# Copyright (C) 2011 jerradgenson, Marcus Wanner
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+from .common import *
+from .digital import BaseDigitalSensor
+from .analog import BaseAnalogSensor
+
+
+class Compass(BaseDigitalSensor):
+ """Hitechnic compass sensor."""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'mode': (0x41, 'B'),
+ 'heading': (0x42, 'B'),
+ 'adder' : (0x43, 'B'),
+ })
+
+ class Modes:
+ MEASUREMENT = 0x00
+ CALIBRATION = 0x43
+ CALIBRATION_FAILED = 0x02
+
+ def get_heading(self):
+ """Returns heading from North in degrees."""
+
+ two_degree_heading = self.read_value('heading')[0]
+ adder = self.read_value('adder')[0]
+ heading = two_degree_heading * 2 + adder
+
+ return heading
+
+ get_sample = get_heading
+
+ def get_relative_heading(self,target=0):
+ rheading = self.get_sample()-target
+ if rheading > 180:
+ rheading -= 360
+ elif rheading < -180:
+ rheading += 360
+ return rheading
+
+ def is_in_range(self,minval,maxval):
+ """This deserves a little explanation:
+if max > min, it's straightforward, but
+if min > max, it switches the values of max and min
+and returns true if heading is NOT between the new max and min
+ """
+ if minval > maxval:
+ (maxval,minval) = (minval,maxval)
+ inverted = True
+ else:
+ inverted = False
+ heading = self.get_sample()
+ in_range = (heading > minval) and (heading < maxval)
+ #an xor handles the reversal
+ #a faster, more compact way of saying
+ #if !reversed return in_range
+ #if reversed return !in_range
+ return bool(inverted) ^ bool(in_range)
+
+ def get_mode(self):
+ return self.read_value('mode')[0]
+
+ def set_mode(self, mode):
+ if mode != self.Modes.MEASUREMENT and \
+ mode != self.Modes.CALIBRATION:
+ raise ValueError('Invalid mode specified: ' + str(mode))
+ self.write_value('mode', (mode, ))
+
+Compass.add_compatible_sensor(None, 'HiTechnc', 'Compass ') #Tested with version '\xfdV1.23 '
+Compass.add_compatible_sensor(None, 'HITECHNC', 'Compass ') #Tested with version '\xfdV2.1 '
+
+
+class Accelerometer(BaseDigitalSensor):
+ 'Object for Accelerometer sensors. Thanks to Paulo Vieira.'
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'x_axis_high': (0x42, 'b'),
+ 'y_axis_high': (0x43, 'b'),
+ 'z_axis_high': (0x44, 'b'),
+ 'xyz_short': (0x42, '3b'),
+ 'all_data': (0x42, '3b3B')
+ })
+
+ class Acceleration:
+ def __init__(self, x, y, z):
+ self.x, self.y, self.z = x, y, z
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Accelerometer, self).__init__(brick, port, check_compatible)
+
+ def get_acceleration(self):
+ """Returns the acceleration along x, y, z axes. Units are unknown to me.
+ """
+ xh, yh, zh, xl, yl, zl = self.read_value('all_data')
+ x = xh << 2 + xl
+ y = yh << 2 + yl
+ z = zh << 2 + yl
+ return self.Acceleration(x, y, z)
+
+ get_sample = get_acceleration
+
+Accelerometer.add_compatible_sensor(None, 'HiTechnc', 'Accel. ')
+Accelerometer.add_compatible_sensor(None, 'HITECHNC', 'Accel. ') #Tested with version '\xfdV1.1 '
+
+
+class IRReceiver(BaseDigitalSensor):
+ """Object for HiTechnic IRReceiver sensors for use with LEGO Power Functions IR
+Remotes. Coded to HiTechnic's specs for the sensor but not tested. Please report
+whether this worked for you or not!
+ """
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'm1A': (0x42, 'b'),
+ 'm1B': (0x43, 'b'),
+ 'm2A': (0x44, 'b'),
+ 'm2B': (0x45, 'b'),
+ 'm3A': (0x46, 'b'),
+ 'm3B': (0x47, 'b'),
+ 'm4A': (0x48, 'b'),
+ 'm4B': (0x49, 'b'),
+ 'all_data': (0x42, '8b')
+ })
+
+ class SpeedReading:
+ def __init__(self, m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B):
+ self.m1A, self.m1B, self.m2A, self.m2B, self.m3A, self.m3B, self.m4A, self.m4B = m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B
+ self.channel_1 = (m1A, m1B)
+ self.channel_2 = (m2A, m2B)
+ self.channel_3 = (m3A, m3B)
+ self.channel_4 = (m4A, m4B)
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(IRReceiver, self).__init__(brick, port, check_compatible)
+
+ def get_speeds(self):
+ """Returns the motor speeds for motors A and B on channels 1-4.
+Values are -128, -100, -86, -72, -58, -44, -30, -16, 0, 16, 30, 44, 58, 72, 86
+and 100. -128 specifies motor brake mode. Note that no motors are actually
+being controlled here!
+ """
+ m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B = self.read_value('all_data')
+ return self.SpeedReading(m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B)
+
+ get_sample = get_speeds
+
+IRReceiver.add_compatible_sensor(None, 'HiTechnc', 'IRRecv ')
+IRReceiver.add_compatible_sensor(None, 'HITECHNC', 'IRRecv ')
+
+
+class IRSeekerv2(BaseDigitalSensor):
+ """Object for HiTechnic IRSeeker sensors. Coded to HiTechnic's specs for the sensor
+but not tested. Please report whether this worked for you or not!
+ """
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'dspmode': (0x41, 'B'),
+ 'DC_direction': (0x42, 'B'),
+ 'DC_sensor_1': (0x43, 'B'),
+ 'DC_sensor_2': (0x44, 'B'),
+ 'DC_sensor_3': (0x45, 'B'),
+ 'DC_sensor_4': (0x46, 'B'),
+ 'DC_sensor_5': (0x47, 'B'),
+ 'DC_sensor_mean': (0x48, 'B'),
+ 'all_DC': (0x42, '7B'),
+ 'AC_direction': (0x49, 'B'),
+ 'AC_sensor_1': (0x4A, 'B'),
+ 'AC_sensor_2': (0x4B, 'B'),
+ 'AC_sensor_3': (0x4C, 'B'),
+ 'AC_sensor_4': (0x4D, 'B'),
+ 'AC_sensor_5': (0x4E, 'B'),
+ 'all_AC': (0x49, '6B')
+ })
+ I2C_DEV = 0x10 #different from standard 0x02
+
+ class DSPModes:
+ #Modes for modulated (AC) data.
+ AC_DSP_1200Hz = 0x00
+ AC_DSP_600Hz = 0x01
+
+ class _data:
+ def get_dir_brightness(self, direction):
+ "Gets the brightness of a given direction (1-9)."
+ if direction%2 == 1: #if it's an odd number
+ exec("val = self.sensor_%d" % ((direction-1)/2+1))
+ else:
+ exec("val = (self.sensor_%d+self.sensor_%d)/2" % (direction/2, (direction/2)+1))
+ return val
+
+ class DCData(_data):
+ def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean):
+ self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5, self.sensor_mean = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean
+
+ class ACData(_data):
+ def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5):
+ self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5 = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5
+
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(IRSeekerv2, self).__init__(brick, port, check_compatible)
+
+ def get_dc_values(self):
+ """Returns the unmodulated (DC) values.
+ """
+ direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean = self.read_value('all_DC')
+ return self.DCData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean)
+
+ def get_ac_values(self):
+ """Returns the modulated (AC) values. 600Hz and 1200Hz modes can be selected
+between by using the set_dsp_mode() function.
+ """
+ direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5 = self.read_value('all_AC')
+ return self.ACData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5)
+
+ def get_dsp_mode(self):
+ return self.read_value('dspmode')[0]
+
+ def set_dsp_mode(self, mode):
+ self.write_value('dspmode', (mode, ))
+
+ get_sample = get_ac_values
+
+IRSeekerv2.add_compatible_sensor(None, 'HiTechnc', 'NewIRDir')
+IRSeekerv2.add_compatible_sensor(None, 'HITECHNC', 'NewIRDir')
+
+
+class EOPD(BaseAnalogSensor):
+ """Object for HiTechnic Electro-Optical Proximity Detection sensors.
+ """
+
+ # To be divided by processed value.
+ _SCALE_CONSTANT = 250
+
+ # Maximum distance the sensor can detect.
+ _MAX_DISTANCE = 1023
+
+ def __init__(self, brick, port):
+ super(EOPD, self).__init__(brick, port)
+ from math import sqrt
+ self.sqrt = sqrt
+
+ def set_range_long(self):
+ ''' Choose this mode to increase the sensitivity
+ of the EOPD sensor by approximately 4x. May
+ cause sensor overload.
+ '''
+
+ self.set_input_mode(Type.LIGHT_ACTIVE, Mode.RAW)
+
+ def set_range_short(self):
+ ''' Choose this mode to prevent the EOPD sensor from
+ being overloaded by white objects.
+ '''
+
+ self.set_input_mode(Type.LIGHT_INACTIVE, Mode.RAW)
+
+ def get_raw_value(self):
+ '''Unscaled value read from sensor.'''
+
+ return self._MAX_DISTANCE - self.get_input_values().raw_ad_value
+
+ def get_processed_value(self):
+ '''Derived from the square root of the raw value.'''
+
+ return self.sqrt(self.get_raw_value())
+
+ def get_scaled_value(self):
+ ''' Returns a value that will scale linearly as distance
+ from target changes. This is the method that should
+ generally be called to get EOPD sensor data.
+ '''
+
+ try:
+ result = self._SCALE_CONSTANT / self.get_processed_value()
+ return result
+
+ except ZeroDivisionError:
+ return self._SCALE_CONSTANT
+
+ get_sample = get_scaled_value
+
+
+class Colorv2(BaseDigitalSensor):
+ """Object for HiTechnic Color v2 Sensors. Coded to HiTechnic's specs for the sensor
+but not tested. Please report whether this worked for you or not!"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'mode': (0x41, 'B'),
+ 'number': (0x42, 'B'),
+ 'red': (0x43, 'B'),
+ 'green': (0x44, 'B'),
+ 'blue': (0x45, 'B'),
+ 'white': (0x46, 'B'),
+ 'index': (0x47, 'B'),
+ 'normred': (0x48, 'B'),
+ 'normgreen': (0x49, 'B'),
+ 'normblue': (0x4A, 'B'),
+ 'all_data': (0x42, '9B'),
+ 'rawred': (0x42, '<H'),
+ 'rawgreen': (0x44, '<H'),
+ 'rawblue': (0x46, '<H'),
+ 'rawwhite': (0x48, '<H'),
+ 'all_raw_data': (0x42, '<4H')
+ })
+
+ class Modes:
+ ACTIVE = 0x00 #get measurements using get_active_color
+ PASSIVE = 0x01 #get measurements using get_passive_color
+ RAW = 0x03 #get measurements using get_passive_color
+ BLACK_CALIBRATION = 0x42 #hold away from objects, results saved in EEPROM
+ WHITE_CALIBRATION = 0x43 #hold in front of white surface, results saved in EEPROM
+ LED_POWER_LOW = 0x4C #saved in EEPROM, must calibrate after using
+ LED_POWER_HIGH = 0x48 #saved in EEPROM, must calibrate after using
+ RANGE_NEAR = 0x4E #saved in EEPROM, only affects active mode
+ RANGE_FAR = 0x46 #saved in EEPROM, only affects active mode, more susceptable to noise
+ FREQ_50 = 0x35 #saved in EEPROM, use when local wall power is 50Hz
+ FREQ_60 = 0x36 #saved in EEPROM, use when local wall power is 60Hz
+
+ class ActiveData:
+ def __init__(self, number, red, green, blue, white, index, normred, normgreen, normblue):
+ self.number, self.red, self.green, self.blue, self.white, self.index, self.normred, self.normgreen, self.normblue = number, red, green, blue, white, index, normred, normgreen, normblue
+
+ class PassiveData:
+ #also holds raw mode data
+ def __init__(self, red, green, blue, white):
+ self.red, self.green, self.blue, self.white = red, green, blue, white
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Colorv2, self).__init__(brick, port, check_compatible)
+
+ def get_active_color(self):
+ """Returns color values when in active mode.
+ """
+ number, red, green, blue, white, index, normred, normgreen, normblue = self.read_value('all_data')
+ return self.ActiveData(number, red, green, blue, white, index, normred, normgreen, normblue)
+
+ get_sample = get_active_color
+
+ def get_passive_color(self):
+ """Returns color values when in passive or raw mode.
+ """
+ red, green, blue, white = self.read_value('all_raw_data')
+ return self.PassiveData(red, green, blue, white)
+
+ def get_mode(self):
+ return self.read_value('mode')[0]
+
+ def set_mode(self, mode):
+ self.write_value('mode', (mode, ))
+
+Colorv2.add_compatible_sensor(None, 'HiTechnc', 'ColorPD')
+Colorv2.add_compatible_sensor(None, 'HITECHNC', 'ColorPD')
+Colorv2.add_compatible_sensor(None, 'HiTechnc', 'ColorPD ')
+Colorv2.add_compatible_sensor(None, 'HITECHNC', 'ColorPD ')
+
+
+class Gyro(BaseAnalogSensor):
+ 'Object for gyro sensors'
+#This class is for the hitechnic gryo sensor. When the gryo is not
+#moving there will be a constant offset that will change with
+#temperature and other ambient factors. The calibrate() function
+#takes the currect value and uses it to offset subsequesnt ones.
+
+ def __init__(self, brick, port):
+ super(Gyro, self).__init__(brick, port)
+ self.set_input_mode(Type.ANGLE, Mode.RAW)
+ self.offset = 0
+
+ def get_rotation_speed(self):
+ return self.get_input_values().scaled_value - self.offset
+
+ def set_zero(self, value):
+ self.offset = value
+
+ def calibrate(self):
+ self.set_zero(self.get_rotation_speed())
+
+ get_sample = get_rotation_speed
+
+
+class Prototype(BaseDigitalSensor):
+ """Object for HiTechnic sensor prototype boards. Coded to HiTechnic's specs but not
+tested. Please report whether this worked for you or not!
+ """
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'A0': (0x42, '<H'),
+ 'A0': (0x44, '<H'),
+ 'A0': (0x46, '<H'),
+ 'A0': (0x48, '<H'),
+ 'A0': (0x4A, '<H'),
+ 'all_analog': (0x42, '<5H'),
+ 'digital_in': (0x4C, 'B'),
+ 'digital_out': (0x4D, 'B'),
+ 'digital_cont': (0x4E, 'B'),
+ 'sample_time': (0x4F, 'B'),
+ })
+
+ class Digital_Data():
+ """Container for 6 bits of digital data. Takes an integer or a list of six bools
+and can be converted into a list of bools or an integer."""
+ def __init__(self, pins):
+ if isinstance(pins, int):
+ self.dataint = pins
+ self.datalst = self.tolist(pins)
+ else:
+ self.dataint = self.toint(pins)
+ self.datalst = pins
+ self.d0, self.d1, self.d2, self.d3, self.d4, self.d5 = self.datalst
+
+ def tolist(self, val):
+ lst = []
+ for i in range(6):
+ lst.append(bool(val & 2**i))
+ return lst
+
+ def toint(self, lst):
+ val = 0
+ for i in range(6):
+ val += int(bool(lst[i])) * (2**i)
+ return val
+
+ def __int__(self):
+ return self.dataint
+
+ def __iter__(self):
+ return iter(self.datalst)
+
+ def __getitem__(self, i):
+ return self.datalst[i]
+
+ class Analog_Data():
+ def __init__(self, a0, a1, a2, a3, a4):
+ self.a0, self.a1, self.a2, self.a3, self.a4 = a0, a1, a2, a3, a4
+
+ def get_analog(self):
+ return Analog_Data(self.read_value('all_analog'))
+
+ def get_digital(self):
+ return Digital_Data(self.read_value('digital_in')[0])
+
+ def set_digital(self, pins):
+ """Can take a Digital_Data() object"""
+ self.write_value('digital_out', (int(pins), ))
+
+ def set_digital_modes(self, modes):
+ """Sets input/output mode of digital pins. Can take a Digital_Data() object."""
+ self.write_value('digital_cont', (int(modes), ))
+
+Prototype.add_compatible_sensor(None, 'HiTechnc', 'Proto ')
+
+
+class ServoCon(BaseDigitalSensor):
+ """Object for HiTechnic FIRST Servo Controllers. Coded to HiTechnic's specs for
+the sensor but not tested. Please report whether this worked for you or not!"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'status': (0x40, 'B'),
+ 'steptime': (0x41, 'B'),
+ 's1pos': (0x42, 'B'),
+ 's2pos': (0x43, 'B'),
+ 's3pos': (0x44, 'B'),
+ 'p4pos': (0x45, 'B'),
+ 'p5pos': (0x46, 'B'),
+ 'p6pos': (0x47, 'B'),
+ 'pwm': (0x46, 'B'),
+ })
+
+ class Status:
+ RUNNING = 0x00 #all motors stopped
+ STOPPED = 0x01 #motor(s) moving
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(ServoCon, self).__init__(brick, port, check_compatible)
+
+ def get_status(self):
+ """Returns the status of the motors. 0 for all stopped, 1 for
+some running.
+ """
+ return self.read_value('status')[0]
+
+ def set_step_time(self, time):
+ """Sets the step time (0-15).
+ """
+ self.write_value('steptime', (time, ))
+
+ def set_pos(self, num, pos):
+ """Sets the position of a server. num is the servo number (1-6),
+pos is the position (0-255).
+ """
+ self.write_value('s%dpos' % num, (pos, ))
+
+ def get_pwm(self):
+ """Gets the "PWM enable" value. The function of this value is
+nontrivial and can be found in the documentation for the sensor.
+ """
+ return self.read_value('pwm')[0]
+
+ def set_pwm(self, pwm):
+ """Sets the "PWM enable" value. The function of this value is
+nontrivial and can be found in the documentation for the sensor.
+ """
+ self.write_value('pwm', (pwm, ))
+
+ServoCon.add_compatible_sensor(None, 'HiTechnc', 'ServoCon')
+
+
+class MotorCon(BaseDigitalSensor):
+ """Object for HiTechnic FIRST Motor Controllers. Coded to HiTechnic's specs for
+the sensor but not tested. Please report whether this worked for you or not!"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({
+ 'm1enctarget': (0x40, '>l'),
+ 'm1mode': (0x44, 'B'),
+ 'm1power': (0x45, 'b'),
+ 'm2power': (0x46, 'b'),
+ 'm2mode': (0x47, 'B'),
+ 'm2enctarget': (0x48, '>l'),
+ 'm1enccurrent': (0x4c, '>l'),
+ 'm2enccurrent': (0x50, '>l'),
+ 'batteryvoltage': (0x54, '2B'),
+ 'm1gearratio': (0x56, 'b'),
+ 'm1pid': (0x57, '3B'),
+ 'm2gearratio': (0x5a, 'b'),
+ 'm2pid': (0x5b, '3B'),
+ })
+
+ class PID_Data():
+ def __init__(self, p, i, d):
+ self.p, self.i, self.d = p, i, d
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(MotorCon, self).__init__(brick, port, check_compatible)
+
+ def set_enc_target(self, mot, val):
+ """Set the encoder target (-2147483648-2147483647) for a motor
+ """
+ self.write_value('m%denctarget'%mot, (val, ))
+
+ def get_enc_target(self, mot):
+ """Get the encoder target for a motor
+ """
+ return self.read_value('m%denctarget'%mot)[0]
+
+ def get_enc_current(self, mot):
+ """Get the current encoder value for a motor
+ """
+ return self.read_value('m%denccurrent'%mot)[0]
+
+ def set_mode(self, mot, mode):
+ """Set the mode for a motor. This value is a bit mask and you can
+find details about it in the sensor's documentation.
+ """
+ self.write_value('m%dmode'%mot, (mode, ))
+
+ def get_mode(self, mot):
+ """Get the mode for a motor. This value is a bit mask and you can
+find details about it in the sensor's documentation.
+ """
+ return self.read_value('m%dmode'%mot)[0]
+
+ def set_power(self, mot, power):
+ """Set the power (-100-100) for a motor
+ """
+ self.write_value('m%dpower'%mot, (power, ))
+
+ def get_power(self, mot):
+ """Get the power for a motor
+ """
+ return self.read_value('m%dpower'%mot)[0]
+
+ def set_gear_ratio(self, mot, ratio):
+ """Set the gear ratio for a motor
+ """
+ self.write_value('m%dgearratio'%mot, (ratio, ))
+
+ def get_gear_ratio(self, mot):
+ """Get the gear ratio for a motor
+ """
+ return self.read_value('m%dgearratio'%mot)[0]
+
+ def set_pid(self, mot, piddata):
+ """Set the PID coefficients for a motor. Takes data in
+MotorCon.PID_Data(p, i, d) format.
+ """
+ self.write_value('m%dpid'%mot, (piddata.p, piddata.i, piddata.d))
+
+ def get_pid(self, mot):
+ """Get the PID coefficients for a motor. Returns a PID_Data() object.
+ """
+ p, i, d = self.read_value('m%dpid'%mot)
+ return self.PID_Data(p, i, d)
+
+ def get_battery_voltage(self):
+ """Gets the battery voltage (in millivolts/20)
+ """
+ high, low = self.read_value('bateryvoltage')[0]
+ return high << 2 + low
+
+MotorCon.add_compatible_sensor(None, 'HiTechnc', 'MotorCon')
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py
new file mode 100644
index 0000000..de6c7ee
--- /dev/null
+++ b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/mindsensors.py
@@ -0,0 +1,815 @@
+# nxt.sensor.mindsensors module -- Classes implementing Mindsensors sensors
+# Copyright (C) 2006,2007 Douglas P Lau
+# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
+# Copyright (C) 2010 Marcus Wanner, MindSensors
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+
+
+from .common import *
+from .digital import BaseDigitalSensor, SensorInfo
+from .analog import BaseAnalogSensor
+
+
+class SumoEyes(BaseAnalogSensor):
+ """The class to control Mindsensors Sumo sensor. Warning: long range not
+ working for my sensor.
+ """
+ #range: 5-10cm
+ class Reading:
+ """Contains the reading of SumoEyes sensor. left and right can be True or
+ False. If True, then there is something there, if False, then it's empty
+ there.
+ """
+ def __init__(self, raw_reading):
+ self.raw = raw_reading
+ val = raw_reading.normalized_ad_value # FIXME: make it rely on raw_ad_value
+ right = 600 < val < 700
+ both = 700 <= val < 900
+ left = 300 < val < 400
+ self.left = left or both
+ self.right = right or both
+
+ def __str__(self):
+ return '(left: ' + str(self.left) + ', right: ' + str(self.right) + ')'
+
+ def __init__(self, brick, port, long_range=False):
+ super(SumoEyes, self).__init__(brick, port)
+ self.set_long_range(long_range)
+
+ def set_long_range(self, val):
+ """Sets if the sensor should operate in long range mode (12 inches) or
+ the short range mode (6 in). val should be True or False.
+ """
+ if val:
+ type_ = Type.LIGHT_INACTIVE
+ else:
+ type_ = Type.LIGHT_ACTIVE
+ self.set_input_mode(type_, Mode.RAW)
+
+ def get_sample(self):
+ """Returns the processed meaningful values of the sensor"""
+ return self.Reading(self.get_input_values())
+
+
+class Compassv2(BaseDigitalSensor):
+ """Class for the now-discontinued CMPS-Nx sensor. Also works with v1.1 sensors.
+Note that when using a v1.x sensor, some of the commands are not supported!
+To determine your sensor's version, use get_sensor_info().version"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command': (0x41, '<B'),
+ 'heading': (0x42, '<H'),
+ 'x_offset': (0x44, '<h'), #unsure about signedness for this one
+ 'y_offset': (0x46, '<h'), #and this one
+ 'x_range': (0x48, '<H'),
+ 'y_range': (0x4A, '<H'),
+ 'x_raw': (0x4C, '<H'), #and this one
+ 'y_raw': (0x4E, '<H'), #and this one
+ })
+
+ class Commands:
+ AUTO_TRIG_ON = 'A'
+ AUTO_TRIG_OFF = 'S'
+ MAP_HEADING_BYTE = 'B' # map heading to 0-255 range
+ MAP_HEADING_INTEGER = 'I' # map heading to 0-36000 (or 3600) range
+ SAMPLING_50_HZ = 'E' # set sampling frequency to 50 Hz
+ SAMPLING_60_HZ = 'U' # set sampling frequency to 60 Hz
+ SET_ADPA_MODE_ON = 'N' # set ADPA mode on
+ SET_ADPA_MODE_OFF = 'O' # set ADPA mode off
+ BEGIN_CALIBRATION = 'C' # begin calibration
+ DONE_CALIBRATION = 'D' # done with calibration
+ LOAD_USER_CALIBRATION = 'L' # load user calibration value
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Compassv2, self).__init__(brick, port, check_compatible)
+ self.command(self.Commands.MAP_HEADING_INTEGER)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_heading(self):
+ return self.read_value('heading')[0]
+
+ get_sample = get_heading
+
+Compassv2.add_compatible_sensor(None, 'mndsnsrs', 'CMPS')
+
+
+class DIST(BaseDigitalSensor):
+ """Class for the Distance Infrared Sensor"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command': (0x41, '<B'),
+ 'distance': (0x42, '<H'),
+ 'voltage': (0x44, '<H'),
+ 'type': (0x50, '<B'),
+ 'no_of_data_points': (0x51, '<B'),
+ 'min_distance': (0x52, '<H'),
+ 'max_distance': (0x54, '<H'),
+ })
+
+ class Commands:
+ TYPE_GP2D12 = '1' #GP2D12 sensor Module
+ TYPE_GP2D120 = '2' #Short range sensor Module
+ TYPE_GP2Y0A21YK = '3' #Medium range sensor Module
+ TYPE_GP2Y0A02YK = '4' #Long range sensor Module
+ TYPE_CUSTOM = '5' #Custom sensor Module
+ POWER_ON = 'E' #Sensor module power on
+ POWER_OFF = 'D' #Sensor module power offset
+ ADPA_ON = 'N' #ADPA mode on
+ ADPA_OFF = 'O' #ADPA mode off (default)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_distance(self):
+ return self.read_value('distance')[0]
+
+ get_sample = get_distance
+
+ def get_type(self):
+ return self.read_value('type')[0]
+
+ def get_voltage(self):
+ return self.read_value('voltage')[0]
+
+ def get_min_distance(self):
+ return self.read_value('min_distance')[0]
+
+ def get_max_distance(self):
+ return self.read_value('max_distance')[0]
+
+DIST.add_compatible_sensor(None, 'mndsnsrs', 'DIST')
+
+
+class RTC(BaseDigitalSensor):
+ """Class for the RealTime Clock sensor"""
+ #TODO: Create a function to set the clock
+ #Has no indentification
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'seconds': (0x00, '<B'),
+ 'minutes': (0x01, '<B'),
+ 'hours': (0x02, '<B'),
+ 'day': (0x03, '<B'),
+ 'date': (0x04, '<B'),
+ 'month': (0x05, '<B'),
+ 'year': (0x06, '<B'),
+ })
+ I2C_DEV = 0xD0
+
+ def __init__(self, brick, port, check_compatible=False): #check_compatible must remain false due to no identification!
+ super(RTC, self).__init__(brick, port, check_compatible)
+
+ def get_seconds(self):
+ gs = self.read_value('seconds')[0]
+ gs2 = gs & 0xf # bitmasks
+ gs3 = gs & 0x70
+ gs3 = gs3 >> 4
+ return str(gs3) + str(gs2)
+
+ def get_minutes(self):
+ gm = self.read_value('minutes')[0]
+ gm2 = gm & 0xf
+ gm3 = gm & 0x70
+ gm3 = gm3 >> 4
+ return str(gm3) + str(gm2)
+
+ def get_hours(self):
+ gh = self.read_value('hours')[0]
+ gh2 = gh & 0xf
+ gh3 = gh & 0x30
+ gh3 = gh3 >> 4
+ return str(gh3) + str(gh2)
+
+ def get_day(self):
+ gwd = self.read_value('day')[0]
+ gwd = gwd & 0x07
+ return gwd
+
+ def get_month(self):
+ gmo = self.read_value('month')[0]
+ gmo2 = gmo & 0xf
+ gmo3 = gmo & 0x10
+ gmo3 = gmo3 >> 4
+ return str(gmo3) + str(gmo2)
+
+ def get_year(self):
+ """Last two digits (10 for 2010)"""
+ gy = self.read_value('year')[0]
+ gy2 = gy & 0xf
+ gy3 = gy & 0xF0
+ gy3 = gy3 >> 4
+ return str(gy3) + str(gy2)
+
+ def get_date(self):
+ gd = self.read_value('date')[0]
+ gd2 = gd & 0xf
+ gd3 = gd & 0x60
+ gd3 = gd3 >> 4
+ return str(gd3) + str(gd2)
+
+ def hour_mode(self, mode):
+ """Writes mode bit and re-enters hours, which is required"""
+ if mode == 12 or 24:
+ hm = self.read_value('hours')[0]
+ hm2 = hm & 0x40
+ hm2 = hm2 >> 6
+ if mode == 12 and hm2 == 0: #12_HOUR = 1
+ hm3 = hm + 64
+ self.write_value('hours', (hm3, ))
+ elif mode == 24 and hm2 == 1: #24_HOUR = 0
+ hm3 = hm - 64
+ self.write_value('hours', (hm3, ))
+ else:
+ print 'That mode is already selected!'
+ else:
+ raise ValueError('Must be 12 or 24!')
+
+ def get_mer(self):
+ mer = self.read_value('hours')[0]
+ mer2 = mer & 0x40
+ mer2 = mer2 >> 6
+ if mer2 == 1:
+ mer3 = mer & 0x20
+ mer3 = mer3 >> 0x10
+ return mer3
+ else:
+ print 'Cannot get mer! In 24-hour mode!'
+
+ def get_sample(self):
+ """Returns a struct_time() tuple which can be processed by the time module."""
+ import time
+ return time.struct_time((
+ int(self.get_year())+2000,
+ int(self.get_month()),
+ int(self.get_date()),
+ int(self.get_hours()),
+ int(self.get_minutes()),
+ int(self.get_seconds()),
+ int(self.get_day()),
+ 0, #Should be the Julian Day, but computing that is hard.
+ 0 #No daylight savings time to worry about here.
+ ))
+
+
+class ACCL(BaseDigitalSensor):
+ """Class for Accelerometer sensor"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'sensitivity': (0x19, 'B'),
+ 'command': (0x41, 'B'),
+ 'x_tilt': (0x42, 'b'),
+ 'y_tilt': (0x43, 'b'),
+ 'z_tilt': (0x44, 'b'),
+ 'all_tilt': (0x42, '3b'),
+
+ 'x_accel': (0x45, '<h'),
+ 'y_accel': (0x47, '<h'),
+ 'z_accel': (0x49, '<h'),
+ 'all_accel': (0x45, '<3h'),
+
+ 'x_offset': (0x4B, '<h'),
+ 'x_range': (0x4D, '<h'),
+
+ 'y_offset': (0x4F, '<h'),
+ 'y_range': (0x51, '<h'),
+
+ 'z_offset': (0x53, '<h'),
+ 'z_range': (0x55, '<h'),
+ })
+
+ class Commands:
+ SENS_15G = '1' #that's 1.5...Alt. 2.5G (sensors older than V3.20)
+ SENS_2G = '2' #Alt .3.3G
+ SENS_4G = '3' #Alt. 6.7G
+ SENS_6G = '4' #Alt. 10G
+ X_CALIBRATION = 'X' #Acquire X point calibration
+ X_CAL_AND_END = 'x' #X point calibration and end calibration
+ Y_CALIBRATION = 'Y' #Acquire Y point calibration
+ Y_CAL_AND_END = 'y' #Y point calibration and end calibration
+ Z_CALIBRATION = 'Z' #Acquire Z point calibration
+ Z_CAL_AND_END = 'z' #Z point calibration and end calibration
+ CAL_RESET = 'R' #Reset to factory set calibration
+ ADPA_ON = 'N' #Set ADPA mode On
+ ADPA_OFF = 'O' #Set ADPA mode Off (default)
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(ACCL, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_sensitivity(self):
+ return chr(self.read_value('sensitivity')[0])
+
+ def get_tilt(self, axis):
+ xyz = str(axis) + '_tilt'
+ return self.read_value(xyz)[0]
+
+ def get_all_tilt(self):
+ return self.read_value('all_tilt')
+
+ def get_accel(self, axis):
+ xyz = str(axis) + '_accel'
+ return self.read_value(xyz)[0]
+
+ def get_all_accel(self):
+ return self.read_value('all_accel')
+
+ get_sample = get_all_accel
+
+ def get_offset(self, axis):
+ xyz = str(axis) + '_offset'
+ return self.read_value(xyz)[0]
+
+ def get_range(self, axis):
+ xyz = str(axis) + '_range'
+ return self.read_value(xyz)[0]
+
+ def set_offset(self, axis, value):
+ xyz = str(axis) + '_offset'
+ self.write_value(xyz, (value, ))
+
+ def set_range(self, axis, value):
+ xyz = str(axis) + '_range'
+ self.write_value(xyz, (value, ))
+
+ACCL.add_compatible_sensor(None, 'mndsnsrs', 'ACCL-NX') #Tested with version 'V3.20'
+
+
+class MTRMUX(BaseDigitalSensor):
+ """Class for RCX Motor Multiplexer sensor"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ 'direction_m1': (0x42, '<B'),
+ 'speed_m1': (0x43, '<B'),
+ 'direction_m2': (0x44, '<B'),
+ 'speed_m2': (0x45, '<B'),
+ 'direction_m3': (0x46, '<B'),
+ 'speed_m3': (0x47, '<B'),
+ 'direction_m4': (0x48, '<B'),
+ 'speed_m4': (0x49, '<B'),
+ })
+ I2C_DEV = 0xB4
+
+ class Commands:
+ FLOAT = 0x00
+ FORWARD = 0x01
+ REVERSE = 0x02
+ BRAKE = 0x03
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(MTRMUX, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ self.write_value('command', (command, ))
+
+ def set_direction(self, number, value):
+ addressname = 'direction_m' + str(number)
+ self.write_value(addressname, (value, ))
+
+ def set_speed(self, number, value):
+ addressname = 'speed_m' + str(number)
+ self.write_value(addressname, (value, ))
+
+ def get_direction(self, number):
+ addressname = 'direction_m' + str(number)
+ self.read_value(addressname)
+
+ def get_speed(self, number):
+ addressname = 'speed_m' + str(number)
+ self.read_value(addressname)
+
+MTRMUX.add_compatible_sensor(None, 'mndsnsrs', 'MTRMUX') #Tested with version 'V2.11'
+
+
+class LineLeader(BaseDigitalSensor):
+ """Class for Line Sensor Array"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command': (0x41, '<B'),
+ 'steering': (0x42, '<b'),
+ 'average': (0x43, '<B'),
+ 'result': (0x44, '<B'),
+ 'set_point': (0x45, '<B'),
+
+ 'kp': (0x46, '<B'),
+ 'ki': (0x47, '<B'),
+ 'kd': (0x48, '<B'),
+ 'kp_divisor':(0x61, '<B'),
+ 'ki_divisor':(0x62, '<B'),
+ 'kd_divisor':(0x63, '<B'),
+ #One byte for each sensor, so byte# = sensor#
+ 'calibrated_reading_byte1': (0x49, '<B'),
+ 'calibrated_reading_byte2': (0x4A, '<B'),
+ 'calibrated_reading_byte3': (0x4B, '<B'),
+ 'calibrated_reading_byte4': (0x4C, '<B'),
+ 'calibrated_reading_byte5': (0x4D, '<B'),
+ 'calibrated_reading_byte6': (0x4E, '<B'),
+ 'calibrated_reading_byte7': (0x4F, '<B'),
+ 'calibrated_reading_byte8': (0x50, '<B'),
+ 'all_calibrated_readings': (0x49, '<8B'),
+
+ 'w_read_limit':(0x51, '<H'),
+ 'b_read_limit':(0x59, '<B'),
+ 'w_cal_data1':(0x64, '<B'),
+ 'b_cal_data':(0x6C, '<B'),
+
+ 'uncal_sensor1_voltage_byte1':(0x74, '<B'),
+ 'uncal_sensor2_voltage_byte1':(0x76, '<B'),
+ 'uncal_sensor3_voltage_byte1':(0x78, '<B'),
+ 'uncal_sensor4_voltage_byte1':(0x7A, '<B'),
+ 'uncal_sensor5_voltage_byte1':(0x7C, '<B'),
+ 'uncal_sensor6_voltage_byte1':(0x7E, '<B'),
+ 'uncal_sensor7_voltage_byte1':(0x80, '<B'),
+ 'uncal_sensor8_voltage_byte1':(0x82, '<B'),
+ 'all_uncal_readings': (0x74, '<8B'),
+ })
+
+ class Commands:
+ CALIBRATE_WHITE = 'W'
+ CALIBRATE_BLACK = 'B'
+ SENSOR_SLEEP = 'D'
+ US_CONFIG = 'A'
+ EU_CONFIG = 'E'
+ UNI_CONFIG = 'U'
+ SENSOR_WAKE = 'P'
+ COLOR_INVERT = 'I'
+ COLOR_INVERT_REVERSE = 'R'
+ SNAPSHOT = 'S'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(LineLeader, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_steering(self):
+ 'Value to add to the left and subtract from the right motor\'s power.'
+ return self.read_value('steering')[0]
+
+ def get_average(self):
+ 'Weighted average; greater as line is closer to right edge. 0 for no line.'
+ return self.read_value('average')[0]
+
+ def get_result(self):
+ 'Bitmap, one bit for each sensor'
+ return self.read_value('result')[0]
+
+ def set_set_point(self, value):
+ 'Average value for steering to gravitate to. 10 (left) to 80 (right).'
+ self.write_value('set_point', (value, ))
+
+ def set_pid(self, pid, value):
+ addressname = 'k' + str(pid)
+ self.write_value(addressname, (value, ))
+
+ def set_pid_divisor(self, pid, value):
+ addressname = 'k' + str(pid) + '_divisor'
+ self.write_value(addressname, (value, ))
+
+ def get_reading(self, number):
+ addressname = 'calibrated_reading_byte' + str(number)
+ return self.read_value(addressname)[0]
+
+ def get_reading_all(self):
+ return self.read_value('all_calibrated_readings')
+
+ get_sample = get_reading_all
+
+ def get_uncal_reading(self, number):
+ addressname = 'uncal_sensor' + str(number) + '_voltage_byte1'
+ return self.read_value(addressname)[0]
+
+ def get_uncal_all(self):
+ return self.read_value('all_uncal_readings')
+
+LineLeader.add_compatible_sensor(None, 'mndsnsrs', 'LineLdr') #Tested with version 'V1.16'
+
+
+class Servo(BaseDigitalSensor):
+ """Class for Servo sensors"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+
+ 'servo_1_pos': (0x42, '<H'),
+ 'servo_2_pos': (0x44, '<H'),
+ 'servo_3_pos': (0x46, '<H'),
+ 'servo_4_pos': (0x48, '<H'),
+ 'servo_5_pos': (0x4A, '<H'),
+ 'servo_6_pos': (0x4C, '<H'),
+ 'servo_7_pos': (0x4E, '<H'),
+ 'servo_8_pos': (0x50, '<H'),
+
+ 'servo_1_speed': (0x52, '<B'),
+ 'servo_2_speed': (0x53, '<B'),
+ 'servo_3_speed': (0x54, '<B'),
+ 'servo_4_speed': (0x55, '<B'),
+ 'servo_5_speed': (0x56, '<B'),
+ 'servo_6_speed': (0x57, '<B'),
+ 'servo_7_speed': (0x58, '<B'),
+ 'servo_8_speed': (0x59, '<B'),
+
+ 'servo_1_quick': (0x5A, '<B'),
+ 'servo_2_quick': (0x5B, '<B'),
+ 'servo_3_quick': (0x5C, '<B'),
+ 'servo_4_quick': (0x5D, '<B'),
+ 'servo_5_quick': (0x5E, '<B'),
+ 'servo_6_quick': (0x5F, '<B'),
+ 'servo_7_quick': (0x60, '<B'),
+ 'servo_8_quick': (0x61, '<B'),
+ })
+ I2C_DEV = 0xB0
+
+ COMMANDVALUES = {'R': (0x52), #Resume macro execution
+ 'S': (0x53), #reset initial position and speed
+ 'I1': (0x4931), #store initial position motor 1
+ 'I2': (0x4932), #store initial position motor 2
+ 'I3': (0x4933), #etc...
+ 'I4': (0x4934),
+ 'I5': (0x4935),
+ 'I6': (0x4936),
+ 'I7': (0x4937),
+ 'I8': (0x4938),
+ 'H': (0x48), #Halt macro
+ 'Gx': (0x4778), #not going to work yet x = variable
+ 'EM': (0x454d), #Edit Macro
+ 'P': (0x50), #Pause Macro
+ }
+
+ class Commands:
+ RESUME_MACRO = 'R'
+ RESET_POS_SPEED = 'S'
+ STORE_MOTOR_POS_1 = 'I1'
+ STORE_MOTOR_POS_2 = 'I2'
+ STORE_MOTOR_POS_3 = 'I3'
+ STORE_MOTOR_POS_4 = 'I4'
+ STORE_MOTOR_POS_5 = 'I5'
+ STORE_MOTOR_POS_6 = 'I6'
+ STORE_MOTOR_POS_7 = 'I7'
+ STORE_MOTOR_POS_8 = 'I8'
+ HALT_MACRO = 'H'
+ X_TO_VAR = 'Gx' #not going to work yet
+ EDIT_MACRO = 'EM'
+ PAUSE_MACRO = 'P'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(Servo, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = self.COMMANDVALUES[command]
+ self.write_value('command', (value, ))
+
+ def get_bat_level(self):
+ return self.read_value('command')[0]
+
+ def set_position(self, number, value):
+ addressname = 'servo_' + str(number) + '_pos'
+ self.write_value(addressname, (value, ))
+
+ def get_position(self, number):
+ return self.read_value('servo_' + str(number) + '_pos')[0]
+
+ def set_speed(self, number, value):
+ addressname = 'servo_' + str(number) + '_speed'
+ self.write_value(addressname, (value, ))
+
+ def get_speed(self, number):
+ return self.read_value('servo_' + str(number) + '_speed')[0]
+
+ def set_quick(self, number, value):
+ addressname = 'servo_' + str(number) + '_quick'
+ self.write_value(addressname, (value, ))
+
+Servo.add_compatible_sensor(None, 'mndsnsrs', 'NXTServo') #Tested with version 'V1.20'
+
+
+class MMX(BaseDigitalSensor):
+ """Class for MMX sensors"""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ #Motor Writes
+ 'encoder_1_target': (0x42, '<l'),
+ 'speed_1': (0x46, '<B'),
+ 'seconds_to_run_1': (0x47, '<B'),
+ 'command_b_1': (0x48, '<B'),
+ 'command_a_1': (0x49, '<B'),
+
+ 'encoder_2_target': (0x4A, '<l'),
+ 'speed_2': (0x4E, '<B'),
+ 'seconds_to_run_2': (0x4F, '<B'),
+ 'command_b_2': (0x50, '<B'),
+ 'command_a_2': (0x51, '<B'),
+ #Motor reads
+ 'encoder_1_pos': (0x62, '<H'),
+ 'encoder_2_pos': (0x66, '<H'),
+ 'status_m1': (0x72, '<B'),
+ 'status_m2': (0x73, '<B'),
+ 'tasks_running_m1': (0x76, '<H'),
+ 'tasks_running_m2': (0x77, '<H'),
+ #PID Control
+ 'p_encoder': (0x7A, '<H'),
+ 'i_encoder': (0x7C, '<H'),
+ 'd_encoder': (0x7E, '<H'),
+ 'p_speed': (0x80, '<H'),
+ 'i_speed': (0x82, '<H'),
+ 'd_speed': (0x84, '<H'),
+ 'pass_count': (0x86, '<B'),
+ 'tolerance': (0x87, '<B'),
+ })
+ I2C_DEV = 0x06
+
+ class Commands:
+ RESET_PARAMS_ENCODERS = 'R'
+ ISSUE_SYNCED_COMMANDS = 'S'
+ MOTOR_1_FLOAT_STOP = 'a'
+ MOTOR_2_FLOAT_STOP = 'b'
+ BOTH_FLOAT_STOP = 'c'
+ MOTOR_1_BRAKE_STOP = 'A'
+ MOTOR_2_BRAKE_STOP = 'B'
+ BOTH_BRAKE_STOP = 'C'
+ MOTOR_1_ENC_RESET = 'r'
+ MOTOR_2_ENC_RESET = 's'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(MMX, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_bat_level(self):
+ return self.read_value('command')[0]
+
+ def set_encoder_target(self, motor_number, value):
+ addressname = 'encoder_' + str(motor_number) + '_target'
+ self.write_value(addressname, (value, ))
+
+ def set_speed(self, motor_number, value):
+ addressname = 'speed_' + str(motor_number)
+ self.write_value(addressname, (value, ))
+
+ def set_time_run(self, motor_number, seconds):
+ addressname = 'seconds_to_run_' + str(motor_number)
+ self.write_value(addressname, (seconds, ))
+
+ def command_b(self, motor_number, value):
+ addressname = 'command_b_' + str(motor_number)
+ self.write_value(addressname, (value, ))
+
+ def command_a(self, motor_number, bit_num, bit_val):
+ addressname = 'command_a_' + str(motor_number)
+ s = self.read_value(addressname)[0]
+ #I feel like there must be an easier way to write one bit...
+ val = bit_val << bit_num
+ if bit_val == 1:
+ value = val | s
+ self.write_value(addressname, (value, ))
+ return value #testing purposes
+ elif bit_val == 0:
+ val = 1
+ val = val << bit_num
+ val = val ^ 0xFF
+ value = val & s
+ self.write_value(addressname, (value, ))
+ return value
+
+ def get_encoder_pos(self, motor_number):
+ addressname = 'encoder_' +str(motor_number) +'_pos'
+ return self.read_value(addressname)[0]
+
+ def get_motor_status(self, motor_number, bit_num):
+ addressname = 'status_m' + str(motor_number)
+ s = self.read_value(addressname)[0]
+ x = 1
+ x = x << bit_num
+ value = x & s
+ value = value >> bit_num
+ return value
+
+ def get_tasks(self, motor_number):
+ addressname = 'tasks_running_m' + str(motor_number)
+ return self.read_value(addressname)[0]
+
+ def set_pid(self, pid, target, value):
+ addressname = str(pid) + '_' + str(target)
+ self.write_value(addressname, (value, ))
+
+ def set_pass_count(self, value):
+ self.write_value('pass_count', (value, ))
+
+ def set_tolerance(self, value):
+ self.write_value('tolerance', (value, ))
+
+MMX.add_compatible_sensor(None, 'mndsnsrs', 'NxTMMX') #Tested with version 'V1.01'
+
+
+class HID(BaseDigitalSensor):
+ """Class for Human Interface Device sensors.
+These are connected to a computer and look like a keyboard to it."""
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ 'modifier' : (0x42, '<B'),
+ 'keyboard_data' : (0x43, '<B'),
+ })
+ I2C_DEV = 0x04
+
+ class Commands:
+ TRANSMIT = 'T'
+ ASCII_MODE = 'A'
+ DIRECT_MODE = 'D'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(HID, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def set_modifier(self, mod):
+ self.write_value('modifier', (mod, ))
+
+ def write_data(self, data):
+ data = ord(data)
+ self.write_value('keyboard_data', (data, ))
+
+HID.add_compatible_sensor(None, 'mndsnsrs', 'NXTHID') #Tested with version 'V1.02'
+
+
+class PS2(BaseDigitalSensor):
+ I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
+ I2C_ADDRESS.update({'command' : (0x41, '<B'),
+ 'button_set_1': (0x42, '<B'),
+ 'button_set_2': (0x43, '<B'),
+ 'x_left_joystick': (0x44, '<b'),
+ 'y_left_joystick': (0x45, '<b'),
+ 'x_right_joystick': (0x46, '<b'),
+ 'y_right_joystick': (0x47, '<b'),
+ })
+
+ class ControllerState:
+ class Buttons:
+ left, down, right, up, square, cross, circle, triangle, r1, r2, r3, l1, l2, l3 = [0 for i in range(14)] #14 zeros
+ def __init__(self, buttons_1, buttons_2, left_x, left_y, right_x, right_y):
+ self.leftstick = (left_x, left_y)
+ self.rightstick = (right_x, right_y)
+ buttons_1 = ~buttons_1
+ buttons_2 = ~buttons_2
+ self.buttons = self.Buttons()
+ self.buttons.left = bool(buttons_1 & 0x80)
+ self.buttons.down = bool(buttons_1 & 0x40)
+ self.buttons.right = bool(buttons_1 & 0x20)
+ self.buttons.up = bool(buttons_1 & 0x10)
+ self.buttons.square = bool(buttons_2 & 0x80)
+ self.buttons.cross = bool(buttons_2 & 0x40)
+ self.buttons.circle = bool(buttons_2 & 0x20)
+ self.buttons.triangle = bool(buttons_2 & 0x10)
+ self.buttons.r1 = bool(buttons_2 & 0x08)
+ self.buttons.r2 = bool(buttons_2 & 0x02)
+ self.buttons.r3 = bool(buttons_1 & 0x04)
+ self.buttons.l1 = bool(buttons_2 & 0x04)
+ self.buttons.l2 = bool(buttons_2 & 0x01)
+ self.buttons.l3 = bool(buttons_1 & 0x02)
+
+ class Commands:
+ POWER_ON = 'E'
+ POWER_OFF = 'D'
+ DIGITAL_MODE = 'A'
+ ANALOG_MODE = 's'
+ ADPA_ON = 'N'
+ ADPA_OFF = 'O'
+
+ def __init__(self, brick, port, check_compatible=True):
+ super(PS2, self).__init__(brick, port, check_compatible)
+
+ def command(self, command):
+ value = ord(command)
+ self.write_value('command', (value, ))
+
+ def get_joystick(self, xy, lr):
+ addressname = str(xy) + '_' + str(lr) + '_joystick'
+ return self.read_value(addressname)[0]
+
+ def get_buttons(self, setnum):
+ addressname = 'button_set_' + str(setnum)
+ return self.read_value(addressname)[0]
+
+ def get_sample(self):
+ return self.ControllerState(
+ get_buttons(0),
+ get_buttons(1),
+ get_joystick('x', 'l'),
+ get_joystick('y', 'l'),
+ get_joystick('x', 'r'),
+ get_joystick('y', 'r'))
+
+PS2.add_compatible_sensor(None, 'mndsnsrs', 'PSPNX') #Tested with version 'V2.00'