barobo.mobot

17 """ 18 The Mobot Class 19 =============== 20 21 Each instance of this class can be used to represent a physical Mobot. The 22 member functions of this class can be used to get data, set motor angles, 23 scan for button events, and more. 24 25 Blocking and Non-Blocking Member Functions 26 ========================================== 27 28 The member functions of the Mobot class which are responsible for moving 29 the joints of the Mobot can be categorized into two general types of 30 functions; "blocking" functions and "non-blocking" functions. A blocking 31 function is a function that "hangs" until the complete motion is done, 32 whereas a "non-blocking" function returns as soon as the motion begins, 33 but does not wait until it is done. In the Mobot class, all functions 34 are blocking unless the have the suffix "NB", such as "Mobot.moveNB()". 35 36 For example, consider the following lines of code:: 37 mobot.move(360, 0, 0, 360) 38 angles = mobot.getJointAngles() 39 When the above lines of code are executed, the Mobot will rotate joint 1 40 360 degrees. Once the joint has rotated the full revolution, the joint 41 angles will be retrieved and stored in the variable "angles". Now 42 consider the following code:: 43 mobot.moveNB(360, 0, 0) 44 angles = mobot.getJointAngles() 45 For these lines of code, joint 1 also moves 360 degrees. The difference is 46 that with these lines of code, the joint angles will be retrieved as soon 47 as the joint begins turning, instead of waiting for the motion to finish. 48 This is because the non-blocking version of move() was used, which returns 49 immediately after the joint begins moving allowing the getJointAngles() 50 function to execute as the joint begins moving. 51 52 The L{moveWait()<barobo.mobot.Mobot.moveWait>} function can be used to 53 block until non-blocking motion functions are finished. For instance, the 54 following two blocks of code will accomplish the same task:: 55 mobot.move(360, 0, 0) 56 angles = mobot.getJointAngles() 57 58 mobot.moveNB(360, 0, 0) 59 mobot.moveWait() 60 angles = mobot.getJointAngles() 61 62 """

63 - def __init__(self):

64 self.responseQueue = Queue.Queue() 65 self.eventQueue = Queue.Queue() 66 self.readQueue = Queue.Queue() 67 self.writeQueue = Queue.Queue() 68 self.zigbeeAddr = None 69 self.serialID = None 70 self.baroboCtx = None 71 self.messageThread = threading.Thread(target=self.__messageThread) 72 self.messageThread.daemon = True 73 self.messageThread.start() 74 self.messageLock = threading.Lock() 75 self.eventThread = threading.Thread(target=self.__eventThread) 76 self.eventThread.daemon = True 77 self.eventThread.start() 78 79 self.callbackEnabled = False 80 self.jointCallbackEnabled = False 81 self.accelCallbackEnabled = False 82 self.numJoints = 4 83 self.packetSequenceNumber = 0x80

84

85 - def checkStatus(self):

86 """ 87 Check to see if the Linkbot is online. Raises an exception if the 88 Linkbot is not online. 89 """ 90 buf = bytearray([barobo.BaroboCtx.CMD_STATUS, 3, 0]) 91 self._transactMessage(buf)

92

93 - def connect(self):

94 """ 95 Connect to a Linkbot through BaroboLink 96 """ 97 # Connect to a running instance of BaroboLink 98 # First, make sure we have a BaroboCtx 99 self.zigbeeAddr = 0x8000 100 if not self.baroboCtx: 101 self.baroboCtx = barobo.BaroboCtx() 102 self.baroboCtx.connect() 103 self.baroboCtx.addLinkbot(self) 104 self.getSerialID() 105 self.form = self.getFormFactor()

106

107 - def connectBluetooth(self, bluetooth_mac_addr):

108 self.connectMobotBluetooth(bluetooth_mac_addr)

109

110 - def connectMobotBluetooth(self, bluetooth_mac_addr):

111 """ 112 Connect to a legacy bluetooth Linkbot or Mobot. 113 114 @type bluetooth_mac_addr: string 115 @param bluetooth_mac_addr: The MAC address of the bluetooth Linkbot. 116 Should be something like '00:06:66:6D:12:34' 117 """ 118 self.zigbeeAddr = 0x8000 119 if not self.baroboCtx: 120 self.baroboCtx = barobo.BaroboCtx() 121 self.baroboCtx.connectMobotBluetooth(bluetooth_mac_addr) 122 self.baroboCtx.addLinkbot(self) 123 #self.zigbeeAddr = self.baroboCtx.zigbeeAddr 124 self.checkStatus() 125 self.form = barobo.ROBOTFORM_MOBOT

