eddiebot_driver

214 215 """A higher-level wrapper around PySerial specifically designed for use with 216 Parallax Eddie Propeller Board. 217 218 """ 219

220 - def __init__(self, tty, baudrate):

221 self.ser = serial.Serial( 222 port='/dev/ttyUSB0', 223 baudrate=BAUDRATE, 224 parity=serial.PARITY_NONE, 225 stopbits=serial.STOPBITS_ONE, 226 bytesize=serial.EIGHTBITS, 227 timeout=SERIAL_TIMEOUT 228 ) 229 230 self.ser.open() 231 # self.wake() 232 self.opcodes = {} 233 234 #TODO: kwc all locking code should be outside of the driver. Instead, 235 #could place a lock object in Roomba and let people "with" it 236 self.lock = threading.RLock()

237

238 - def wake(self):

239 """wake up robot.""" 240 print "wake"

241 # self.ser.setRTS(0) 242 # time.sleep(0.1) 243 # self.ser.setRTS(1) 244 # time.sleep(0.75) # Technically it should wake after 500ms. 245 # for i in range(3): 246 # self.ser.setRTS(0) 247 # time.sleep(0.25) 248 # self.ser.setRTS(1) 249 # time.sleep(0.25) 250

251 - def add_opcodes(self, opcodes):

252 """Add available opcodes to the SCI.""" 253 self.opcodes.update(opcodes)

254

255 - def send(self, bytes):

256 """send a string of bytes to the robot.""" 257 with self.lock: 258 self.ser.write(bytes)

259 260 #TODO: kwc the locking should be done at a higher level

261 - def read(self, num_bytes):

262 """Read a string of 'num_bytes' bytes from the robot.""" 263 # logging.debug('Attempting to read %d bytes from SCI port.' % num_bytes) 264 # with self.lock: 265 data = self.ser.read(num_bytes) 266 # logging.debug('Read %d bytes from SCI port.' % len(data)) 267 # if not data: 268 # raise DriverError('Error reading from SCI port. No data.') 269 # if len(data) != num_bytes: 270 # raise DriverError('Error reading from SCI port. Wrong data length.') 271 return data

272

273 - def flush_input(self):

274 """Flush input buffer, discarding all its contents.""" 275 logging.debug('Flushing serial input buffer.') 276 self.ser.flushInput()

277

278 - def inWaiting(self):

279 """ InWaiting Called """ 280 logging.debug('Called inWaiting') 281 self.ser.inWaiting()

282

283 - def __getattr__(self, name):

284 """Eddiebots methods for opcodes on the fly. 285 286 Each opcode method sends the opcode optionally followed by a string of 287 parameter. 288 289 """ 290 #TODO: kwc do static initialization instead 291 if name in self.opcodes: 292 def send_opcode(input_string): 293 logging.debug('sending opcode %s.' % name) 294 self.send(self.opcodes[name] + ' ' + input_string)

295 return send_opcode 296 raise AttributeError

304 305 """Represents a Eddiebot robot.""" 306

307 - def __init__(self):

308 """ 309 @param sensor_class: Sensor class to use for fetching and decoding sensor data. 310 """ 311 logging.basicConfig(filename='eddiebot_driver.log', level=logging.INFO) 312 self.tty = None 313 self.sci = None 314 self.safe = True 315 self.start()

316

317 - def start(self, tty='/dev/ttyUSB0', baudrate=115200):

318 self.tty = tty 319 self.sci = SerialCommandInterface(tty, baudrate) 320 self.sci.add_opcodes(EDDIE_OPCODES)

321

322 - def control(self):

323 """Start the robot's SCI interface and place it in safe or full mode.""" 324 logging.info('sending control opcodes.')

325 # self.passive() Not implemented in Eddie 326 # if self.safe: 327 # self.sci.safe() 328 # else: 329 # self.sci.full() 330 # time.sleep(0.5) 331

332 - def power_low_side_drivers(self, drivers):

333 """Enable or disable power to low side drivers. 334 335 'drivers' should be a list of booleans indicating which low side drivers 336 should be powered. 337 338 """ 339 assert len(drivers) == 3, 'Expecting 3 low side driver power settings.' 340 byte = 0 341 for driver, power in enumerate(drivers): 342 byte += (2 ** driver) * int(power)

343 # self.sci.low_side_drivers(byte) Not implemented in Eddie 344

345 - def set_digital_outputs(self, value):

