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