126

127 - def disableButtonCallback(self):

128 """ 129 Disable the button callback. 130 131 See also: enableButtonCallback() 132 """ 133 self.callbackEnabled = False 134 buf = bytearray( 135 [barobo.BaroboCtx.CMD_ENABLEBUTTONHANDLER, 0x04, 0x00, 0x00]) 136 self._transactMessage(buf)

137

138 - def disconnect(self):

139 """ 140 Disconnect from the Linkbot. 141 """ 142 buf = bytearray([barobo.BaroboCtx.CMD_UNPAIRPARENT, 3, 0]) 143 response = self._transactMessage(buf) 144 self.baroboCtx.disconnect() 145 self.baroboCtx = None

146

147 - def driveJointTo(self, joint, angle):

148 """ 149 Drive a single joint to a position as fast as possible, using the 150 on-board PID motor controller. 151 152 @type joint: number [1,3] 153 @param joint: The joint to move 154 @type angle: number 155 @param angle: The angle to move the joint to, in degrees 156 """ 157 self.driveJointToNB(joint, angle) 158 self.moveWait()

159

160 - def driveJointToNB(self, joint, angle):

161 """ 162 Non-blocking version of driveJointTo(). Please see driveJointTo() for 163 more details. 164 """ 165 angle = _util.deg2rad(angle) 166 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORANGLEPID, 0x08, joint-1]) 167 buf += bytearray(struct.pack('<f', angle)) 168 buf += bytearray([0x00]) 169 self._transactMessage(buf)

170

171 - def driveTo(self, angle1, angle2, angle3, angle4):

172 """ 173 Drive the joints to angles as fast as possible using on-board PID 174 controller. 175 176 @type angle1: number 177 @param angle1: Position to move the joint, in degrees 178 @type angle2: number 179 @param angle2: Position to move the joint, in degrees 180 @type angle3: number 181 @param angle3: Position to move the joint, in degrees 182 """ 183 self.driveToNB(angle1, angle2, angle3, angle4) 184 self.moveWait()

185

186 - def driveToNB(self, angle1, angle2, angle3, angle4):

187 """ 188 Non-blocking version of driveTo(). See driveTo() for more details 189 """ 190 angle1 = _util.deg2rad(angle1) 191 angle2 = _util.deg2rad(angle2) 192 angle3 = _util.deg2rad(angle3) 193 angle4 = _util.deg2rad(angle4) 194 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORANGLESPID, 0x13]) 195 buf += bytearray(struct.pack('<4f', angle1, angle2, angle3, angle4)) 196 buf += bytearray([0x00]) 197 self._transactMessage(buf)

198

199 - def enableButtonCallback(self, callbackfunc, userdata=None):

200 """ 201 Enable button callbacks. This function temporarily disables the 202 robot's default button actions. Instead, whenever a button is 203 pressed or released, the function given as the parameter 'callbackfunc' 204 is called. 205 206 See also: disableButtonCallback() 207 208 @type userdata: Anything 209 @param userdata: This is data that will be passed into the callbackfunc 210 whenever it is called. 211 @type callbackfunc: function: func(buttonMask, buttonDown, userdata) . The 212 'buttonMask' parameter of the callback function will contain a bitmask 213 indicating which buttons have changed. The buttonDown parameter 214 is another bitmask, indicating the current state of each button. 215 """ 216 self.callbackfunc = callbackfunc 217 self.callbackUserData = userdata 218 self.callbackEnabled = True 219 buf = bytearray( 220 [barobo.BaroboCtx.CMD_ENABLEBUTTONHANDLER, 0x04, 0x01, 0x00]) 221 self._transactMessage(buf)

222

223 - def getFormFactor(self):

224 """ 225 Get the form factor. 226 227 @rtype: Robot Form 228 @return: Returns barobo.ROBOTFORM_MOBOT, barobo.ROBOTFORM_I, 229 barobo.ROBOTFORM_L, or barobo.ROBOTFORM_T 230 """ 231 buf = bytearray([barobo.BaroboCtx.CMD_GETFORMFACTOR, 0x03, 0x00]) 232 response = self._transactMessage(buf) 233 return response[2]

