brickpi3

1 # https://www.dexterindustries.com/BrickPi/ 2 # https://github.com/DexterInd/BrickPi3 3 # 4 # Copyright (c) 2017 Dexter Industries 5 # Released under the MIT license (http://choosealicense.com/licenses/mit/). 6 # For more information see https://github.com/DexterInd/BrickPi3/blob/master/LICENSE.md 7 # 8 # Python drivers for the BrickPi3 9 10 from __future__ import print_function 11 from __future__ import division 12 #from builtins import input 13 14 import subprocess # for executing system calls 15 import spidev 16 import array # for converting hex string to byte array 17 18 FIRMWARE_VERSION_REQUIRED = "1.4.x" # Make sure the top 2 of 3 numbers match 19 20 BP_SPI = spidev.SpiDev() 21 BP_SPI.open(0, 1) 22 BP_SPI.max_speed_hz = 500000 23 BP_SPI.mode = 0b00 24 BP_SPI.bits_per_word = 8 25 26

27 -class Enumeration(object):

28 - def __init__(self, names): # or *names, with no .split()

29 number = 0 30 for line, name in enumerate(names.split('\n')): 31 if name.find(",") >= 0: 32 # strip out the spaces 33 while(name.find(" ") != -1): 34 name = name[:name.find(" ")] + name[(name.find(" ") + 1):] 35 36 # strip out the commas 37 while(name.find(",") != -1): 38 name = name[:name.find(",")] + name[(name.find(",") + 1):] 39 40 # if the value was specified 41 if(name.find("=") != -1): 42 number = int(float(name[(name.find("=") + 1):])) 43 name = name[:name.find("=")] 44 45 # optionally print to confirm that it's working correctly 46 #print "%40s has a value of %d" % (name, number) 47 48 setattr(self, name, number) 49 number = number + 1

50 51

52 -class FirmwareVersionError(Exception):

53 """Exception raised if the BrickPi3 firmware needs to be updated"""

54 55

56 -class SensorError(Exception):

57 """Exception raised if a sensor is not yet configured when trying to read it with get_sensor"""

58 59

60 -def set_address(address, id):

61 """ 62 Set the SPI address of the BrickPi3 63 64 Keyword arguments: 65 address -- the new SPI address to use (1 to 255) 66 id -- the BrickPi3's unique serial number ID (so that the address can be set while multiple BrickPi3s are stacked on a Raspberry Pi). 67 """ 68 address = int(address) 69 if address < 1 or address > 255: 70 raise IOError("brickpi3.set_address error: SPI address must be in the range of 1 to 255") 71 return 72 73 if(len(id) != 32): 74 if id == "": 75 id_arr = [0 for i in range(16)] 76 else: 77 raise IOError("brickpi3.set_address error: wrong serial number id length. Must be a 32-digit hex string.") 78 return 79 else: 80 id_arr = array.array('B', bytearray.fromhex(id)) 81 if(len(id_arr) != 16): 82 raise IOError("brickpi3.set_address error: unknown serial number id problem. Make sure to use a valid 32-digit hex string serial number.") 83 return 84 85 outArray = [0, BrickPi3.BPSPI_MESSAGE_TYPE.SET_ADDRESS, address] 86 outArray.extend(id_arr) 87 BP_SPI.xfer2(outArray)

88 89

90 -class BrickPi3(object):

