aboutsummaryrefslogtreecommitdiff
path: root/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
diff options
context:
space:
mode:
Diffstat (limited to 'AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py')
-rw-r--r--AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py611
1 files changed, 0 insertions, 611 deletions
diff --git a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py b/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
deleted file mode 100644
index 3c152d3..0000000
--- a/AT91SAM7S256/armdebug/nxt-python-fantom/nxt/sensor/hitechnic.py
+++ /dev/null
@@ -1,611 +0,0 @@
-# nxt.sensor.hitechnic module -- Classes to read HiTechnic sensors
-# Copyright (C) 2006,2007 Douglas P Lau
-# Copyright (C) 2009 Marcus Wanner, Paulo Vieira, rhn
-# Copyright (C) 2010 rhn, Marcus Wanner, melducky, Samuel Leeman-Munk
-# Copyright (C) 2011 jerradgenson, Marcus Wanner
-#
-# This program is free software: you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation, either version 3 of the License, or
-# (at your option) any later version.
-#
-# This program is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-# GNU General Public License for more details.
-
-from .common import *
-from .digital import BaseDigitalSensor
-from .analog import BaseAnalogSensor
-
-
-class Compass(BaseDigitalSensor):
- """Hitechnic compass sensor."""
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({'mode': (0x41, 'B'),
- 'heading': (0x42, 'B'),
- 'adder' : (0x43, 'B'),
- })
-
- class Modes:
- MEASUREMENT = 0x00
- CALIBRATION = 0x43
- CALIBRATION_FAILED = 0x02
-
- def get_heading(self):
- """Returns heading from North in degrees."""
-
- two_degree_heading = self.read_value('heading')[0]
- adder = self.read_value('adder')[0]
- heading = two_degree_heading * 2 + adder
-
- return heading
-
- get_sample = get_heading
-
- def get_relative_heading(self,target=0):
- rheading = self.get_sample()-target
- if rheading > 180:
- rheading -= 360
- elif rheading < -180:
- rheading += 360
- return rheading
-
- def is_in_range(self,minval,maxval):
- """This deserves a little explanation:
-if max > min, it's straightforward, but
-if min > max, it switches the values of max and min
-and returns true if heading is NOT between the new max and min
- """
- if minval > maxval:
- (maxval,minval) = (minval,maxval)
- inverted = True
- else:
- inverted = False
- heading = self.get_sample()
- in_range = (heading > minval) and (heading < maxval)
- #an xor handles the reversal
- #a faster, more compact way of saying
- #if !reversed return in_range
- #if reversed return !in_range
- return bool(inverted) ^ bool(in_range)
-
- def get_mode(self):
- return self.read_value('mode')[0]
-
- def set_mode(self, mode):
- if mode != self.Modes.MEASUREMENT and \
- mode != self.Modes.CALIBRATION:
- raise ValueError('Invalid mode specified: ' + str(mode))
- self.write_value('mode', (mode, ))
-
-Compass.add_compatible_sensor(None, 'HiTechnc', 'Compass ') #Tested with version '\xfdV1.23 '
-Compass.add_compatible_sensor(None, 'HITECHNC', 'Compass ') #Tested with version '\xfdV2.1 '
-
-
-class Accelerometer(BaseDigitalSensor):
- 'Object for Accelerometer sensors. Thanks to Paulo Vieira.'
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({'x_axis_high': (0x42, 'b'),
- 'y_axis_high': (0x43, 'b'),
- 'z_axis_high': (0x44, 'b'),
- 'xyz_short': (0x42, '3b'),
- 'all_data': (0x42, '3b3B')
- })
-
- class Acceleration:
- def __init__(self, x, y, z):
- self.x, self.y, self.z = x, y, z
-
- def __init__(self, brick, port, check_compatible=True):
- super(Accelerometer, self).__init__(brick, port, check_compatible)
-
- def get_acceleration(self):
- """Returns the acceleration along x, y, z axes. Units are unknown to me.
- """
- xh, yh, zh, xl, yl, zl = self.read_value('all_data')
- x = xh << 2 + xl
- y = yh << 2 + yl
- z = zh << 2 + yl
- return self.Acceleration(x, y, z)
-
- get_sample = get_acceleration
-
-Accelerometer.add_compatible_sensor(None, 'HiTechnc', 'Accel. ')
-Accelerometer.add_compatible_sensor(None, 'HITECHNC', 'Accel. ') #Tested with version '\xfdV1.1 '
-
-
-class IRReceiver(BaseDigitalSensor):
- """Object for HiTechnic IRReceiver sensors for use with LEGO Power Functions IR
-Remotes. Coded to HiTechnic's specs for the sensor but not tested. Please report
-whether this worked for you or not!
- """
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({
- 'm1A': (0x42, 'b'),
- 'm1B': (0x43, 'b'),
- 'm2A': (0x44, 'b'),
- 'm2B': (0x45, 'b'),
- 'm3A': (0x46, 'b'),
- 'm3B': (0x47, 'b'),
- 'm4A': (0x48, 'b'),
- 'm4B': (0x49, 'b'),
- 'all_data': (0x42, '8b')
- })
-
- class SpeedReading:
- def __init__(self, m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B):
- self.m1A, self.m1B, self.m2A, self.m2B, self.m3A, self.m3B, self.m4A, self.m4B = m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B
- self.channel_1 = (m1A, m1B)
- self.channel_2 = (m2A, m2B)
- self.channel_3 = (m3A, m3B)
- self.channel_4 = (m4A, m4B)
-
- def __init__(self, brick, port, check_compatible=True):
- super(IRReceiver, self).__init__(brick, port, check_compatible)
-
- def get_speeds(self):
- """Returns the motor speeds for motors A and B on channels 1-4.
-Values are -128, -100, -86, -72, -58, -44, -30, -16, 0, 16, 30, 44, 58, 72, 86
-and 100. -128 specifies motor brake mode. Note that no motors are actually
-being controlled here!
- """
- m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B = self.read_value('all_data')
- return self.SpeedReading(m1A, m1B, m2A, m2B, m3A, m3B, m4A, m4B)
-
- get_sample = get_speeds
-
-IRReceiver.add_compatible_sensor(None, 'HiTechnc', 'IRRecv ')
-IRReceiver.add_compatible_sensor(None, 'HITECHNC', 'IRRecv ')
-
-
-class IRSeekerv2(BaseDigitalSensor):
- """Object for HiTechnic IRSeeker sensors. Coded to HiTechnic's specs for the sensor
-but not tested. Please report whether this worked for you or not!
- """
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({
- 'dspmode': (0x41, 'B'),
- 'DC_direction': (0x42, 'B'),
- 'DC_sensor_1': (0x43, 'B'),
- 'DC_sensor_2': (0x44, 'B'),
- 'DC_sensor_3': (0x45, 'B'),
- 'DC_sensor_4': (0x46, 'B'),
- 'DC_sensor_5': (0x47, 'B'),
- 'DC_sensor_mean': (0x48, 'B'),
- 'all_DC': (0x42, '7B'),
- 'AC_direction': (0x49, 'B'),
- 'AC_sensor_1': (0x4A, 'B'),
- 'AC_sensor_2': (0x4B, 'B'),
- 'AC_sensor_3': (0x4C, 'B'),
- 'AC_sensor_4': (0x4D, 'B'),
- 'AC_sensor_5': (0x4E, 'B'),
- 'all_AC': (0x49, '6B')
- })
- I2C_DEV = 0x10 #different from standard 0x02
-
- class DSPModes:
- #Modes for modulated (AC) data.
- AC_DSP_1200Hz = 0x00
- AC_DSP_600Hz = 0x01
-
- class _data:
- def get_dir_brightness(self, direction):
- "Gets the brightness of a given direction (1-9)."
- if direction%2 == 1: #if it's an odd number
- exec("val = self.sensor_%d" % ((direction-1)/2+1))
- else:
- exec("val = (self.sensor_%d+self.sensor_%d)/2" % (direction/2, (direction/2)+1))
- return val
-
- class DCData(_data):
- def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean):
- self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5, self.sensor_mean = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean
-
- class ACData(_data):
- def __init__(self, direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5):
- self.direction, self.sensor_1, self.sensor_2, self.sensor_3, self.sensor_4, self.sensor_5 = direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5
-
-
- def __init__(self, brick, port, check_compatible=True):
- super(IRSeekerv2, self).__init__(brick, port, check_compatible)
-
- def get_dc_values(self):
- """Returns the unmodulated (DC) values.
- """
- direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean = self.read_value('all_DC')
- return self.DCData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5, sensor_mean)
-
- def get_ac_values(self):
- """Returns the modulated (AC) values. 600Hz and 1200Hz modes can be selected
-between by using the set_dsp_mode() function.
- """
- direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5 = self.read_value('all_AC')
- return self.ACData(direction, sensor_1, sensor_2, sensor_3, sensor_4, sensor_5)
-
- def get_dsp_mode(self):
- return self.read_value('dspmode')[0]
-
- def set_dsp_mode(self, mode):
- self.write_value('dspmode', (mode, ))
-
- get_sample = get_ac_values
-
-IRSeekerv2.add_compatible_sensor(None, 'HiTechnc', 'NewIRDir')
-IRSeekerv2.add_compatible_sensor(None, 'HITECHNC', 'NewIRDir')
-
-
-class EOPD(BaseAnalogSensor):
- """Object for HiTechnic Electro-Optical Proximity Detection sensors.
- """
-
- # To be divided by processed value.
- _SCALE_CONSTANT = 250
-
- # Maximum distance the sensor can detect.
- _MAX_DISTANCE = 1023
-
- def __init__(self, brick, port):
- super(EOPD, self).__init__(brick, port)
- from math import sqrt
- self.sqrt = sqrt
-
- def set_range_long(self):
- ''' Choose this mode to increase the sensitivity
- of the EOPD sensor by approximately 4x. May
- cause sensor overload.
- '''
-
- self.set_input_mode(Type.LIGHT_ACTIVE, Mode.RAW)
-
- def set_range_short(self):
- ''' Choose this mode to prevent the EOPD sensor from
- being overloaded by white objects.
- '''
-
- self.set_input_mode(Type.LIGHT_INACTIVE, Mode.RAW)
-
- def get_raw_value(self):
- '''Unscaled value read from sensor.'''
-
- return self._MAX_DISTANCE - self.get_input_values().raw_ad_value
-
- def get_processed_value(self):
- '''Derived from the square root of the raw value.'''
-
- return self.sqrt(self.get_raw_value())
-
- def get_scaled_value(self):
- ''' Returns a value that will scale linearly as distance
- from target changes. This is the method that should
- generally be called to get EOPD sensor data.
- '''
-
- try:
- result = self._SCALE_CONSTANT / self.get_processed_value()
- return result
-
- except ZeroDivisionError:
- return self._SCALE_CONSTANT
-
- get_sample = get_scaled_value
-
-
-class Colorv2(BaseDigitalSensor):
- """Object for HiTechnic Color v2 Sensors. Coded to HiTechnic's specs for the sensor
-but not tested. Please report whether this worked for you or not!"""
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({
- 'mode': (0x41, 'B'),
- 'number': (0x42, 'B'),
- 'red': (0x43, 'B'),
- 'green': (0x44, 'B'),
- 'blue': (0x45, 'B'),
- 'white': (0x46, 'B'),
- 'index': (0x47, 'B'),
- 'normred': (0x48, 'B'),
- 'normgreen': (0x49, 'B'),
- 'normblue': (0x4A, 'B'),
- 'all_data': (0x42, '9B'),
- 'rawred': (0x42, '<H'),
- 'rawgreen': (0x44, '<H'),
- 'rawblue': (0x46, '<H'),
- 'rawwhite': (0x48, '<H'),
- 'all_raw_data': (0x42, '<4H')
- })
-
- class Modes:
- ACTIVE = 0x00 #get measurements using get_active_color
- PASSIVE = 0x01 #get measurements using get_passive_color
- RAW = 0x03 #get measurements using get_passive_color
- BLACK_CALIBRATION = 0x42 #hold away from objects, results saved in EEPROM
- WHITE_CALIBRATION = 0x43 #hold in front of white surface, results saved in EEPROM
- LED_POWER_LOW = 0x4C #saved in EEPROM, must calibrate after using
- LED_POWER_HIGH = 0x48 #saved in EEPROM, must calibrate after using
- RANGE_NEAR = 0x4E #saved in EEPROM, only affects active mode
- RANGE_FAR = 0x46 #saved in EEPROM, only affects active mode, more susceptable to noise
- FREQ_50 = 0x35 #saved in EEPROM, use when local wall power is 50Hz
- FREQ_60 = 0x36 #saved in EEPROM, use when local wall power is 60Hz
-
- class ActiveData:
- def __init__(self, number, red, green, blue, white, index, normred, normgreen, normblue):
- self.number, self.red, self.green, self.blue, self.white, self.index, self.normred, self.normgreen, self.normblue = number, red, green, blue, white, index, normred, normgreen, normblue
-
- class PassiveData:
- #also holds raw mode data
- def __init__(self, red, green, blue, white):
- self.red, self.green, self.blue, self.white = red, green, blue, white
-
- def __init__(self, brick, port, check_compatible=True):
- super(Colorv2, self).__init__(brick, port, check_compatible)
-
- def get_active_color(self):
- """Returns color values when in active mode.
- """
- number, red, green, blue, white, index, normred, normgreen, normblue = self.read_value('all_data')
- return self.ActiveData(number, red, green, blue, white, index, normred, normgreen, normblue)
-
- get_sample = get_active_color
-
- def get_passive_color(self):
- """Returns color values when in passive or raw mode.
- """
- red, green, blue, white = self.read_value('all_raw_data')
- return self.PassiveData(red, green, blue, white)
-
- def get_mode(self):
- return self.read_value('mode')[0]
-
- def set_mode(self, mode):
- self.write_value('mode', (mode, ))
-
-Colorv2.add_compatible_sensor(None, 'HiTechnc', 'ColorPD')
-Colorv2.add_compatible_sensor(None, 'HITECHNC', 'ColorPD')
-Colorv2.add_compatible_sensor(None, 'HiTechnc', 'ColorPD ')
-Colorv2.add_compatible_sensor(None, 'HITECHNC', 'ColorPD ')
-
-
-class Gyro(BaseAnalogSensor):
- 'Object for gyro sensors'
-#This class is for the hitechnic gryo sensor. When the gryo is not
-#moving there will be a constant offset that will change with
-#temperature and other ambient factors. The calibrate() function
-#takes the currect value and uses it to offset subsequesnt ones.
-
- def __init__(self, brick, port):
- super(Gyro, self).__init__(brick, port)
- self.set_input_mode(Type.ANGLE, Mode.RAW)
- self.offset = 0
-
- def get_rotation_speed(self):
- return self.get_input_values().scaled_value - self.offset
-
- def set_zero(self, value):
- self.offset = value
-
- def calibrate(self):
- self.set_zero(self.get_rotation_speed())
-
- get_sample = get_rotation_speed
-
-
-class Prototype(BaseDigitalSensor):
- """Object for HiTechnic sensor prototype boards. Coded to HiTechnic's specs but not
-tested. Please report whether this worked for you or not!
- """
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({
- 'A0': (0x42, '<H'),
- 'A0': (0x44, '<H'),
- 'A0': (0x46, '<H'),
- 'A0': (0x48, '<H'),
- 'A0': (0x4A, '<H'),
- 'all_analog': (0x42, '<5H'),
- 'digital_in': (0x4C, 'B'),
- 'digital_out': (0x4D, 'B'),
- 'digital_cont': (0x4E, 'B'),
- 'sample_time': (0x4F, 'B'),
- })
-
- class Digital_Data():
- """Container for 6 bits of digital data. Takes an integer or a list of six bools
-and can be converted into a list of bools or an integer."""
- def __init__(self, pins):
- if isinstance(pins, int):
- self.dataint = pins
- self.datalst = self.tolist(pins)
- else:
- self.dataint = self.toint(pins)
- self.datalst = pins
- self.d0, self.d1, self.d2, self.d3, self.d4, self.d5 = self.datalst
-
- def tolist(self, val):
- lst = []
- for i in range(6):
- lst.append(bool(val & 2**i))
- return lst
-
- def toint(self, lst):
- val = 0
- for i in range(6):
- val += int(bool(lst[i])) * (2**i)
- return val
-
- def __int__(self):
- return self.dataint
-
- def __iter__(self):
- return iter(self.datalst)
-
- def __getitem__(self, i):
- return self.datalst[i]
-
- class Analog_Data():
- def __init__(self, a0, a1, a2, a3, a4):
- self.a0, self.a1, self.a2, self.a3, self.a4 = a0, a1, a2, a3, a4
-
- def get_analog(self):
- return Analog_Data(self.read_value('all_analog'))
-
- def get_digital(self):
- return Digital_Data(self.read_value('digital_in')[0])
-
- def set_digital(self, pins):
- """Can take a Digital_Data() object"""
- self.write_value('digital_out', (int(pins), ))
-
- def set_digital_modes(self, modes):
- """Sets input/output mode of digital pins. Can take a Digital_Data() object."""
- self.write_value('digital_cont', (int(modes), ))
-
-Prototype.add_compatible_sensor(None, 'HiTechnc', 'Proto ')
-
-
-class ServoCon(BaseDigitalSensor):
- """Object for HiTechnic FIRST Servo Controllers. Coded to HiTechnic's specs for
-the sensor but not tested. Please report whether this worked for you or not!"""
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({
- 'status': (0x40, 'B'),
- 'steptime': (0x41, 'B'),
- 's1pos': (0x42, 'B'),
- 's2pos': (0x43, 'B'),
- 's3pos': (0x44, 'B'),
- 'p4pos': (0x45, 'B'),
- 'p5pos': (0x46, 'B'),
- 'p6pos': (0x47, 'B'),
- 'pwm': (0x46, 'B'),
- })
-
- class Status:
- RUNNING = 0x00 #all motors stopped
- STOPPED = 0x01 #motor(s) moving
-
- def __init__(self, brick, port, check_compatible=True):
- super(ServoCon, self).__init__(brick, port, check_compatible)
-
- def get_status(self):
- """Returns the status of the motors. 0 for all stopped, 1 for
-some running.
- """
- return self.read_value('status')[0]
-
- def set_step_time(self, time):
- """Sets the step time (0-15).
- """
- self.write_value('steptime', (time, ))
-
- def set_pos(self, num, pos):
- """Sets the position of a server. num is the servo number (1-6),
-pos is the position (0-255).
- """
- self.write_value('s%dpos' % num, (pos, ))
-
- def get_pwm(self):
- """Gets the "PWM enable" value. The function of this value is
-nontrivial and can be found in the documentation for the sensor.
- """
- return self.read_value('pwm')[0]
-
- def set_pwm(self, pwm):
- """Sets the "PWM enable" value. The function of this value is
-nontrivial and can be found in the documentation for the sensor.
- """
- self.write_value('pwm', (pwm, ))
-
-ServoCon.add_compatible_sensor(None, 'HiTechnc', 'ServoCon')
-
-
-class MotorCon(BaseDigitalSensor):
- """Object for HiTechnic FIRST Motor Controllers. Coded to HiTechnic's specs for
-the sensor but not tested. Please report whether this worked for you or not!"""
- I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
- I2C_ADDRESS.update({
- 'm1enctarget': (0x40, '>l'),
- 'm1mode': (0x44, 'B'),
- 'm1power': (0x45, 'b'),
- 'm2power': (0x46, 'b'),
- 'm2mode': (0x47, 'B'),
- 'm2enctarget': (0x48, '>l'),
- 'm1enccurrent': (0x4c, '>l'),
- 'm2enccurrent': (0x50, '>l'),
- 'batteryvoltage': (0x54, '2B'),
- 'm1gearratio': (0x56, 'b'),
- 'm1pid': (0x57, '3B'),
- 'm2gearratio': (0x5a, 'b'),
- 'm2pid': (0x5b, '3B'),
- })
-
- class PID_Data():
- def __init__(self, p, i, d):
- self.p, self.i, self.d = p, i, d
-
- def __init__(self, brick, port, check_compatible=True):
- super(MotorCon, self).__init__(brick, port, check_compatible)
-
- def set_enc_target(self, mot, val):
- """Set the encoder target (-2147483648-2147483647) for a motor
- """
- self.write_value('m%denctarget'%mot, (val, ))
-
- def get_enc_target(self, mot):
- """Get the encoder target for a motor
- """
- return self.read_value('m%denctarget'%mot)[0]
-
- def get_enc_current(self, mot):
- """Get the current encoder value for a motor
- """
- return self.read_value('m%denccurrent'%mot)[0]
-
- def set_mode(self, mot, mode):
- """Set the mode for a motor. This value is a bit mask and you can
-find details about it in the sensor's documentation.
- """
- self.write_value('m%dmode'%mot, (mode, ))
-
- def get_mode(self, mot):
- """Get the mode for a motor. This value is a bit mask and you can
-find details about it in the sensor's documentation.
- """
- return self.read_value('m%dmode'%mot)[0]
-
- def set_power(self, mot, power):
- """Set the power (-100-100) for a motor
- """
- self.write_value('m%dpower'%mot, (power, ))
-
- def get_power(self, mot):
- """Get the power for a motor
- """
- return self.read_value('m%dpower'%mot)[0]
-
- def set_gear_ratio(self, mot, ratio):
- """Set the gear ratio for a motor
- """
- self.write_value('m%dgearratio'%mot, (ratio, ))
-
- def get_gear_ratio(self, mot):
- """Get the gear ratio for a motor
- """
- return self.read_value('m%dgearratio'%mot)[0]
-
- def set_pid(self, mot, piddata):
- """Set the PID coefficients for a motor. Takes data in
-MotorCon.PID_Data(p, i, d) format.
- """
- self.write_value('m%dpid'%mot, (piddata.p, piddata.i, piddata.d))
-
- def get_pid(self, mot):
- """Get the PID coefficients for a motor. Returns a PID_Data() object.
- """
- p, i, d = self.read_value('m%dpid'%mot)
- return self.PID_Data(p, i, d)
-
- def get_battery_voltage(self):
- """Gets the battery voltage (in millivolts/20)
- """
- high, low = self.read_value('bateryvoltage')[0]
- return high << 2 + low
-
-MotorCon.add_compatible_sensor(None, 'HiTechnc', 'MotorCon')