234

235 - def getJointAngle(self, joint):

236 """ 237 Get the current angle position of a joint. 238 239 @type joint: number 240 @param joint: Get the position of this joint. Can be 1, 2, or 3 241 @rtype: number 242 @return: Return the joint angle in degrees 243 """ 244 buf = bytearray( 245 [barobo.BaroboCtx.CMD_GETMOTORANGLE, 0x04, joint-1, 0x00]) 246 response = self._transactMessage(buf) 247 return _util.rad2deg(barobo._unpack('<f', response[2:6])[0])

248

249 - def getJointAngles(self):

250 """ 251 Get the current joint angles. 252 253 @rtype: [float, float, float] 254 @return: The joint angles in degrees 255 """ 256 buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORANGLESABS, 3, 0]) 257 response = self._transactMessage(buf) 258 angles = barobo._unpack('<4f', response[2:18]) 259 return list(map(_util.rad2deg, angles))

260

261 - def getJointSpeed(self, joint):

262 """ 263 Get the speed setting of a joint. Returned value is in deg/s 264 265 @rtype: float 266 @return: The joint speed in deg/sec 267 """ 268 buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORSPEED, 4, joint-1, 0]) 269 response = self._transactMessage(buf) 270 speed = barobo._unpack('<f', response[2:6])[0] 271 return _util.rad2deg(speed)

272

273 - def getJointSpeeds(self):

274 """ 275 Get a tuple of all joint speeds. See getJointSpeed() 276 277 @rtype: (float, float, float, float) 278 """ 279 return list(map(self.getJointSpeed, range(1, self.numJoints+1)))

280

281 - def getJointState(self, joint):

282 """ 283 Get the current state of a joint. Possible return values are 284 ROBOT_NEUTRAL=0, 285 ROBOT_FORWARD=1, 286 ROBOT_BACKWARD=2, 287 ROBOT_HOLD=3, 288 ROBOT_POSITIVE=4, 289 ROBOT_NEGATIVE=5 290 """ 291 buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORSTATE, 4, joint, 0x00]) 292 response = self._transactMessage(buf) 293 return response[2]

294

295 - def getJointAnglesTime(self):

296 """ 297 Get the joint angles with a timestamp. The timestamp is the number of 298 seconds since the robot has powered on. 299 300 @rtype: [numbers] 301 @return: [seconds, angle1, angle2, angle3], all angles in degrees 302 """ 303 buf = bytearray( 304 [barobo.BaroboCtx.CMD_GETMOTORANGLESTIMESTAMPABS, 0x03, 0x00]) 305 response = self._transactMessage(buf) 306 millis = barobo._unpack('<L', response[2:6])[0] 307 data = barobo._unpack('<4f', response[6:6+16]) 308 rc = [millis/1000.0] 309 rc += list(map(_util.rad2deg, data)) 310 return rc

311

312 - def getVersion(self):

313 """ 314 Get the firmware version of the Linkbot 315 316 @return: Something like (0, 0, 94) or (3, 0, 3), depending on the 317 oldness of the firmware 318 """ 319 try: 320 buf = bytearray([barobo.BaroboCtx.CMD_GETVERSIONS, 3, 0]) 321 response = self._transactMessage(buf) 322 version = barobo._unpack('!3B', response[2:5]) 323 except Exception as e: 324 buf = bytearray([barobo.BaroboCtx.CMD_GETVERSION, 3, 0]) 325 response = self._transactMessage(buf) 326 version = (0, 0, response[2]) 327 return version

328

329 - def isMoving(self):

330 for i in range(4): 331 state = self.getJointState(i) 332 if (state != barobo.ROBOT_NEUTRAL) and (state != barobo.ROBOT_HOLD): 333 return True 334 return False

335

336 - def moveJoint(self, joint, angle):

337 """ 338 Move a joint from it's current position by 'angle' degrees. 339 340 @type joint: number 341 @param joint: The joint to move: 1, 2, or 3 342 @type angle: number 343 @param angle: The number of degrees to move the joint from it's current 344 position. For example, "45" will move the joint in the positive direction 345 by 45 degrees from it's current location, and "-30" will move the joint 346 in the negative direction by 30 degrees. 347 """ 348 curangle = self.getJointAngle(joint) 349 self.moveJointTo(joint, curangle + angle)