91 PORT_1 = 0x01 92 PORT_2 = 0x02 93 PORT_3 = 0x04 94 PORT_4 = 0x08 95 96 PORT_A = 0x01 97 PORT_B = 0x02 98 PORT_C = 0x04 99 PORT_D = 0x08 100 101 MOTOR_FLOAT = -128 102 103 SensorType = [0, 0, 0, 0] 104 I2CInBytes = [0, 0, 0, 0] 105 106 BPSPI_MESSAGE_TYPE = Enumeration(""" 107 NONE, 108 109 GET_MANUFACTURER, 110 GET_NAME, 111 GET_HARDWARE_VERSION, 112 GET_FIRMWARE_VERSION, 113 GET_ID, 114 SET_LED, 115 GET_VOLTAGE_3V3, 116 GET_VOLTAGE_5V, 117 GET_VOLTAGE_9V, 118 GET_VOLTAGE_VCC, 119 SET_ADDRESS, 120 121 SET_SENSOR_TYPE, 122 123 GET_SENSOR_1, 124 GET_SENSOR_2, 125 GET_SENSOR_3, 126 GET_SENSOR_4, 127 128 I2C_TRANSACT_1, 129 I2C_TRANSACT_2, 130 I2C_TRANSACT_3, 131 I2C_TRANSACT_4, 132 133 SET_MOTOR_POWER, 134 135 SET_MOTOR_POSITION, 136 137 SET_MOTOR_POSITION_KP, 138 139 SET_MOTOR_POSITION_KD, 140 141 SET_MOTOR_DPS, 142 143 SET_MOTOR_DPS_KP, 144 145 SET_MOTOR_DPS_KD, 146 147 SET_MOTOR_LIMITS, 148 149 OFFSET_MOTOR_ENCODER, 150 151 GET_MOTOR_A_ENCODER, 152 GET_MOTOR_B_ENCODER, 153 GET_MOTOR_C_ENCODER, 154 GET_MOTOR_D_ENCODER, 155 156 GET_MOTOR_A_STATUS, 157 GET_MOTOR_B_STATUS, 158 GET_MOTOR_C_STATUS, 159 GET_MOTOR_D_STATUS, 160 """) 161 162 SENSOR_TYPE = Enumeration(""" 163 NONE = 1, 164 I2C, 165 CUSTOM, 166 167 TOUCH, 168 NXT_TOUCH, 169 EV3_TOUCH, 170 171 NXT_LIGHT_ON, 172 NXT_LIGHT_OFF, 173 174 NXT_COLOR_RED, 175 NXT_COLOR_GREEN, 176 NXT_COLOR_BLUE, 177 NXT_COLOR_FULL, 178 NXT_COLOR_OFF, 179 180 NXT_ULTRASONIC, 181 182 EV3_GYRO_ABS, 183 EV3_GYRO_DPS, 184 EV3_GYRO_ABS_DPS, 185 186 EV3_COLOR_REFLECTED, 187 EV3_COLOR_AMBIENT, 188 EV3_COLOR_COLOR, 189 EV3_COLOR_RAW_REFLECTED, 190 EV3_COLOR_COLOR_COMPONENTS, 191 192 EV3_ULTRASONIC_CM, 193 EV3_ULTRASONIC_INCHES, 194 EV3_ULTRASONIC_LISTEN, 195 196 EV3_INFRARED_PROXIMITY, 197 EV3_INFRARED_SEEK, 198 EV3_INFRARED_REMOTE, 199 """) 200 201 SENSOR_STATE = Enumeration(""" 202 VALID_DATA, 203 NOT_CONFIGURED, 204 CONFIGURING, 205 NO_DATA, 206 I2C_ERROR, 207 """) 208 209 SENSOR_CUSTOM = Enumeration(""" 210 PIN1_9V, 211 PIN5_OUT, 212 PIN5_STATE, 213 PIN6_OUT, 214 PIN6_STATE, 215 PIN1_ADC, 216 PIN6_ADC, 217 """) 218 """ 219 Flags for use with SENSOR_TYPE.CUSTOM 220 221 PIN1_9V 222 Enable 9V out on pin 1 (for LEGO NXT Ultrasonic sensor). 223 224 PIN5_OUT 225 Set pin 5 state to output. Pin 5 will be set to input if this flag is not set. 226 227 PIN5_STATE 228 If PIN5_OUT is set, this will set the state to output high, otherwise the state will 229 be output low. If PIN5_OUT is not set, this flag has no effect. 230 231 PIN6_OUT 232 Set pin 6 state to output. Pin 6 will be set to input if this flag is not set. 233 234 PIN6_STATE 235 If PIN6_OUT is set, this will set the state to output high, otherwise the state will 236 be output low. If PIN6_OUT is not set, this flag has no effect. 237 238 PIN1_ADC 239 Enable the analog/digital converter on pin 1 (e.g. for NXT analog sensors). 240 241 PIN6_ADC 242 Enable the analog/digital converter on pin 6. 243 """ 244 245 SENSOR_CUSTOM.PIN1_9V = 0x0002 246 SENSOR_CUSTOM.PIN5_OUT = 0x0010 247 SENSOR_CUSTOM.PIN5_STATE = 0x0020 248 SENSOR_CUSTOM.PIN6_OUT = 0x0100 249 SENSOR_CUSTOM.PIN6_STATE = 0x0200 250 SENSOR_CUSTOM.PIN1_ADC = 0x1000 251 SENSOR_CUSTOM.PIN6_ADC = 0x4000 252 253 SENSOR_I2C_SETTINGS = Enumeration(""" 254 MID_CLOCK, 255 PIN1_9V, 256 SAME, 257 ALLOW_STRETCH_ACK, 258 ALLOW_STRETCH_ANY, 259 """) 260 261 SENSOR_I2C_SETTINGS.MID_CLOCK = 0x01 # Send the clock pulse between reading and writing. Required by the NXT US sensor. 262 SENSOR_I2C_SETTINGS.PIN1_9V = 0x02 # 9v pullup on pin 1 263 SENSOR_I2C_SETTINGS.SAME = 0x04 # Keep performing the same transaction e.g. keep polling a sensor 264 265 MOTOR_STATUS_FLAG = Enumeration(""" 266 LOW_VOLTAGE_FLOAT, 267 """) 268 269 MOTOR_STATUS_FLAG.LOW_VOLTAGE_FLOAT = 0x01 # If the motors are floating due to low battery voltage 270 271 #SUCCESS = 0 272 #SPI_ERROR = 1 273 #SENSOR_ERROR = 2 274 #SENSOR_TYPE_ERROR = 3 275

276 - def __init__(self, addr = 1, detect = True): # Configure for the BrickPi. Optionally set the address (default to 1). Optionally disable detection (default to detect).