346 """Enable or disable digital outputs."""

347 # self.sci.digital_outputs(value) Not implemented in Eddie 348

349 - def soft_reset(self):

350 """Do a soft reset of the Turtlebot.""" 351 logging.info('sending soft reset.')

352 # self.sci.soft_reset() 353 # time.sleep(START_DELAY) 354 # self.passive() 355 356 357 #------------------- Roomba methods -----------------------------

358 - def change_baud_rate(self, baud_rate):

359 """Sets the baud rate in bits per second (bps) at which SCI commands and 360 data are sent according to the baud code sent in the data byte. 361 362 The default baud rate at power up is 57600 bps. (See Serial Port Settings, 363 above.) Once the baud rate is changed, it will persist until Roomba is 364 power cycled by removing the battery (or until the battery voltage falls 365 below the minimum requir''' ed for processor operation). You must wait 100ms 366 after sending this command before sending additional commands at the new 367 baud rate. The SCI must be in passive, safe, or full mode to accept this 368 command. This command puts the SCI in passive mode. 369 370 """ 371 if baud_rate not in BAUD_RATES: 372 raise DriverError('Invalid baud rate specified.') 373 self.sci.baud(baud_rate) 374 self.sci = SerialCommandInterface(self.tty, baud_rate)

375

376 - def passive(self):

377 """Put the robot in passive mode.""" 378 # self.sci.start() Not implemented in Eddie 379 print "passive"

380 # time.sleep(0.5) 381

382 - def direct_drive(self, velocity_left, velocity_right):

383 # print("direct_drive(self, velocity_left, velocity_right)") 384 # print("velocity_left: " + str(int(velocity_left))) 385 # print("velocity_right: " + str(int(velocity_right))) 386 # Mask integers to 2 bytes. 387 vl = int(velocity_left) & 0xffff 388 vr = int(velocity_right) & 0xffff 389 390 parameters = binascii.hexlify(struct.pack('>2H', vr, vl)) 391 392 parameters_output = '' 393 394 for i in range(len(parameters)): 395 parameters_output += parameters[i] 396 if(i == 3): 397 parameters_output += ' ' 398 399 self.sci.direct_drive(parameters_output + chr(13))

400

401 - def drive(self, velocity, radius):

402 logging.debug("drive(self, velocity, radius)") 403 """controls Roomba's drive wheels. 404 405 NOTE(damonkohler): The following specification applies to both the Roomba 406 and the Turtlebot. 407 408 The Roomba takes four data bytes, interpreted as two 16-bit signed values 409 using two's complement. The first two bytes specify the average velocity 410 of the drive wheels in millimeters per second (mm/s), with the high byte 411 being sent first. The next two bytes specify the radius in millimeters at 412 which Roomba will turn. The longer radii make Roomba drive straighter, 413 while the shorter radii make Roomba turn more. The radius is measured from 414 the center of the turning circle to the center of Roomba. 415 416 A drive command with a positive velocity and a positive radius makes 417 Roomba drive forward while turning toward the left. A negative radius 418 makes Roomba turn toward the right. Special cases for the radius make 419 Roomba turn in place or drive straight, as specified below. A negative 420 velocity makes Roomba drive backward. 421 422 Also see drive_straight and turn_in_place convenience methods. 423 424 """ 425 # Mask integers to 2 bytes. 426 velocity = int(velocity)# & 0xffff TODO:removed the marks to avoid problem 427 radius = int(radius)# & 0xffff 428 429 # Convert to direct_drive parameter 430 velocity_left = ((2000.0 + radius) / 2000.0) * velocity 431 velocity_right = ((2000.0 - radius) / 2000.0) * velocity 432 433 # STRAIGHT radius set to velocity to drive straight line FW and BW 434 if(RADIUS_STRAIGHT == radius): 435 velocity_left = velocity 436 velocity_right = velocity 437 438 self.direct_drive(velocity_left, velocity_right)

439

440 - def command_joints(self, pan_degree, tilt_degree):

441 pan_degree = int(pan_degree) 442 tilt_degree = int(tilt_degree) 443 parameters = binascii.hexlify(struct.pack('>H', pan_degree)) 444 self.sci.motors(' 4 ' + parameters + chr(13)) # Pan Servo

445 446 #parameters = binascii.hexlify(struct.pack('>H', tilt_degree)) 447 #self.sci.motors(' 5 ' + parameters + chr(13)) # Pan Servo 448