350

351 - def moveJointNB(self, joint, angle):

352 """ 353 Non-blocking version of moveJoint(). See moveJoint() for more details. 354 """ 355 curangle = self.getJointAngle(joint) 356 self.moveJointToNB(joint, curangle + angle)

357

358 - def moveJointTo(self, joint, angle):

359 """ 360 Move a joint to an angle. 361 362 @type joint: number 363 @param joint: The joint to move: 1, 2, or 3 364 @type angle: number 365 @param angle: The absolute position you want the joint to move to. 366 Values are in degrees and can be any value. For example, the value 367 "720" means two full rotations in the positive directions past "0". 368 """ 369 self.moveJointToNB(joint, angle) 370 self.moveWait()

371

372 - def moveJointToNB(self, joint, angle):

373 """ 374 Non-blocking version of moveJointTo. See moveJointTo for more details. 375 """ 376 angle = _util.deg2rad(angle) 377 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORANGLEABS, 0x08, joint-1]) 378 buf += bytearray(struct.pack('<f', angle)) 379 buf += bytearray([0x00]) 380 self._transactMessage(buf)

381

382 - def move(self, angle1, angle2, angle3, angle4):

383 """ 384 Move all of the joints on a robot by a number of degrees. 385 386 @type angle1: number 387 @param angle1: Number of degrees to move joint 1. Similar for parameters 388 'angle2' and 'angle3'. 389 """ 390 self.moveNB(angle1, angle2, angle3, angle4) 391 self.moveWait()

392

393 - def moveNB(self, angle1, angle2, angle3, angle4):

394 """ 395 Non-blocking version of move(). See move() for more details 396 """ 397 angle1 = _util.deg2rad(angle1) 398 angle2 = _util.deg2rad(angle2) 399 angle3 = _util.deg2rad(angle3) 400 angle4 = _util.deg2rad(angle4) 401 buf = bytearray([barobo.BaroboCtx.CMD_MOVE_MOTORS, 0x13]) 402 buf += bytearray(struct.pack('<4f', angle1, angle2, angle3, angle4)) 403 buf += bytearray([0x00]) 404 self._transactMessage(buf)

405

406 - def moveContinuous(self, dir1, dir2, dir3, dir4):

407 """ 408 Make the joints begin moving continuously. 409 410 @type dir1: Barobo Direction Macro 411 @param dir1: This parameter may take the following values: 412 - ROBOT_NEUTRAL: Makes the joint relax 413 - ROBOT_FORWARD: Rotates the joint to move the robot in the "forward" 414 direction, if the robot has wheels. 415 - ROBOT_BACKWARD: Same as above but backwards 416 - ROBOT_HOLD: Hold the joint at its current position 417 - ROBOT_POSITIVE: Rotates the joint in the "positive" direction, 418 according to the right-hand-rule. 419 - ROBOT_NEGATIVE: Same as above but in the negative direction. 420 """ 421 Mobot.setMovementState(self, dir1, dir2, dir3, dir4)

422

423 - def moveTo(self, angle1, angle2, angle3, angle4):

424 self.moveToNB(angle1, angle2, angle3, angle4) 425 self.moveWait()

426

427 - def moveToNB(self, angle1, angle2, angle3, angle4):

428 """ 429 Move all joints on the Linkbot. Linkbot-I modules will ignore the 430 'angle2' parameter and Linkbot-L modules will ignore the 'angle3' 431 parameter. 432 433 This function is the non-blocking version of moveTo(), meaning this 434 function will return immediately after the robot has begun moving and 435 will not wait until the motion is finished. 436 437 @type angle1: number 438 @param angle1: Position to move the joint, in degrees 439 @type angle2: number 440 @param angle2: Position to move the joint, in degrees 441 @type angle3: number 442 @param angle3: Position to move the joint, in degrees 443 """ 444 angle1 = _util.deg2rad(angle1) 445 angle2 = _util.deg2rad(angle2) 446 angle3 = _util.deg2rad(angle3) 447 angle4 = _util.deg2rad(angle4) 448 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORANGLESABS, 0x13]) 449 buf += bytearray(struct.pack('<4f', angle1, angle2, angle3, 0.0)) 450 buf += bytearray([0x00]) 451 self._transactMessage(buf)