277 """ 278 Do any necessary configuration, and optionally detect the BrickPi3 279 280 Optionally specify the SPI address as something other than 1 281 Optionally disable the detection of the BrickPi3 hardware. This can be used for debugging and testing when the BrickPi3 would otherwise not pass the detection tests. 282 """ 283 284 if addr < 1 or addr > 255: 285 raise IOError("error: SPI address must be in the range of 1 to 255") 286 return 287 288 self.SPI_Address = addr 289 if detect == True: 290 try: 291 manufacturer = self.get_manufacturer() 292 board = self.get_board() 293 vfw = self.get_version_firmware() 294 except IOError(): 295 raise IOError("No SPI response") 296 if manufacturer != "Dexter Industries" or board != "BrickPi3": 297 raise IOError("No SPI response") 298 if vfw.split('.')[0] != FIRMWARE_VERSION_REQUIRED.split('.')[0] or vfw.split('.')[1] != FIRMWARE_VERSION_REQUIRED.split('.')[1]: 299 raise FirmwareVersionError("BrickPi3 firmware needs to be version %s but is currently version %s" % (FIRMWARE_VERSION_REQUIRED, vfw))

300

301 - def spi_transfer_array(self, data_out):

302 """ 303 Conduct a SPI transaction 304 305 Keyword arguments: 306 data_out -- a list of bytes to send. The length of the list will determine how many bytes are transferred. 307 308 Returns a list of the bytes read. 309 """ 310 return BP_SPI.xfer2(data_out)

311

312 - def spi_write_8(self, MessageType, Value):

313 """ 314 Send an 8-bit value over SPI 315 316 Keyword arguments: 317 MessageType -- the SPI message type 318 Value -- the value to be sent 319 """ 320 outArray = [self.SPI_Address, MessageType, (Value & 0xFF)] 321 self.spi_transfer_array(outArray)

322

323 - def spi_read_16(self, MessageType):

324 """ 325 Read a 16-bit value over SPI 326 327 Keyword arguments: 328 MessageType -- the SPI message type 329 330 Returns: 331 value 332 """ 333 outArray = [self.SPI_Address, MessageType, 0, 0, 0, 0] 334 reply = self.spi_transfer_array(outArray) 335 if(reply[3] == 0xA5): 336 return int((reply[4] << 8) | reply[5]) 337 raise IOError("No SPI response") 338 return

339

340 - def spi_write_16(self, MessageType, Value):

341 """ 342 Send a 16-bit value over SPI 343 344 Keyword arguments: 345 MessageType -- the SPI message type 346 Value -- the value to be sent 347 """ 348 outArray = [self.SPI_Address, MessageType, ((Value >> 8) & 0xFF), (Value & 0xFF)] 349 self.spi_transfer_array(outArray)

350

351 - def spi_write_24(self, MessageType, Value):

352 """ 353 Send a 24-bit value over SPI 354 355 Keyword arguments: 356 MessageType -- the SPI message type 357 Value -- the value to be sent 358 """ 359 outArray = [self.SPI_Address, MessageType, ((Value >> 16) & 0xFF), ((Value >> 8) & 0xFF), (Value & 0xFF)] 360 self.spi_transfer_array(outArray)

361

362 - def spi_read_32(self, MessageType):

363 """ 364 Read a 32-bit value over SPI 365 366 Keyword arguments: 367 MessageType -- the SPI message type 368 369 Returns : 370 value 371 """ 372 outArray = [self.SPI_Address, MessageType, 0, 0, 0, 0, 0, 0] 373 reply = self.spi_transfer_array(outArray) 374 if(reply[3] == 0xA5): 375 return int((reply[4] << 24) | (reply[5] << 16) | (reply[6] << 8) | reply[7]) 376 raise IOError("No SPI response") 377 return

378

379 - def spi_write_32(self, MessageType, Value):

380 """ 381 Send a 32-bit value over SPI 382 383 Keyword arguments: 384 MessageType -- the SPI message type 385 Value -- the value to be sent 386 """ 387 outArray = [self.SPI_Address, MessageType, ((Value >> 24) & 0xFF), ((Value >> 16) & 0xFF), ((Value >> 8) & 0xFF), (Value & 0xFF)] 388 self.spi_transfer_array(outArray)

389

390 - def get_manufacturer(self):

391 """ 392 Read the 20 charactor BrickPi3 manufacturer name 393 394 Returns: 395 BrickPi3 manufacturer name string 396 """ 397 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.GET_MANUFACTURER, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 398 reply = self.spi_transfer_array(outArray) 399 if(reply[3] == 0xA5): 400 name = "" 401 for c in range(4, 24): 402 if reply[c] != 0: 403 name += chr(reply[c]) 404 else: 405 break 406 return name 407 raise IOError("No SPI response") 408 return

409

410 - def get_board(self):

411 """ 412 Read the 20 charactor BrickPi3 board name 413 414 Returns: 415 BrickPi3 board name string 416 """ 417 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.GET_NAME, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 418 reply = self.spi_transfer_array(outArray) 419 if(reply[3] == 0xA5): 420 name = "" 421 for c in range(4, 24): 422 if reply[c] != 0: 423 name += chr(reply[c]) 424 else: 425 break 426 return name 427 raise IOError("No SPI response") 428 return

429

430 - def get_version_hardware(self):

