diff options
Diffstat (limited to 'nxt_plugin/nxt/sensor/hitechnic.py')
-rw-r--r-- | nxt_plugin/nxt/sensor/hitechnic.py | 611 |
1 files changed, 611 insertions, 0 deletions
diff --git a/nxt_plugin/nxt/sensor/hitechnic.py b/nxt_plugin/nxt/sensor/hitechnic.py new file mode 100644 index 0000000..cf1bf9c --- /dev/null +++ b/nxt_plugin/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') |