452

453 - def moveWait(self):

454 """ 455 Wait until the current robotic motion is finished. 456 """ 457 while self.isMoving(): 458 time.sleep(0.1)

459

460 - def recordAnglesBegin(self, delay=0.05):

461 """ 462 Begin recording joint angles. 463 464 @type delay: number 465 @param delay: The number of seconds to delay between joint angles 466 readings. 467 """ 468 self.recordThread = _MobotRecordThread(self, delay) 469 self.recordThread.start()

470

471 - def recordAnglesEnd(self):

472 """ End recording angles and return a list consisting of [time_values, 473 joint1angles, joint2angles, joint3angles]""" 474 self.recordThread.runflag_lock.acquire() 475 self.recordThread.runflag = False 476 self.recordThread.runflag_lock.release() 477 # Wait for recording to end 478 while self.recordThread.isRunning: 479 time.sleep(0.5) 480 return [map( lambda x: x-self.recordThread.time[0], 481 self.recordThread.time), 482 self.recordThread.angles[0], 483 self.recordThread.angles[1], 484 self.recordThread.angles[2], 485 self.recordThread.angles[3]]

486

487 - def recordAnglesPlot(self):

488 import pylab 489 """Plot recorded angles. 490 491 See recordAnglesBegin() and recordAnglesEnd() to record joint motions. 492 """ 493 pylab.plot( 494 self.recordThread.time, 495 self.recordThread.angles[0], 496 self.recordThread.time, 497 self.recordThread.angles[1], 498 self.recordThread.time, 499 self.recordThread.angles[2], 500 self.recordThread.time, 501 self.recordThread.angles[3]) 502 pylab.show()

503

504 - def reset(self):

505 """ 506 Reset the multi-revolution counter on the joints. 507 """ 508 buf = bytearray([barobo.BaroboCtx.CMD_RESETABSCOUNTER, 0x03, 0x00]) 509 self._transactMessage(buf)

510

511 - def resetToZero(self):

512 """ 513 Reset the multi-revolution counter and move all the joints to zero 514 positions. 515 """ 516 self.reset() 517 self.moveTo(0, 0, 0)

518

519 - def resetToZeroNB(self):

520 self.reset() 521 self.moveToZeroNB()

522

523 - def setJointMovementState(self, joint, state):

524 """ 525 Set a joint movement state 526 527 @type state: Barobo Direction Macro 528 @param state: This parameter may take the following values: 529 - ROBOT_NEUTRAL: Makes the joint relax 530 - ROBOT_FORWARD: Rotates the joint to move the robot in the "forward" 531 direction, if the robot has wheels. 532 - ROBOT_BACKWARD: Same as above but backwards 533 - ROBOT_HOLD: Hold the joint at its current position 534 - ROBOT_POSITIVE: Rotates the joint in the "positive" direction, 535 according to the right-hand-rule. 536 - ROBOT_NEGATIVE: Same as above but in the negative direction. 537 """ 538 if (self.form == ROBOTFORM_I) and (joint==3): 539 if state == ROBOT_FORWARD: 540 state = ROBOT_BACKWARD 541 elif state == ROBOT_BACKWARD: 542 state = ROBOT_FORWARD 543 elif state == ROBOT_POSITIVE: 544 state = ROBOT_FORWARD 545 elif state == ROBOT_NEGATIVE: 546 state = ROBOT_BACKWARD 547 buf = bytearray( 548 [barobo.BaroboCtx.CMD_SETMOTORDIR, 0x05, joint-1, state, 0x00]) 549 self._transactMessage(buf)

550

551 - def setJointSpeed(self, joint, speed):

552 """ 553 Set the speed of a joint. 554 555 @type joint: number 556 @param joint: The joint to change the speed. Can be 1, 2, or 3 557 @type speed: number 558 @param speed: The speed to set the joint to, in degrees/second. 559 """ 560 _speed = _util.deg2rad(speed) 561 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORSPEED, 0x08, joint-1]) 562 buf += bytearray(struct.pack('<f', _speed)) 563 buf += bytearray([0x00]) 564 self._transactMessage(buf)

565

566 - def setJointSpeeds(self, speed1, speed2, speed3, speed4):

567 """ 568 Set all three motor speed simultaneously. Parameters in degrees/second. 569 """ 570 self.setJointSpeed(1, speed1) 571 self.setJointSpeed(2, speed2) 572 self.setJointSpeed(3, speed3) 573 self.setJointSpeed(4, speed4)