431 """ 432 Read the hardware version 433 434 Returns: 435 hardware version 436 """ 437 version = self.spi_read_32(self.BPSPI_MESSAGE_TYPE.GET_HARDWARE_VERSION) 438 return ("%d.%d.%d" % ((version / 1000000), ((version / 1000) % 1000), (version % 1000)))

439

440 - def get_version_firmware(self):

441 """ 442 Read the firmware version 443 444 Returns: 445 firmware version 446 """ 447 version = self.spi_read_32(self.BPSPI_MESSAGE_TYPE.GET_FIRMWARE_VERSION) 448 return ("%d.%d.%d" % ((version / 1000000), ((version / 1000) % 1000), (version % 1000)))

449

450 - def get_id(self):

451 """ 452 Read the 128-bit BrickPi hardware serial number 453 454 Returns: 455 serial number as 32 char HEX formatted string 456 """ 457 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.GET_ID, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 458 reply = self.spi_transfer_array(outArray) 459 if(reply[3] == 0xA5): 460 return ("%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X" % (reply[4], reply[5], reply[6], reply[7], reply[8], reply[9], reply[10], reply[11], reply[12], reply[13], reply[14], reply[15], reply[16], reply[17], reply[18], reply[19])) 461 raise IOError("No SPI response") 462 return

463

464 - def set_led(self, value):

465 """ 466 Control the onboard LED 467 468 Keyword arguments: 469 value -- the value (in percent) to set the LED brightness to. -1 returns control of the LED to the firmware. 470 """ 471 self.spi_write_8(self.BPSPI_MESSAGE_TYPE.SET_LED, value)

472

473 - def get_voltage_3v3(self):

474 """ 475 Get the 3.3v circuit voltage 476 477 Returns: 478 3.3v circuit voltage 479 """ 480 value = self.spi_read_16(self.BPSPI_MESSAGE_TYPE.GET_VOLTAGE_3V3) 481 return (value / 1000.0)

482

483 - def get_voltage_5v(self):

484 """ 485 Get the 5v circuit voltage 486 487 Returns: 488 5v circuit voltage 489 """ 490 value = self.spi_read_16(self.BPSPI_MESSAGE_TYPE.GET_VOLTAGE_5V) 491 return (value / 1000.0)

492

493 - def get_voltage_9v(self):

494 """ 495 Get the 9v circuit voltage 496 497 Returns: 498 9v circuit voltage 499 """ 500 value = self.spi_read_16(self.BPSPI_MESSAGE_TYPE.GET_VOLTAGE_9V) 501 return (value / 1000.0)

502

503 - def get_voltage_battery(self):

504 """ 505 Get the battery voltage 506 507 Returns: 508 battery voltage 509 """ 510 value = self.spi_read_16(self.BPSPI_MESSAGE_TYPE.GET_VOLTAGE_VCC) 511 return (value / 1000.0)

512

513 - def set_sensor_type(self, port, type, params = 0):

514 """ 515 Set the sensor type 516 517 Keyword arguments: 518 port -- The sensor port(s). PORT_1, PORT_2, PORT_3, and/or PORT_4. 519 type -- The sensor type 520 params = 0 -- the parameters needed for some sensor types. 521 522 params is used for the following sensor types: 523 CUSTOM -- a 16-bit integer used to configure the hardware. 524 I2C -- a list of settings: 525 params[0] -- Settings/flags 526 params[1] -- target Speed in microseconds (0-255). Realistically the speed will vary. 527 if SENSOR_I2C_SETTINGS_SAME flag set in I2C Settings: 528 params[2] -- Delay in microseconds between transactions. 529 params[3] -- Address 530 params[4] -- List of bytes to write 531 params[5] -- Number of bytes to read 532 """ 533 for p in range(4): 534 if port & (1 << p): 535 self.SensorType[p] = type 536 if(type == self.SENSOR_TYPE.CUSTOM): 537 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.SET_SENSOR_TYPE, int(port), type, ((params[0] >> 8) & 0xFF), (params[0] & 0xFF)] 538 539 #self.spi_write_24(self.BPSPI_MESSAGE_TYPE.SET_SENSOR_TYPE, int(port), ((type << 16) + (params[0]))) 540 elif(type == self.SENSOR_TYPE.I2C): 541 if len(params) >= 2: 542 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.SET_SENSOR_TYPE, int(port), type, params[0], params[1]] # Settings, SpeedUS 543 if params[0] & self.SENSOR_I2C_SETTINGS.SAME and len(params) >= 6: 544 outArray.append((params[2] >> 24) & 0xFF) # DelayUS 545 outArray.append((params[2] >> 16) & 0xFF) # '' 546 outArray.append((params[2] >> 8) & 0xFF) # '' 547 outArray.append(params[2] & 0xFF) # '' 548 outArray.append(params[3] & 0xFF) # Address 549 outArray.append(params[5] & 0xFF) # InBytes 550 for p in range(4): 551 if port & (1 << p): 552 self.I2CInBytes[p] = params[5] & 0xFF 553 outArray.append(len(params[4])) # OutBytes 554 outArray.extend(params[4]) # OutArray 555 else: 556 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.SET_SENSOR_TYPE, int(port), type] 557 558 self.spi_transfer_array(outArray)

