nxt.sensor

1 # nxt.sensor module -- Classes to read LEGO Mindstorms NXT sensors 2 # Copyright (C) 2006,2007 Douglas P Lau 3 # Copyright (C) 2009 Marcus Wanner, Paulo Vieira 4 # 5 # This program is free software: you can redistribute it and/or modify 6 # it under the terms of the GNU General Public License as published by 7 # the Free Software Foundation, either version 3 of the License, or 8 # (at your option) any later version. 9 # 10 # This program is distributed in the hope that it will be useful, 11 # but WITHOUT ANY WARRANTY; without even the implied warranty of 12 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 # GNU General Public License for more details. 14 15 from time import sleep 16 from nxt.error import I2CError, I2CPendingError 17 18 PORT_1 = 0x00 19 PORT_2 = 0x01 20 PORT_3 = 0x02 21 PORT_4 = 0x03 22

23 -class Type(object):

24 'Namespace for enumeration of the type of sensor' 25 # NOTE: just a namespace (enumeration) 26 NO_SENSOR = 0x00 27 SWITCH = 0x01 # Touch sensor 28 TEMPERATURE = 0x02 29 REFLECTION = 0x03 30 ANGLE = 0x04 31 LIGHT_ACTIVE = 0x05 # Light sensor (illuminated) 32 LIGHT_INACTIVE = 0x06 # Light sensor (ambient) 33 SOUND_DB = 0x07 # Sound sensor (unadjusted) 34 SOUND_DBA = 0x08 # Sound sensor (adjusted) 35 CUSTOM = 0x09 36 LOW_SPEED = 0x0A 37 LOW_SPEED_9V = 0x0B # Low-speed I2C (Ultrasonic sensor) 38 HIGH_SPEED = 0x0C #found this in another spec ans thought it should be included 39 COLORFULL = 0x0D #NXT 2.0 color sensor in full color mode (color sensor mode) 40 COLORRED = 0x0E #NXT 2.0 color sensor with red light on (light sensor mode) 41 COLORGREEN = 0x0F #NXT 2.0 color sensor with green light on (light sensor mode) 42 COLORBLUE = 0x10 #NXT 2.0 color sensor in with blue light on (light sensor mode) 43 COLORNONE = 0x11 #NXT 2.0 color sensor in with light off (light sensor mode) 44 COLOREXIT =0x12 #NXT 2.0 color sensor internal state (not sure what this is for yet)

45

46 -class Mode(object):

47 'Namespace for enumeration of the mode of sensor' 48 # NOTE: just a namespace (enumeration) 49 RAW = 0x00 50 BOOLEAN = 0x20 51 TRANSITION_CNT = 0x40 52 PERIOD_COUNTER = 0x60 53 PCT_FULL_SCALE = 0x80 54 CELSIUS = 0xA0 55 FAHRENHEIT = 0xC0 56 ANGLE_STEPS = 0xE0 57 MASK = 0xE0 58 MASK_SLOPE = 0x1F # Why isn't this slope thing documented?

59

60 -class Sensor(object):

61 'Main sensor object' 62

63 - def __init__(self, brick, port):

64 self.brick = brick 65 self.port = port 66 self.sensor_type = Type.NO_SENSOR 67 self.mode = Mode.RAW

68

69 - def set_input_mode(self):

70 self.brick.set_input_mode(self.port, self.sensor_type, 71 self.mode)