574

575 - def setJointState(self, joint, state):

576 """ 577 Set a joint's movement state. 578 579 @param joint: The joint id: 1, 2, or 3 580 @type state: Barobo Direction Macro 581 @param state: This parameter may take the following values: 582 - ROBOT_NEUTRAL: Makes the joint relax 583 - ROBOT_FORWARD: Rotates the joint to move the robot in the "forward" 584 direction, if the robot has wheels. 585 - ROBOT_BACKWARD: Same as above but backwards 586 - ROBOT_HOLD: Hold the joint at its current position 587 - ROBOT_POSITIVE: Rotates the joint in the "positive" direction, 588 according to the right-hand-rule. 589 - ROBOT_NEGATIVE: Same as above but in the negative direction. 590 """ 591 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORDIR, 5, joint-1, state, 0]) 592 self._transactMessage(buf)

593

594 - def setJointStates(self, states, speeds):

595 """ 596 Set the joint states for all 3 joints simultaneously. 597 598 @type states: [state1, state2...] 599 @param states: Each state may take the following values: 600 - ROBOT_NEUTRAL: Makes the joint relax 601 - ROBOT_FORWARD: Rotates the joint to move the robot in the "forward" 602 direction, if the robot has wheels. 603 - ROBOT_BACKWARD: Same as above but backwards 604 - ROBOT_HOLD: Hold the joint at its current position 605 - ROBOT_POSITIVE: Rotates the joint in the "positive" direction, 606 according to the right-hand-rule. 607 - ROBOT_NEGATIVE: Same as above but in the negative direction. 608 @type speeds: [speed1, speed2 ...] 609 @param speeds: The speeds to set each joint 610 611 """ 612 if len(states) < 4: 613 states += [0]*(4-len(states)) 614 if len(speeds) < 4: 615 speeds += [0.0]*(4-len(speeds)) 616 speeds = list(map(_util.deg2rad, speeds)) 617 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORSTATES, 23]) 618 buf += bytearray(states) 619 buf += bytearray(struct.pack('<4f', 620 speeds[0], 621 speeds[1], 622 speeds[2], 623 speeds[3])) 624 buf += bytearray([0x00]) 625 self._transactMessage(buf)

626

627 - def setMotorPower(self, joint, power):

628 """ 629 Set the power of a motor. 630 631 @type joint: number 632 @param joint: The joint to control. Can be 1, 2, or 3 633 @type power: integer 634 @param power: An integer between -255 and 255. 635 """ 636 joint = joint-1 637 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORPOWER, 0x0A, 1<<joint]) 638 buf +=bytearray(struct.pack('>3h', power, power, power)) 639 buf +=bytearray([0x00]) 640 self._transactMessage(buf)

641

642 - def setMotorPowers(self, power1, power2, power3, power4):

643 buf = bytearray([barobo.BaroboCtx.CMD_SETMOTORPOWER, 0x0A, 0x07]) 644 buf +=bytearray(struct.pack('>4h', power1, power2, power3, power4)) 645 buf +=bytearray([0x00]) 646 self._transactMessage(buf)

647

648 - def setMovementState(self, state1, state2, state3, state4, time=-1):

649 """ 650 Set the movement state for all three motors. 651 652 @type state1: movement_state 653 @param state1: This parameter may take the following values: 654 - ROBOT_NEUTRAL: Makes the joint relax 655 - ROBOT_FORWARD: Rotates the joint to move the robot in the "forward" 656 direction, if the robot has wheels. 657 - ROBOT_BACKWARD: Same as above but backwards 658 - ROBOT_HOLD: Hold the joint at its current position 659 - ROBOT_POSITIVE: Rotates the joint in the "positive" direction, 660 according to the right-hand-rule. 661 - ROBOT_NEGATIVE: Same as above but in the negative direction. 662 """ 663 if self.form == barobo.ROBOTFORM_I: 664 if state3 == barobo.ROBOT_FORWARD: 665 state3 = barobo.ROBOT_BACKWARD 666 elif state3 == barobo.ROBOT_BACKWARD: 667 state3 = barobo.ROBOT_FORWARD 668 elif state3 == barobo.ROBOT_POSITIVE: 669 state3 = barobo.ROBOT_FORWARD 670 elif state3 == barobo.ROBOT_NEGATIVE: 671 state3 = barobo.ROBOT_BACKWARD 672 states = [state1, state2, state3, state4] 673 if time > 0: 674 time = time * 1000 675 buf = bytearray([barobo.BaroboCtx.CMD_TIMEDACTION, 0, 0x07]) 676 for state in states: 677 buf += bytearray([state, barobo.ROBOT_HOLD]) 678 buf += bytearray(struct.pack('<i', time)) 679 buf += bytearray([0x00]) 680 buf[1] = len(buf) 681 self._transactMessage(buf)