559

560 - def transact_i2c(self, port, Address, OutArray, InBytes):

561 """ 562 Conduct an I2C transaction 563 564 Keyword arguments: 565 port -- The sensor port (one at a time). PORT_1, PORT_2, PORT_3, or PORT_4. 566 Address -- The I2C address for the device. Bits 1-7, not 0-6. 567 OutArray -- A list of bytes to write to the device 568 InBytes -- The number of bytes to read from the device 569 """ 570 if port == self.PORT_1: 571 message_type = self.BPSPI_MESSAGE_TYPE.I2C_TRANSACT_1 572 port_index = 0 573 elif port == self.PORT_2: 574 message_type = self.BPSPI_MESSAGE_TYPE.I2C_TRANSACT_2 575 port_index = 1 576 elif port == self.PORT_3: 577 message_type = self.BPSPI_MESSAGE_TYPE.I2C_TRANSACT_3 578 port_index = 2 579 elif port == self.PORT_4: 580 message_type = self.BPSPI_MESSAGE_TYPE.I2C_TRANSACT_4 581 port_index = 3 582 else: 583 raise IOError("transact_i2c error. Must be one sensor port at a time. PORT_1, PORT_2, PORT_3, or PORT_4.") 584 return 585 586 if self.SensorType[port_index] != self.SENSOR_TYPE.I2C: 587 return 588 outArray = [self.SPI_Address, message_type, Address, InBytes] 589 self.I2CInBytes[port_index] = InBytes 590 OutBytes = len(OutArray) 591 if(OutBytes > 16): 592 outArray.append(16) 593 for b in range(16): 594 outArray.append(OutArray[b]) 595 else: 596 outArray.append(OutBytes) 597 outArray.extend(OutArray) 598 self.spi_transfer_array(outArray)

599

600 - def get_sensor(self, port):