72 73 74 I2C_ADDRESS = { 75 0x00: ('version', 8), 76 0x08: ('product_id', 8), 77 0x10: ('sensor_type', 8), 78 0x11: ('factory_zero', 1), # is this really correct? 79 0x12: ('factory_scale_factor', 1), 80 0x13: ('factory_scale_divisor', 1), 81 0x14: ('measurement_units', 1), 82 } 83

84 -def _make_query(address, n_bytes):

85 def query(self): 86 data = self.i2c_query(address, n_bytes) 87 if n_bytes == 1: 88 return ord(data) 89 else: 90 return data

91 return query 92

93 -class _Meta(type):

94 'Metaclass which adds accessor methods for I2C addresses' 95

96 - def __init__(cls, name, bases, dict):

97 super(_Meta, cls).__init__(name, bases, dict) 98 for address in I2C_ADDRESS: 99 name, n_bytes = I2C_ADDRESS[address] 100 q = _make_query(address, n_bytes) 101 setattr(cls, 'get_' + name, q)

102

103 -class DigitalSensor(Sensor):

104 'Object for digital sensors' 105 106 __metaclass__ = _Meta 107 108 I2C_DEV = 0x02 109

110 - def __init__(self, brick, port):

111 super(DigitalSensor, self).__init__(brick, port)

112

113 - def _ls_get_status(self, n_bytes):

114 for n in range(3): 115 try: 116 b = self.brick.ls_get_status(self.port) 117 if b >= n_bytes: 118 return b 119 except I2CPendingError: 120 sleep(0.01) 121 raise I2CError, 'ls_get_status timeout'

122

123 - def i2c_command(self, address, value):

124 msg = chr(DigitalSensor.I2C_DEV) + chr(address) + chr(value) 125 self.brick.ls_write(self.port, msg, 0)

126

127 - def i2c_query(self, address, n_bytes):

128 msg = chr(DigitalSensor.I2C_DEV) + chr(address) 129 self.brick.ls_write(self.port, msg, n_bytes) 130 self._ls_get_status(n_bytes) 131 data = self.brick.ls_read(self.port) 132 if len(data) < n_bytes: 133 raise I2CError, 'Read failure' 134 return data[-n_bytes:]

135

136 -class CommandState(object):

137 'Namespace for enumeration of the command state of sensors' 138 # NOTE: just a namespace (enumeration) 139 OFF = 0x00 140 SINGLE_SHOT = 0x01 141 CONTINUOUS_MEASUREMENT = 0x02 142 EVENT_CAPTURE = 0x03 # Check for ultrasonic interference 143 REQUEST_WARM_RESET = 0x04

144 145 # I2C addresses for an Ultrasonic sensor 146 I2C_ADDRESS_US = { 147 0x40: ('continuous_measurement_interval', 1, True), 148 0x41: ('command_state', 1, True), 149 0x42: ('measurement_byte_0', 1, False), 150 0x43: ('measurement_byte_1', 1, False), 151 0x44: ('measurement_byte_2', 1, False), 152 0x45: ('measurement_byte_3', 1, False), 153 0x46: ('measurement_byte_4', 1, False), 154 0x47: ('measurement_byte_5', 1, False), 155 0x48: ('measurement_byte_6', 1, False), 156 0x49: ('measurement_byte_7', 1, False), 157 0x50: ('actual_zero', 1, True), 158 0x51: ('actual_scale_factor', 1, True), 159 0x52: ('actual_scale_divisor', 1, True), 160 } 161

162 -def _make_command(address):

163 def command(self, value): 164 self.i2c_command(address, value)

165 return command 166

167 -class _MetaUS(_Meta):

168 'Metaclass which adds accessor methods for US I2C addresses' 169

170 - def __init__(cls, name, bases, dict):

171 super(_MetaUS, cls).__init__(name, bases, dict) 172 for address in I2C_ADDRESS_US: 173 name, n_bytes, set_method = I2C_ADDRESS_US[address] 174 q = _make_query(address, n_bytes) 175 setattr(cls, 'get_' + name, q) 176 if set_method: 177 c = _make_command(address) 178 setattr(cls, 'set_' + name, c)

179 180

181 -class AnalogSensor(Sensor):

182 'Object for analog sensors' 183

184 - def __init__(self, brick, port):

185 super(AnalogSensor, self).__init__(brick, port) 186 self.valid = False 187 self.calibrated = False 188 self.raw_ad_value = 0 189 self.normalized_ad_value = 0 190 self.scaled_value = 0 191 self.calibrated_value = 0

192

193 - def get_input_values(self):

194 values = self.brick.get_input_values(self.port) 195 (self.port, self.valid, self.calibrated, self.sensor_type, 196 self.mode, self.raw_ad_value, self.normalized_ad_value, 197 self.scaled_value, self.calibrated_value) = values 198 return values

199

200 - def reset_input_scaled_value(self):

201 self.brick.reset_input_scaled_value()

202

203 - def get_sample(self):

204 self.get_input_values() 205 return self.scaled_value

206

207 -class TouchSensor(AnalogSensor):

208 'Object for touch sensors' 209

210 - def __init__(self, brick, port):

211 super(TouchSensor, self).__init__(brick, port) 212 self.sensor_type = Type.SWITCH 213 self.mode = Mode.BOOLEAN 214 self.set_input_mode()

215

216 - def is_pressed(self):

217 return bool(self.scaled_value)

218

219 - def get_sample(self):

220 self.get_input_values() 221 return self.is_pressed()

222

223 -class LightSensor(AnalogSensor):

224 'Object for light sensors' 225

226 - def __init__(self, brick, port):

227 super(LightSensor, self).__init__(brick, port) 228 self.set_illuminated(True)

229

230 - def set_illuminated(self, active):

231 if active: 232 self.sensor_type = Type.LIGHT_ACTIVE 233 else: 234 self.sensor_type = Type.LIGHT_INACTIVE 235 self.set_input_mode()

236

237 -class ColorSensor(AnalogSensor):

238 'Object for color sensors' 239 # this is a class for the lego NXT 2.0 RGB color sensor 240 # not to be confused with the hitechnic color sensor 241 # the color sensor can run in two modes: 242 # a light sensor which returns the reflected light from the lamp that is 243 # currently on (red, green, blue, off/ambient) on a scale of 1-1023 244 # a color sensor that returns a 1-6 decimal value corresponding to 245 # (black, blue, green, yellow, red, white) unfortunately the RGB values 246 # are not sent over the wire 247 248 # TODO: calibration 249 250 #note: if you create a new object everytime you make a call the light 251 # will flash on an off because each time the object is created the light 252 # color is set to none 253

254 - def __init__(self, brick, port):

255 super(ColorSensor, self).__init__(brick, port) 256 self.set_light_color(None)

257

258 - def get_input_values(self):

259 values = self.brick.get_input_values(self.port) 260 (self.port, self.valid, self.calibrated, self.sensor_type, 261 self.mode, self.raw_ad_value, self.normalized_ad_value, 262 self.scaled_value, self.calibrated_value) = values 263 return values

264

265 - def set_light_color(self, color):

266 if color == 'red': 267 self.sensor_type = Type.COLORRED 268 elif color == 'green': 269 self.sensor_type = Type.COLORGREEN 270 elif color == 'blue': 271 self.sensor_type = Type.COLORBLUE 272 elif color == 'full': 273 self.sensor_type = Type.COLORFULL 274 elif color == 'off': 275 self.sensor_type = Type.COLORNONE 276 else: 277 self.sensor_type = Type.COLORNONE 278 self.set_input_mode()

279

280 - def get_color(self):

281 self.set_light_color('full') 282 self.get_input_values() 283 return self.scaled_value

284

285 - def get_reflected_light(self, color):

286 self.set_light_color(color) 287 self.get_input_values() 288 return self.scaled_value

289

290 -class SoundSensor(AnalogSensor):

291 'Object for sound sensors' 292

293 - def __init__(self, brick, port):

294 super(SoundSensor, self).__init__(brick, port) 295 self.set_adjusted(True)

296

297 - def set_adjusted(self, active):

298 if active: 299 self.sensor_type = Type.SOUND_DBA 300 else: 301 self.sensor_type = Type.SOUND_DB 302 self.set_input_mode()

303 304

305 -class UltrasonicSensor(DigitalSensor):

306 'Object for ultrasonic sensors' 307 308 __metaclass__ = _MetaUS 309

310 - def __init__(self, brick, port):

311 super(UltrasonicSensor, self).__init__(brick, port) 312 self.sensor_type = Type.LOW_SPEED_9V 313 self.mode = Mode.RAW 314 self.set_input_mode() 315 sleep(0.1) # Give I2C time to initialize

316

317 - def get_sample(self):

318 'Function to get data from ultrasonic sensors' 319 self.set_command_state(CommandState.SINGLE_SHOT) 320 return self.get_measurement_byte_0()

321

322 -class AccelerometerSensor(DigitalSensor):

323 'Object for Accelerometer sensors. Thanks to Paulo Vieira.' 324 325 __metaclass__ = _MetaUS 326

327 - def __init__(self, brick, port):

328 super(AccelerometerSensor, self).__init__(brick, port) 329 self.sensor_type = Type.LOW_SPEED_9V 330 self.mode = Mode.RAW 331 self.set_input_mode() 332 sleep(0.1) # Give I2C time to initialize

333

334 - def get_sample(self):

335 self.set_command_state(CommandState.SINGLE_SHOT) 336 out_buffer = [0,0,0,0,0,0] 337 # Upper X, Y, Z 338 out_buffer[0] = self.get_measurement_byte_0() 339 out_buffer[1] = self.get_measurement_byte_1() 340 out_buffer[2] = self.get_measurement_byte_2() 341 # Lower X, Y, Z 342 out_buffer[3] = self.get_measurement_byte_3() 343 out_buffer[4] = self.get_measurement_byte_4() 344 out_buffer[5] = self.get_measurement_byte_5() 345 self.xval = out_buffer[0] 346 if self.xval > 127: 347 self.xval -= 256 348 self.xval = self.xval * 4 + out_buffer[3] 349 350 self.yval = out_buffer[1] 351 if self.yval > 127: 352 self.yval -= 256 353 self.yval = self.yval * 4 + out_buffer[4] 354 355 self.zval = out_buffer[2] 356 if self.zval > 127: 357 self.zval -= 256 358 self.zval = self.zval * 4 + out_buffer[5] 359 360 self.xval = float(self.xval)/200 361 self.yval = float(self.yval)/200 362 self.zval = float(self.zval)/200 363 364 return self.xval, self.yval, self.zval

365 366

367 -class GyroSensor(AnalogSensor):

368 'Object for gyro sensors' 369 #This class is for the hitechnic gryo accelerometer. When the gryo is 370 #not moving there will be a constant offset that will change with 371 #temperature and other ambient factors. It might be appropriate to 372 #write a calibration function to account for this offset. 373 # 374 #TODO: calibration 375

376 - def __init__(self, brick, port):

377 super(GyroSensor, self).__init__(brick, port) 378 self.sensor_type = Type.ANGLE 379 self.set_input_mode() 380 self.calibration_zero = 0 381 self.calibrate()

382

383 - def get_input_values(self):

384 values = self.brick.get_input_values(self.port) 385 (self.port, self.valid, self.calibrated, self.sensor_type, 386 self.mode, self.raw_ad_value, self.normalized_ad_value, 387 self.scaled_value, self.calibrated_value) = values 388 return values

389

390 - def get_sample(self):

391 self.get_input_values() 392 return -self.scaled_value - self.zero # negative sign to make it return angular rate about z axis up.

393 - def calibrate(self):

394 self.get_input_values() 395 self.zero = -self.scaled_value # negative sign to make it return angular rate about z axis up.

396

Source Code for Module nxt.sensor