682

683 - def stop(self):

684 buf = bytearray([barobo.BaroboCtx.CMD_STOP, 0x03, 0x00]) 685 self._transactMessage(buf)

686

687 - def _transactMessage(self, buf, maxTries = 3, timeout = 10.0):

688 self.messageLock.acquire() 689 numTries = 0 690 response = [0xff] 691 buf[-1] = self.packetSequenceNumber 692 # Flush the responseQueue 693 while not self.responseQueue.empty(): 694 self.responseQueue.get(block=False) 695 self.baroboCtx.writePacket(_comms.Packet(buf, self.zigbeeAddr)) 696 try: 697 while True: 698 response = self.responseQueue.get(block=True, timeout = timeout) 699 if response[response[1]-1] == self.packetSequenceNumber: 700 break 701 elif response[response[1]-1] == 0x11: 702 break 703 else: 704 print('Rejected packet; sequence number incorrect. ' 705 '{0} != {1}'.format(response[-2], 706 self.packetSequenceNumber)) 707 except Queue.Empty: 708 self.messageLock.release() 709 self.packetSequenceNumber += 1 710 self.packetSequenceNumber %= 0xff 711 if self.packetSequenceNumber == 0: 712 self.packetSequenceNumber = 0x80 713 raise barobo.BaroboException('Did not receive response from robot.') 714 self.packetSequenceNumber += 1 715 self.packetSequenceNumber %= 0xff 716 if self.packetSequenceNumber == 0: 717 self.packetSequenceNumber = 0x80 718 if response[0] != barobo.BaroboCtx.RESP_OK: 719 self.messageLock.release() 720 raise barobo.BaroboException('Robot returned error status.') 721 self.messageLock.release() 722 return response

723

724 - def __messageThread(self):

725 # Scan and act on incoming messages 726 while True: 727 pkt = self.readQueue.get(block=True, timeout=None) 728 if (pkt[0] == barobo.BaroboCtx.RESP_OK) or \ 729 (pkt[0] == barobo.BaroboCtx.RESP_ERR) or \ 730 (pkt[0] == barobo.BaroboCtx.RESP_ALREADY_PAIRED): 731 self.responseQueue.put(pkt) 732 else: 733 self.eventQueue.put(pkt)

734

735 - def __eventThread(self):

736 while True: 737 evt = self.eventQueue.get(block=True, timeout=None) 738 if (evt[0] == barobo.BaroboCtx.EVENT_BUTTON) \ 739 and self.callbackEnabled: 740 self.callbackfunc(evt[6], evt[7], self.callbackUserData) 741 elif (evt[0] == barobo.BaroboCtx.EVENT_JOINT_MOVED) \ 742 and self.jointCallbackEnabled: 743 values = barobo._unpack('<Lfff', evt[2:18]) 744 self.jointcallbackfunc( 745 values[0], 746 values[1]*180.0/math.pi, 747 values[2]*180.0/math.pi, 748 values[3]*180.0/math.pi, 749 evt[22]) 750 elif (evt[0] == barobo.BaroboCtx.EVENT_ACCEL_CHANGED) \ 751 and self.accelCallbackEnabled: 752 values = barobo._unpack('<L', evt[2:6]) + \ 753 barobo._unpack('>3h', evt[6:12]) 754 self.accelcallbackfunc( 755 values[0], 756 values[1]/16384.0, 757 values[2]/16384.0, 758 values[3]/16384.0) 759 elif evt[0] == barobo.BaroboCtx.EVENT_DEBUG_MSG: 760 s = barobo._unpack('s', evt[2:-1]) 761 print ("Debug msg from {0}: {1}".format(self.serialID, s))

Source Code for Module barobo.mobot