449 - def stop(self):

450 """Set velocity and radius to 0 to stop movement.""" 451 self.direct_drive(0, 0)

452

453 - def slow_stop(self, velocity):

454 """Slowly reduce the velocity to 0 to stop movement.""" 455 velocities = xrange(velocity, VELOCITY_SLOW, -8) 456 if velocity < 0: 457 velocities = xrange(velocity, -VELOCITY_SLOW, 8) 458 for v in velocities: 459 self.drive(v, RADIUS_STRAIGHT) 460 time.sleep(1) 461 self.stop()

462 463 # def drive_straight(self, velocity): 464 # """drive in a straight line.""" 465 # self.gospd(velocity) 466

467 - def drive_straight(self, velocity):

468 self.drive(velocity, RADIUS_STRAIGHT)

469

470 - def turn_in_place(self, velocity, direction):

471 """Turn in place either clockwise or counter-clockwise.""" 472 # valid_directions = {'cw': RADIUS_TURN_IN_PLACE_CW, 473 # 'ccw': RADIUS_TURN_IN_PLACE_CCW} 474 # self.drive(velocity, valid_directions[direction]) 475 velocity = int(velocity) 476 if(direction == 'cw'): 477 self.direct_drive(-velocity, velocity) 478 if(direction == 'ccw'): 479 self.direct_drive(velocity, -velocity)

480 481

482 - def dock(self):

483 """Start looking for the dock and then dock.""" 484 # NOTE(damonkohler): We should be able to call dock from any mode, however 485 # it only seems to work from passive. 486 #self.sci.start() Not implemented in Eddie 487 # time.sleep(0.01) 488 # self.sci.force_seeking_dock() Not implemented in Eddie 489 print "dock"

490

491 - def sensors(self):