601 """ 602 Read a sensor value 603 604 Keyword arguments: 605 port -- The sensor port (one at a time). PORT_1, PORT_2, PORT_3, or PORT_4. 606 607 Returns the value(s) for the specified sensor. 608 The following sensor types each return a single value: 609 NONE ----------------------- 0 610 TOUCH ---------------------- 0 or 1 (released or pressed) 611 NXT_TOUCH ------------------ 0 or 1 (released or pressed) 612 EV3_TOUCH ------------------ 0 or 1 (released or pressed) 613 NXT_ULTRASONIC ------------- distance in CM 614 NXT_LIGHT_ON -------------- reflected light 615 NXT_LIGHT_OFF -------------- ambient light 616 NXT_COLOR_RED -------------- red reflected light 617 NXT_COLOR_GREEN ------------ green reflected light 618 NXT_COLOR_BLUE ------------- blue reflected light 619 NXT_COLOR_OFF -------------- ambient light 620 EV3_GYRO_ABS --------------- absolute rotation position in degrees 621 EV3_GYRO_DPS --------------- rotation rate in degrees per second 622 EV3_COLOR_REFLECTED -------- red reflected light 623 EV3_COLOR_AMBIENT ---------- ambient light 624 EV3_COLOR_COLOR ------------ detected color 625 EV3_ULTRASONIC_CM ---------- distance in CM 626 EV3_ULTRASONIC_INCHES ------ distance in inches 627 EV3_ULTRASONIC_LISTEN ------ 0 or 1 (no other ultrasonic sensors or another ultrasonic sensor detected) 628 EV3_INFRARED_PROXIMITY ----- distance 0-100% 629 630 The following sensor types each return a list of values 631 CUSTOM --------------------- Pin 1 ADC (5v scale from 0 to 4095), Pin 6 ADC (3.3v scale from 0 to 4095), Pin 5 digital, Pin 6 digital 632 I2C ------------------------ the I2C bytes read 633 NXT_COLOR_FULL ------------- detected color, red light reflected, green light reflected, blue light reflected, ambient light 634 EV3_GYRO_ABS_DPS ----------- absolute rotation position in degrees, rotation rate in degrees per second 635 EV3_COLOR_RAW_REFLECTED ---- red reflected light, unknown value (maybe a raw ambient value?) 636 EV3_COLOR_COLOR_COMPONENTS - red reflected light, green reflected light, blue reflected light, unknown value (maybe a raw value?) 637 EV3_INFRARED_SEEK ---------- a list for each of the four channels. For each channel heading (-25 to 25), distance (-128 or 0 to 100) 638 EV3_INFRARED_REMOTE -------- a list for each of the four channels. For each channel red up, red down, blue up, blue down, boadcast 639 640 """ 641 if port == self.PORT_1: 642 message_type = self.BPSPI_MESSAGE_TYPE.GET_SENSOR_1 643 port_index = 0 644 elif port == self.PORT_2: 645 message_type = self.BPSPI_MESSAGE_TYPE.GET_SENSOR_2 646 port_index = 1 647 elif port == self.PORT_3: 648 message_type = self.BPSPI_MESSAGE_TYPE.GET_SENSOR_3 649 port_index = 2 650 elif port == self.PORT_4: 651 message_type = self.BPSPI_MESSAGE_TYPE.GET_SENSOR_4 652 port_index = 3 653 else: 654 raise IOError("get_sensor error. Must be one sensor port at a time. PORT_1, PORT_2, PORT_3, or PORT_4.") 655 return 656 657 if self.SensorType[port_index] == self.SENSOR_TYPE.CUSTOM: 658 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0] 659 reply = self.spi_transfer_array(outArray) 660 if(reply[3] == 0xA5): 661 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA): 662 return [(((reply[8] & 0x0F) << 8) | reply[9]), (((reply[8] >> 4) & 0x0F) | (reply[7] << 4)), (reply[6] & 0x01), ((reply[6] >> 1) & 0x01)] 663 else: 664 raise SensorError("get_sensor error: Invalid sensor data") 665 return 666 else: 667 raise IOError("get_sensor error: No SPI response") 668 return 669 670 elif self.SensorType[port_index] == self.SENSOR_TYPE.I2C: 671 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0] 672 for b in range(self.I2CInBytes[port_index]): 673 outArray.append(0) 674 reply = self.spi_transfer_array(outArray) 675 if(reply[3] == 0xA5): 676 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA and len(reply) - 6 == self.I2CInBytes[port_index]): 677 values = [] 678 for b in range(6, len(reply)): 679 values.append(reply[b]) 680 return values 681 else: 682 raise SensorError("get_sensor error: Invalid sensor data") 683 return 684 else: 685 raise IOError("get_sensor error: No SPI response") 686 return 687 688 elif(self.SensorType[port_index] == self.SENSOR_TYPE.TOUCH 689 or self.SensorType[port_index] == self.SENSOR_TYPE.NXT_TOUCH 690 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_TOUCH 691 or self.SensorType[port_index] == self.SENSOR_TYPE.NXT_ULTRASONIC 692 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_COLOR_REFLECTED 693 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_COLOR_AMBIENT 694 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_COLOR_COLOR 695 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_ULTRASONIC_LISTEN 696 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_INFRARED_PROXIMITY): 697 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0] 698 reply = self.spi_transfer_array(outArray) 699 if(reply[3] == 0xA5): 700 if((reply[4] == self.SensorType[port_index] or (self.SensorType[port_index] == self.SENSOR_TYPE.TOUCH and (reply[4] == self.SENSOR_TYPE.NXT_TOUCH or reply[4] == self.SENSOR_TYPE.EV3_TOUCH))) and reply[5] == self.SENSOR_STATE.VALID_DATA): 701 return reply[6] 702 else: 703 raise SensorError("get_sensor error: Invalid sensor data") 704 return 705 else: 706 raise IOError("get_sensor error: No SPI response") 707 return 708 709 elif self.SensorType[port_index] == self.SENSOR_TYPE.NXT_COLOR_FULL: 710 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 711 reply = self.spi_transfer_array(outArray) 712 if(reply[3] == 0xA5): 713 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA): 714 return [reply[6], ((reply[7] << 2) | ((reply[11] >> 6) & 0x03)), ((reply[8] << 2) | ((reply[11] >> 4) & 0x03)), ((reply[9] << 2) | ((reply[11] >> 2) & 0x03)), ((reply[10] << 2) | (reply[11] & 0x03))] 715 else: 716 raise SensorError("get_sensor error: Invalid sensor data") 717 return 718 else: 719 raise IOError("get_sensor error: No SPI response") 720 return 721 722 elif(self.SensorType[port_index] == self.SENSOR_TYPE.NXT_LIGHT_ON 723 or self.SensorType[port_index] == self.SENSOR_TYPE.NXT_LIGHT_OFF 724 or self.SensorType[port_index] == self.SENSOR_TYPE.NXT_COLOR_RED 725 or self.SensorType[port_index] == self.SENSOR_TYPE.NXT_COLOR_GREEN 726 or self.SensorType[port_index] == self.SENSOR_TYPE.NXT_COLOR_BLUE 727 or self.SensorType[port_index] == self.SENSOR_TYPE.NXT_COLOR_OFF 728 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_GYRO_ABS 729 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_GYRO_DPS 730 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_ULTRASONIC_CM 731 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_ULTRASONIC_INCHES): 732 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0] 733 reply = self.spi_transfer_array(outArray) 734 if(reply[3] == 0xA5): 735 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA): 736 value = int((reply[6] << 8) | reply[7]) 737 if((self.SensorType[port_index] == self.SENSOR_TYPE.EV3_GYRO_ABS 738 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_GYRO_DPS) 739 and (value & 0x8000)): 740 value = value - 0x10000 741 elif(self.SensorType[port_index] == self.SENSOR_TYPE.EV3_ULTRASONIC_CM 742 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_ULTRASONIC_INCHES): 743 value = value / 10 744 return value 745 else: 746 raise SensorError("get_sensor error: Invalid sensor data") 747 return 748 else: 749 raise IOError("get_sensor error: No SPI response") 750 return 751 752 elif(self.SensorType[port_index] == self.SENSOR_TYPE.EV3_COLOR_RAW_REFLECTED 753 or self.SensorType[port_index] == self.SENSOR_TYPE.EV3_GYRO_ABS_DPS): 754 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0] 755 reply = self.spi_transfer_array(outArray) 756 if(reply[3] == 0xA5): 757 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA): 758 results = [int((reply[6] << 8) | reply[7]), int((reply[8] << 8) | reply[9])] 759 if self.SensorType[port_index] == self.SENSOR_TYPE.EV3_GYRO_ABS_DPS: 760 for r in range(len(results)): 761 if results[r] >= 0x8000: 762 results[r] = results[r] - 0x10000 763 return results 764 else: 765 raise SensorError("get_sensor error: Invalid sensor data") 766 return 767 else: 768 raise IOError("get_sensor error: No SPI response") 769 return 770 771 elif(self.SensorType[port_index] == self.SENSOR_TYPE.EV3_COLOR_COLOR_COMPONENTS): 772 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 773 reply = self.spi_transfer_array(outArray) 774 if(reply[3] == 0xA5): 775 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA): 776 return [int((reply[6] << 8) | reply[7]), int((reply[8] << 8) | reply[9]), int((reply[10] << 8) | reply[11]), int((reply[12] << 8) | reply[13])] 777 else: 778 raise SensorError("get_sensor error: Invalid sensor data") 779 return 780 else: 781 raise IOError("get_sensor error: No SPI response") 782 return 783 784 elif(self.SensorType[port_index] == self.SENSOR_TYPE.EV3_INFRARED_SEEK): 785 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 786 reply = self.spi_transfer_array(outArray) 787 if(reply[3] == 0xA5): 788 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA): 789 results = [[int(reply[6]), int(reply[7])], [int(reply[8]), int(reply[9])], [int(reply[10]), int(reply[11])], [int(reply[12]), int(reply[13])]] 790 for c in range(len(results)): 791 for v in range(len(results[c])): 792 if results[c][v] >= 0x80: 793 results[c][v] = results[c][v] - 0x100 794 return results 795 else: 796 raise SensorError("get_sensor error: Invalid sensor data") 797 return 798 else: 799 raise IOError("get_sensor error: No SPI response") 800 return 801 802 elif(self.SensorType[port_index] == self.SENSOR_TYPE.EV3_INFRARED_REMOTE): 803 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0] 804 reply = self.spi_transfer_array(outArray) 805 if(reply[3] == 0xA5): 806 if(reply[4] == self.SensorType[port_index] and reply[5] == self.SENSOR_STATE.VALID_DATA): 807 results = [0, 0, 0, 0] 808 for r in range(len(results)): 809 value = int(reply[6 + r]) 810 if value == 1: 811 results[r] = [1, 0, 0, 0, 0] 812 elif value == 2: 813 results[r] = [0, 1, 0, 0, 0] 814 elif value == 3: 815 results[r] = [0, 0, 1, 0, 0] 816 elif value == 4: 817 results[r] = [0, 0, 0, 1, 0] 818 elif value == 5: 819 results[r] = [1, 0, 1, 0, 0] 820 elif value == 6: 821 results[r] = [1, 0, 0, 1, 0] 822 elif value == 7: 823 results[r] = [0, 1, 1, 0, 0] 824 elif value == 8: 825 results[r] = [0, 1, 0, 1, 0] 826 elif value == 9: 827 results[r] = [0, 0, 0, 0, 1] 828 elif value == 10: 829 results[r] = [1, 1, 0, 0, 0] 830 elif value == 11: 831 results[r] = [0, 0, 1, 1, 0] 832 else: 833 results[r] = [0, 0, 0, 0, 0] 834 return results 835 else: 836 raise SensorError("get_sensor error: Invalid sensor data") 837 return 838 else: 839 raise IOError("get_sensor error: No SPI response") 840 return 841 842 raise IOError("get_sensor error: Sensor not configured or not supported.") 843 return # sensor not configured or not supported.

844

845 - def set_motor_power(self, port, power):

846 """ 847 Set the motor power in percent 848 849 Keyword arguments: 850 port -- The Motor port(s). PORT_A, PORT_B, PORT_C, and/or PORT_D. 851 power -- The power from -100 to 100, or -128 for float 852 """ 853 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.SET_MOTOR_POWER, int(port), int(power)] 854 self.spi_transfer_array(outArray)

855

856 - def set_motor_position(self, port, position):

857 """ 858 Set the motor target position in degrees 859 860 Keyword arguments: 861 port -- The motor port(s). PORT_A, PORT_B, PORT_C, and/or PORT_D. 862 position -- The target position 863 """ 864 position = int(position) 865 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.SET_MOTOR_POSITION, int(port), ((position >> 24) & 0xFF), ((position >> 16) & 0xFF), ((position >> 8) & 0xFF), (position & 0xFF)] 866 self.spi_transfer_array(outArray)

867

868 - def set_motor_dps(self, port, dps):

869 """ 870 Set the motor target speed in degrees per second 871 872 Keyword arguments: 873 port -- The motor port(s). PORT_A, PORT_B, PORT_C, and/or PORT_D. 874 dps -- The target speed in degrees per second 875 """ 876 dps = int(dps) 877 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.SET_MOTOR_DPS, int(port), ((dps >> 8) & 0xFF), (dps & 0xFF)] 878 self.spi_transfer_array(outArray)