492 """This is a sensor command to emulate SCI sensor sending""" 493 # =========== Ping ============ 494 495 ping_left_f = 0 496 ping_right_f = 0 497 ping_left_r = 0 498 ping_right_r = 0 499 500 ping_left_f_str = '' 501 ping_right_f_str = '' 502 ping_left_r_str = '' 503 ping_right_r_str = '' 504 505 self.sci.getSer().write('PING' + chr(13)) 506 out = '' 507 # let's wait one second before reading output (let's give device time to answer) 508 time.sleep(0.07) 509 i = 0; 510 while self.sci.getSer().inWaiting() > 0: 511 out += self.sci.getSer().read(1) 512 if(i == 0 or i == 1 or i == 2): 513 ping_right_f_str += out[i] 514 if(i == 4 or i == 5 or i == 6): 515 ping_left_f_str += out[i] 516 if(i == 8 or i == 9 or i == 10): 517 ping_right_r_str += out[i] 518 if(i == 12 or i == 13 or i == 14): 519 ping_left_r_str += out[i] 520 i+=1 521 522 try: 523 ping_left_f = int(ping_left_f_str,16) 524 ping_right_f = int(ping_right_f_str,16) 525 ping_left_r = int(ping_left_r_str,16) 526 ping_right_r = int(ping_right_r_str,16) 527 except Exception as ex: 528 print ex 529 530 # print "ping_left: " + str(ping_left) 531 # print "ping_right: " + str(ping_right) 532 533 ### ========= ADC ============= 534 535 adc_left_f_str = '' 536 adc_right_f_str = '' 537 adc_center_f_str = '' 538 adc_left_r_str = '' 539 adc_right_r_str = '' 540 adc_center_r_str = '' 541 542 543 adc_battery_str = '' 544 self.sci.getSer().write('ADC' + chr(13)) 545 out = '' 546 # let's wait one second before reading output (let's give device time to answer) 547 time.sleep(0.07) 548 549 i = 0; 550 while self.sci.getSer().inWaiting() > 0: 551 out += self.sci.getSer().read(1) 552 if(i == 0 or i == 1 or i == 2): 553 adc_right_f_str += out[i] 554 if(i == 4 or i == 5 or i == 6): 555 adc_center_f_str += out[i] 556 if(i == 8 or i == 9 or i == 10): 557 adc_left_f_str += out[i] 558 if(i == 12 or i == 13 or i == 14): 559 adc_right_r_str += out[i] 560 if(i == 16 or i == 17 or i == 18): 561 adc_center_r_str += out[i] 562 if(i == 20 or i == 21 or i == 22): 563 adc_left_r_str += out[i] 564 565 if(i == 28 or i == 29 or i == 30): 566 adc_battery_str += out[i] 567 i+=1 568 569 try: 570 adc_left_f = int(adc_left_f_str,16) 571 adc_right_f = int(adc_right_f_str,16) 572 adc_center_f = int(adc_center_f_str,16) 573 574 adc_left_r = int(adc_left_r_str,16) 575 adc_right_r = int(adc_right_r_str,16) 576 adc_center_r = int(adc_center_r_str,16) 577 adc_battery = int(adc_battery_str,16) 578 except Exception as ex: 579 print ex 580 581 # print "left: " + str(adc_left) 582 # print "right: " + str(adc_right) 583 # print "center: " + str(adc_center) 584 # print "battery: " + str(adc_battery) 585 PING_THRESHOLD = 0 586 ADC_THRESHOLD = 800 587 588 ping_left_bumps_f = False 589 ping_right_bumps_f = False 590 ping_left_bumps_r = False 591 ping_right_bumps_r = False 592 593 adc_left_bumps_f = False 594 adc_right_bumps_f = False 595 adc_center_bumps_f = False 596 adc_left_bumps_r = False 597 adc_right_bumps_r = False 598 adc_center_bumps_r = False 599 600 # if(ping_left_f < PING_THRESHOLD): 601 # ping_left_bumps_f = True 602 # if(ping_right_f < PING_THRESHOLD): 603 # ping_right_bumps_f = True 604 # if(ping_left_r < PING_THRESHOLD): 605 # ping_left_bumps_r = True 606 # if(ping_right_r < PING_THRESHOLD): 607 # ping_right_bumps_r = True 608 609 if(adc_left_f > ADC_THRESHOLD): 610 adc_left_bumps_f = True 611 if(adc_right_f > ADC_THRESHOLD + 100): 612 adc_right_bumps_f = True 613 if(adc_center_f > ADC_THRESHOLD + 100): 614 adc_center_bumps_f = True 615 if(adc_left_r > ADC_THRESHOLD + 300): # Plus 300 to avoid side sensing during narrow sliding through the obstacle 616 adc_left_bumps_r = True 617 if(adc_right_r > ADC_THRESHOLD + 300): 618 adc_right_bumps_r = True 619 # if(adc_center_r > ADC_THRESHOLD): 620 # adc_center_bumps_r = True 621 622 # print "Bumps!!!!!!!!!:" 623 # print ping_left_bumps_f or ping_right_bumps_f or adc_left_bumps_f or adc_right_bumps_f or adc_center_bumps_f or ping_left_bumps_r or ping_right_bumps_r or adc_left_bumps_r or adc_right_bumps_r or adc_center_bumps_r 624 # 625 return (0,#1 626 adc_center_bumps_f,#2 wall 627 adc_left_bumps_r,#3 cliff_left 628 adc_left_bumps_f,#4 cliff_front_left 629 adc_right_bumps_f,#5 cliff_front_right 630 adc_right_bumps_r,#6 cliff_right 631 ping_left_bumps_f or ping_right_bumps_f or adc_left_bumps_f or adc_right_bumps_f or adc_center_bumps_f or ping_left_bumps_r or ping_right_bumps_r or adc_left_bumps_r or adc_right_bumps_r or adc_center_bumps_r,#7 632 0,#8 633 0,#9 634 0,#10 635 0,#11 636 0,#12 637 0,#13 638 0,#14 639 0,#15 640 0,#16 641 0,#17 642 0,#18 643 0,#19 644 0,#20 adc_battery, 645 adc_center_f,#21 646 adc_left_r,#22 647 adc_left_f,#23 648 adc_right_f,#24 649 adc_right_r,#25 650 0,#26 651 adc_center_r,#27 652 0,#28 653 0,#29 654 0,#30 655 0,#31 656 0,#32 657 ping_left_f,#33 658 ping_right_f,#34 659 ping_left_r,#35 660 ping_right_r,#36 661 0,#37 662 0,#38 663 0,#39 light_bumper 664 0,#40 light_bump_left 665 0,#41 light_bump_front_left 666 0,#42 light_bump_center_left 667 0,#43 light_bump_center_right 668 0,#44 light_bump_front_right 669 0,#45 light_bump_right 670 0,#46 671 0,#47 672 0,#48 673 0,#49 674 0,#50 675 0,#51 676 0)#52

Source Code for Module eddiebot_driver