879

880 - def set_motor_limits(self, port, power = 0, dps = 0):

881 """ 882 Set the motor speed limit 883 884 Keyword arguments: 885 port -- The motor port(s). PORT_A, PORT_B, PORT_C, and/or PORT_D. 886 power -- The power limit in percent (0 to 100), with 0 being no limit (100) 887 dps -- The speed limit in degrees per second, with 0 being no limit 888 """ 889 dps = int(dps) 890 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.SET_MOTOR_LIMITS, int(port), int(power), ((dps >> 8) & 0xFF), (dps & 0xFF)] 891 print(outArray) 892 self.spi_transfer_array(outArray)

893

894 - def get_motor_status(self, port):

895 """ 896 Read a motor status 897 898 Keyword arguments: 899 port -- The motor port (one at a time). PORT_A, PORT_B, PORT_C, or PORT_D. 900 901 Returns a list: 902 flags -- 8-bits of bit-flags that indicate motor status: 903 bit 0 -- LOW_VOLTAGE_FLOAT - The motors are automatically disabled because the battery voltage is too low 904 bit 1 -- OVERLOADED - The motors aren't close to the target (applies to position control and dps speed control). 905 power -- the raw PWM power in percent (-100 to 100) 906 encoder -- The encoder position 907 dps -- The current speed in Degrees Per Second 908 """ 909 if port == self.PORT_A: 910 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_A_STATUS 911 elif port == self.PORT_B: 912 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_B_STATUS 913 elif port == self.PORT_C: 914 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_C_STATUS 915 elif port == self.PORT_D: 916 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_D_STATUS 917 else: 918 raise IOError("get_motor_status error. Must be one motor port at a time. PORT_A, PORT_B, PORT_C, or PORT_D.") 919 return 920 921 outArray = [self.SPI_Address, message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 922 reply = self.spi_transfer_array(outArray) 923 if(reply[3] == 0xA5): 924 speed = int(reply[5]) 925 if speed & 0x80: 926 speed = speed - 0x100 927 928 encoder = int((reply[6] << 24) | (reply[7] << 16) | (reply[8] << 8) | reply[9]) 929 if encoder & 0x80000000: # MT was 0x10000000, but I think it should be 0x80000000 930 encoder = int(encoder - 0x100000000) 931 932 dps = int((reply[10] << 8) | reply[11]) 933 if dps & 0x8000: 934 dps = dps - 0x10000 935 936 return [reply[4], speed, encoder, dps] 937 raise IOError("No SPI response") 938 return

939

940 - def offset_motor_encoder(self, port, position):

941 """ 942 Offset a motor encoder 943 944 Keyword arguments: 945 port -- The motor port(s). PORT_A, PORT_B, PORT_C, and/or PORT_D. 946 offset -- The encoder offset 947 948 Zero the encoder by offsetting it by the current position 949 """ 950 position = int(position) 951 outArray = [self.SPI_Address, self.BPSPI_MESSAGE_TYPE.OFFSET_MOTOR_ENCODER, int(port), ((position >> 24) & 0xFF), ((position >> 16) & 0xFF), ((position >> 8) & 0xFF), (position & 0xFF)] 952 self.spi_transfer_array(outArray)

953

954 - def get_motor_encoder(self, port):

955 """ 956 Read a motor encoder in degrees 957 958 Keyword arguments: 959 port -- The motor port (one at a time). PORT_A, PORT_B, PORT_C, or PORT_D. 960 961 Returns the encoder position in degrees 962 """ 963 if port == self.PORT_A: 964 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_A_ENCODER 965 elif port == self.PORT_B: 966 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_B_ENCODER 967 elif port == self.PORT_C: 968 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_C_ENCODER 969 elif port == self.PORT_D: 970 message_type = self.BPSPI_MESSAGE_TYPE.GET_MOTOR_D_ENCODER 971 else: 972 raise IOError("get_motor_encoder error. Must be one motor port at a time. PORT_A, PORT_B, PORT_C, or PORT_D.") 973 return 974 975 encoder = self.spi_read_32(message_type) 976 if encoder & 0x80000000: 977 encoder = int(encoder - 0x100000000) 978 return int(encoder)

979

980 - def reset_all(self):

981 """ 982 Reset the BrickPi. Set all the sensors' type to NONE, set the motors' speed to 0, and return control of the LED to the firmware. 983 """ 984 # reset all sensors 985 self.set_sensor_type(self.PORT_1 + self.PORT_2 + self.PORT_3 + self.PORT_4, self.SENSOR_TYPE.NONE) 986 987 # turn off all motors 988 self.set_motor_power(self.PORT_A + self.PORT_B + self.PORT_C + self.PORT_D, self.MOTOR_FLOAT) 989 990 # reset motor limits 991 self.set_motor_limits(self.PORT_A + self.PORT_B + self.PORT_C + self.PORT_D, 0, 0) 992 993 # return the LED to the control of the FW 994 self.set_led(-1)

995

Source Code for Module brickpi3