barobo.linkbot

18 """ 19 The Linkbot Class 20 ================= 21 22 Each instance of this class can be used to represent a physical Linkbot. The 23 member functions of this class can be used to get data, set motor angles, 24 beep the buzzer, scan for button events, and more. 25 26 Blocking and Non-Blocking Member Functions 27 ========================================== 28 29 The member functions of the Linkbot class which are responsible for moving 30 the joints of the Linkbot can be categorized into two general types of 31 functions; "blocking" functions and "non-blocking" functions. A blocking 32 function is a function that "hangs" until the complete motion is done, 33 whereas a "non-blocking" function returns as soon as the motion begins, 34 but does not wait until it is done. In the Linkbot class, all functions 35 are blocking unless the have the suffix "NB", such as "Linkbot.moveNB()". 36 37 For example, consider the following lines of code:: 38 linkbot.move(360, 0, 0) 39 linkbot.setBuzzerFrequency(440) 40 When the above lines of code are executed, the Linkbot will rotate joint 1 41 360 degrees. Once the joint has rotated the full revolution, the buzzer will 42 sound. Now consider the following code:: 43 linkbot.moveNB(360, 0, 0) 44 linkbot.setBuzzerFrequency(440) 45 For these lines of code, joint 1 also moves 360 degrees. The difference is 46 that with these lines of code, the buzzer will begin emitting a tone 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 setBuzzerFrequency() 50 function to execute as the joint begins moving. 51 52 The L{moveWait()<barobo.linkbot.Linkbot.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 linkbot.move(360, 0, 0) 56 linkbot.setBuzzerFrequency(440) 57 58 linkbot.moveNB(360, 0, 0) 59 linkbot.moveWait() 60 linkbot.setBuzzerFrequency(440) 61 62 """

63 - def __init__(self, *args, **kwargs):

64 mobot.Mobot.__init__(self, *args, **kwargs) 65 self.numJoints = 3

66

67 - def connectBluetooth(self, bluetooth_mac_addr):

68 """ 69 Connect to a bluetooth enabled Linkbot. Note that you must have a 70 bluetooth enabled LinkPod connected to your Linkbot for this function to 71 work. Most Linkbots do not come with BlueTooth, and instead use a ZigBee 72 like protocol to communicate with each other. 73 74 @type bluetooth_mac_addr: string 75 @param bluetooth_mac_addr: The MAC address of the bluetooth Linkbot. 76 Should be something like '00:06:66:6D:12:34' 77 """ 78 self.zigbeeAddr = 0x0000 79 if not self.baroboCtx: 80 self.baroboCtx = barobo.BaroboCtx() 81 self.baroboCtx.connectBluetooth(bluetooth_mac_addr) 82 self.baroboCtx.addLinkbot(self) 83 self.zigbeeAddr = self.baroboCtx.zigbeeAddr 84 self.checkStatus() 85 self.getSerialID() 86 self.form = self.getFormFactor()

87

88 - def driveTo(self, angle1, angle2, angle3):

89 self.driveToNB(angle1, angle2, angle3) 90 self.moveWait()

91

92 - def driveToNB(self, angle1, angle2, angle3):

93 mobot.Mobot.driveToNB(self, angle1, angle2, angle3, 0)

94

95 - def disableAccelEventCallback(self):

96 buf = bytearray( 97 [barobo.BaroboCtx.CMD_SET_ENABLE_ACCEL_EVENT, 4, 0x00, 0x00]) 98 self._transactMessage(buf) 99 self.accelCallbackEnabled = False

100

101 - def disableJointEventCallback(self):

102 buf = bytearray( 103 [barobo.BaroboCtx.CMD_SET_ENABLE_JOINT_EVENT, 4, 0x00, 0x00]) 104 self._transactMessage(buf) 105 self.jointCallbackEnabled = False

106

107 - def enableJointEventCallback(self, cb):

108 """ 109 Enable the joint event callback. Whenever any joint on the robot moves, 110 the callback function 'cb' will be called. The callback function should 111 be of the form cb(millis_timestamp, j1, j2, j3, mask) where 112 'millis_timestamp' will contain a timestamp from the robot, and j1, j2, 113 and j3 will contain the new angles of all of the joints in degrees, and 114 mask will be a mask of all of the joints that triggered the event. 115 """ 116 buf = bytearray( 117 [barobo.BaroboCtx.CMD_SET_ENABLE_JOINT_EVENT, 4, 0x07, 0x00]) 118 self._transactMessage(buf) 119 self.jointcallbackfunc = cb 120 self.jointCallbackEnabled = True

121

122 - def enableAccelEventCallback(self, cb):

123 """ 124 Enable the acceleration event callback. Whenever a change in 125 acceleration happens on the robot, the callback function will be called. 126 """ 127 buf = bytearray( 128 [barobo.BaroboCtx.CMD_SET_ENABLE_ACCEL_EVENT, 4, 0x07, 0x00]) 129 self._transactMessage(buf) 130 self.accelcallbackfunc = cb 131 self.accelCallbackEnabled = True

132

133 - def _getADCVolts(self, adc):

134 buf = bytearray([barobo.BaroboCtx.CMD_GETENCODERVOLTAGE, 4, adc, 0]) 135 response = self._transactMessage(buf) 136 voltage = barobo._unpack('<f', response[2:6])[0] 137 return voltage

138

139 - def getAccelerometerData(self):

140 """ 141 Get the current accelerometer readings 142 143 @rtype: [number, number, number] 144 @return: A list of acceleration values in the x, y, and z directions. 145 Accelerometer values are in units of "G's", where 1 G is standard 146 earth gravitational acceleration (9.8m/s/s) 147 """ 148 buf = bytearray([barobo.BaroboCtx.CMD_GETACCEL, 0x03, 0x00]) 149 response = self._transactMessage(buf) 150 values = barobo._unpack('<3h', response[2:8]) 151 return list(map(lambda x: x/16384.0, values))

152

153 - def getBatteryVoltage(self):

154 """ 155 Get the current battery voltage of the Linkbot. 156 157 @rtype: number 158 @return: Returns a value in Volts 159 """ 160 buf = bytearray([barobo.BaroboCtx.CMD_GETBATTERYVOLTAGE, 0x03, 0x00]) 161 response = self._transactMessage(buf) 162 voltage = barobo._unpack('<f', response[2:6])[0] 163 return voltage

164

165 - def getBreakoutADC(self, adc):

166 """ 167 Get the ADC (Analog-to-digital) reading from a breakout-board's ADC 168 channel. 169 170 @type adc: number 171 @param adc: The ADC channel to get the value from [0, 7] 172 @rtype: number 173 @return: Value from 0-1023 174 """ 175 buf = bytearray( 176 [barobo.BaroboCtx.TWIMSG_HEADER, 177 barobo.BaroboCtx.TWIMSG_ANALOGREADPIN, 178 adc]) 179 data = self.twiSendRecv(0x02, buf, 2) 180 return barobo._unpack('!h', data[0:2])[0]

181

182 - def getBreakoutADCVolts(self, adc):

183 """ 184 Get the voltage reading of an ADC pin. 185 186 Note that the voltage is calibrated using the AVCC reference. If the 187 reference is changed using the setBreakoutADCReference() function, the 188 values reported by this function may no longer be accurate. 189 190 @type adc: number 191 @param adc: The ADC channel to get the value from [0, 7] 192 @rtype: number 193 @return: Floating point voltage from 0 to 5 194 """ 195 return self.getBreakoutADC(adc)/1024.0 * 5

196

197 - def getBreakoutDigitalPin(self, pin):

198 """ 199 Read value from digital I/O pin. 200 201 @rtype: integer 202 @return: Returns 0 if pin is grounded, or 1 in pin is high. 203 """ 204 buf = bytearray( 205 [barobo.BaroboCtx.TWIMSG_HEADER, 206 barobo.BaroboCtx.TWIMSG_DIGITALREADPIN, 207 pin]) 208 data = self.twiSendRecv(0x02, buf, 1) 209 return data[0]

210

211 - def getColorRGB(self):

212 """ 213 Get the current RGB values of the rgbled 214 215 @rtype: [number, number, number] 216 @return: The red, green, and blue values from 0 to 255 217 """ 218 buf = bytearray([barobo.BaroboCtx.CMD_GETRGB, 0x03, 0x00]) 219 response = self._transactMessage(buf) 220 return barobo._unpack('<3B', response[2:5])

221

222 - def getHWRev(self):

223 """ 224 Get the hardware revision of the Linkbot 225 226 @rtype: [major, minor, micro] 227 @return: The major, minor, and micro version numbers 228 """ 229 buf = bytearray([barobo.BaroboCtx.CMD_GET_HW_REV, 0x03, 0x00]) 230 response = self._transactMessage(buf) 231 return barobo._unpack('<3B', response[2:5])

232

233 - def getLinkbot(self, addr):

234 """ 235 Use an instance of a Linkbot to get instances to other Linkbots. Note 236 that this only works for Linkbots that are connected via Bluetooth or 237 TTY, but does not work for Linkbots that are connected to BaroboLink. 238 """ 239 return self.baroboCtx.getLinkbot(addr)

240

241 - def getSerialID(self):

242 """ 243 Get the serial ID from the Linkbot 244 245 @return: A four character string 246 """ 247 buf = bytearray([barobo.BaroboCtx.CMD_GETSERIALID, 3, 0]) 248 response = self._transactMessage(buf) 249 botid = barobo._unpack('!4s', response[2:6])[0] 250 self.serialID = botid 251 return botid

252

253 - def isMoving(self):

254 buf = bytearray([barobo.BaroboCtx.CMD_IS_MOVING, 3, 0]) 255 response = self._transactMessage(buf) 256 return response[2]

257

258 - def move(self, angle1, angle2, angle3):

259 self.moveNB(angle1, angle2, angle3) 260 self.moveWait()

261

262 - def moveNB(self, angle1, angle2, angle3):

263 angles = self.getJointAngles() 264 self.moveToNB(angle1+angles[0], angle2+angles[1], angle3+angles[2])

265

266 - def moveContinuous(self, dir1, dir2, dir3):

267 mobot.Mobot.moveContinuous(self, dir1, dir2, dir3, 0)

268

269 - def moveTo(self, angle1, angle2, angle3):

270 self.moveToNB(angle1, angle2, angle3) 271 self.moveWait()

272

273 - def moveToNB(self, angle1, angle2, angle3):

274 mobot.Mobot.moveToNB(self, angle1, angle2, angle3, 0)

275

276 - def _pairParent(self):

277 buf = bytearray([barobo.BaroboCtx.CMD_PAIRPARENT, 5]) 278 buf += bytearray(struct.pack('!H', self.baroboCtx.zigbeeAddr)) 279 buf += bytearray([0x00]) 280 self._transactMessage(buf)

281

282 - def ping(self, numbytes=4, sendbytes=[]):

283 import random 284 now = time.time() 285 buf = bytearray([barobo.BaroboCtx.CMD_PING, 0]) 286 if len(sendbytes) != numbytes: 287 randbytes = bytearray( 288 [random.randint(0, 255) for _ in range(numbytes)]) 289 else: 290 randbytes = sendbytes 291 buf += randbytes 292 buf += bytearray([0x00]) 293 buf[1] = len(buf) 294 response = self._transactMessage(buf, maxTries = 1) 295 if response[2:2+numbytes] != randbytes: 296 raise barobo.BaroboException( 297 'Ping did not receive correct bytes. ' 298 'Expected {0}, got {1}'.format( 299 list(map(hex, randbytes)), 300 list(map(hex, response)) )) 301 return time.time() - now

302

303 - def reboot(self):

304 """ 305 Reboot the connect robot. Note that communications with the robot will 306 not succeed while the robot is booting back up. 307 """ 308 buf = bytearray([barobo.BaroboCtx.CMD_REBOOT, 0x03, 0x00]) 309 self._transactMessage(buf)

310

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

312 """ 313 Begin recording joint angles. 314 315 @type delay: number 316 @param delay: The number of seconds to delay between joint angles 317 readings. 318 """ 319 self.recordThread = _LinkbotRecordThread(self, delay) 320 self.recordThread.start()

321

322 - def recordAnglesEnd(self):

323 """ End recording angles and return a list consisting of [time_values, 324 joint1angles, joint2angles, joint3angles]""" 325 self.recordThread.runflag_lock.acquire() 326 self.recordThread.runflag = False 327 self.recordThread.runflag_lock.release() 328 # Wait for recording to end 329 while self.recordThread.isRunning: 330 time.sleep(0.5) 331 return [map(lambda x: x-self.recordThread.time[0], 332 self.recordThread.time), 333 self.recordThread.angles[0], 334 self.recordThread.angles[1], 335 self.recordThread.angles[2]]

336

337 - def recordAnglesPlot(self):

338 import pylab 339 """Plot recorded angles. 340 341 See recordAnglesBegin() and recordAnglesEnd() to record joint motions. 342 """ 343 pylab.plot( 344 self.recordThread.time, 345 self.recordThread.angles[0], 346 self.recordThread.time, 347 self.recordThread.angles[1], 348 self.recordThread.time, 349 self.recordThread.angles[2]) 350 pylab.show()

351

352 - def setAcceleration(self, accel):

353 """ 354 Set the acceleration of all joints. Each joint motion will begin with 355 this acceleration after calling this function. Set the acceleration to 0 356 to disable this feature. 357 """ 358 buf = bytearray([barobo.BaroboCtx.CMD_SETGLOBALACCEL, 0]) 359 buf += struct.pack('<f', _util.deg2rad(accel)) 360 buf += bytearray([0x00]) 361 buf[1] = len(buf) 362 self._transactMessage(buf)

363

364 - def startJointAcceleration(self, joint, accel, timeout=0):

365 """ 366 Make a robot's joint start accelerating forwards (positive accel value) 367 or backwards. 368 369 @type joint: number 370 @type accel: float, degrees/s/s 371 @type timeout: float, number of seconds. After this number of seconds, 372 accel for the joint will be set to 0. 373 """ 374 buf = bytearray([barobo.BaroboCtx.CMD_SET_ACCEL, 0, joint-1]) 375 buf += struct.pack('<f', _util.deg2rad(accel)) 376 buf += bytearray(struct.pack('<i', timeout)) 377 buf += bytearray([0x00]) 378 buf[1] = len(buf) 379 self._transactMessage(buf)

380

381 - def setBreakoutAnalogPin(self, pin, value):

382 """ 383 Set an analog output pin (PWM) to a value between 0-255. This can be 384 used to set the power of a motor, dim a LED, or more. 385 386 @type pin: integer from 0-7 387 @param pin: The pin parameter must be a pin the supports analog output. 388 These pins are indicated by a tilde (~) symbol next to the pin 389 number printed on the breakout board. 390 @type value: integer from 0-255 391 @param value: The amount to power the pin: 0 is equivalent to no power, 392 255 is maximum power. 393 """ 394 buf = bytearray( 395 [barobo.BaroboCtx.TWIMSG_HEADER, 396 barobo.BaroboCtx.TWIMSG_ANALOGWRITEPIN, 397 pin, 398 value]) 399 self.twiSend(0x02, buf)

400

401 - def setBreakoutAnalogRef(self, ref):

402 """ 403 Set the reference voltage of an analog input pin. 404 405 @type ref: integer from 0-7 406 @param ref: Valid values are barobo.AREF_DEFAULT, barobo.AREF_INTERNAL, 407 barobo.AREF_INTERNAL1V1, barobo.AREF_INTERNAL2V56, and 408 barobo.AREF_EXTERNAL. 409 """ 410 buf = bytearray( 411 [barobo.BaroboCtx.TWIMSG_HEADER, 412 barobo.BaroboCtx.TWIMSG_ANALOGREF, 413 ref]) 414 self.twiSend(0x02, buf)

415

416 - def setBreakoutDigitalPin(self, pin, value):

417 """ 418 Set a digital I/O pin to either a high or low value. The pin will be set 419 high if the parameter 'value' is non-zero, or set to ground otherwise. 420 """ 421 buf = bytearray( 422 [barobo.BaroboCtx.TWIMSG_HEADER, 423 barobo.BaroboCtx.TWIMSG_DIGITALWRITEPIN, 424 pin, 425 value]) 426 self.twiSend(0x02, buf)

427

428 - def setBreakoutPinMode(self, pin, mode):

429 """ 430 Set the mode of a digital I/O pin on the breakout board. Valid modes are 431 barobo.PINMODE_INPUT, barobo.PINMODE_OUTPUT, and 432 barobo.PINMODE_INPUTPULLUP. 433 """ 434 buf = bytearray( 435 [barobo.BaroboCtx.TWIMSG_HEADER, 436 barobo.BaroboCtx.TWIMSG_SETPINMODE, 437 pin, 438 mode]) 439 self.twiSend(0x02, buf)

440

441 - def setBuzzerFrequency(self, freq):

442 """ 443 Set the buzzer to begin playing a tone. 444 445 @type freq: number in Hz 446 @param freq: The frequency in Hertz (Hz) for the buzzer to play. Set to 447 zero to turn the buzzer off. 448 """ 449 buf = bytearray([0x6A, 0x05, (int(freq)>>8)&0xff, int(freq)&0xff, 0x00]) 450 self._transactMessage(buf)

451

452 - def setAccelEventThreshold(self, g_force):

453 """ 454 Set the minimum change in g-force required for the robot to report an 455 acceleration event. 456 457 See also: enableAccelEventCallback() 458 459 @type g_force: floating point number 460 @param g_force: The acceleration in standard earth gravity "G's". 461 """ 462 assert g_force >= 0 463 buf = bytearray([barobo.BaroboCtx.CMD_SET_ACCEL_EVENT_THRESHOLD, 464 0]) 465 buf += bytearray(struct.pack('!H', g_force * 16384)) 466 buf += bytearray([0x0]) 467 self._transactMessage(buf)

468

469 - def setJointEventThreshold(self, joint, angle):

470 """ 471 Set the minimum amount the joint must move before a joint motion event 472 is reported. 473 474 Also see enableJointEventCallback() 475 476 @type angle: floating point number 477 @param angle: An angle in degrees. 478 """ 479 assert angle >= 0 480 buf = bytearray([barobo.BaroboCtx.CMD_SET_JOINT_EVENT_THRESHOLD, 481 0, joint-1]) 482 buf += bytearray(struct.pack('<f', angle*math.pi/180.0)) 483 buf += bytearray([0x0]) 484 buf[1] = len(buf) 485 self._transactMessage(buf)

486

487 - def setHWRev(self, major, minor, micro):

488 """ 489 Set the HW revision of the Linkbot. 490 """ 491 buf = bytearray( 492 [barobo.BaroboCtx.CMD_SET_HW_REV, 0, major, minor, micro, 0x00]) 493 buf[1] = len(buf) 494 self._transactMessage(buf)

495

496 - def setJointSpeeds(self, speed1, speed2, speed3):

497 self.setJointSpeed(1, speed1) 498 self.setJointSpeed(2, speed2) 499 self.setJointSpeed(3, speed3)

500

501 - def setLEDColor(self, r, g, b):

502 """ 503 Set the LED color 504 505 @type r: number [0, 255] 506 @type g: number [0, 255] 507 @type b: number [0, 255] 508 """ 509 510 buf = bytearray( 511 [barobo.BaroboCtx.CMD_RGBLED, 512 9, 513 0xff, 514 0xff, 515 0xff, 516 r, g, b, 517 0x00]) 518 self._transactMessage(buf)

519

520 - def setMotorPowers(self, power1, power2, power3):

521 mobot.Mobot.setMotorPowers(self, power1, power2, power3, 0)

522

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

524 mobot.Mobot.setMovementState(self, state1, state2, state3, 0, time)

525

526 - def smoothMoveTo(self, joint, accel0, accelf, vmax, angle):

527 """ 528 See: smoothMoveToNB() 529 """ 530 self.smoothMoveToNB(joint, accel0, accelf, vmax, angle) 531 self.moveWait()

532

533 - def smoothMoveToNB(self, joint, accel0, accelf, vmax, angle):

534 """ 535 Move a joint to a desired position with a specified amount of starting 536 and stopping acceleration. 537 538 @type joint: number 539 @param joint: The joint to move 540 @type accel0: number 541 @param accel0: The starting acceleration, in deg/sec/sec 542 @type accelf: number 543 @param accelf: The stopping deceleration, in deg/sec/sec 544 @type vmax: number 545 @param vmax: The maximum velocity for the joint during the motion, in 546 deg/sec 547 @type angle: number 548 @param angle: The absolute angle to move the joint to, in degrees 549 """ 550 _accel0 = _util.deg2rad(accel0) 551 _accelf = _util.deg2rad(accelf) 552 _vmax = _util.deg2rad(vmax) 553 _angle = _util.deg2rad(angle) 554 buf = bytearray([barobo.BaroboCtx.CMD_SMOOTHMOVE, 20, joint-1]) 555 buf += bytearray(struct.pack('<4f', _accel0, _accelf, _vmax, _angle)) 556 buf += bytearray([0x00]) 557 buf[1] = len(buf) 558 self._transactMessage(buf)

559

560 - def _setSerialID(self, text):

561 buf = bytearray([barobo.BaroboCtx.CMD_SETSERIALID, 0]) 562 buf += bytearray(text) 563 buf += bytearray([0x00]) 564 buf[1] = len(buf) 565 self._transactMessage(buf)

566

567 - def twiRecv(self, addr, size):

568 """ 569 Receive data from a TWI device. See twiSend() for more details. 570 571 @param addr: The TWI address to send data to. 572 @rtype: bytearray 573 """ 574 twibuf = bytearray(data) 575 buf = bytearray( 576 [barobo.BaroboCtx.CMD_TWI_SEND, len(data)+5, addr, len(data)]) 577 buf += bytearray(data) 578 buf += bytearray([0x00]) 579 response = self._transactMessage(buf) 580 return bytearray(response[2:-1])

581

582 - def twiSend(self, addr, data):

583 """ 584 Send data onto the Two-Wire Interface (TWI) (aka I2c) of the Linkbot. 585 Many Linkbot peripherals are located on the TWI bus, including the 586 accelerometer, breakout boards, and some sensors. The black phone-jack on 587 top of the Linkbot exposes TWI pins where custom or prebuild peripherals 588 may be attached. 589 590 @param addr: The TWI address to send data to. 591 @type data: iterable bytes 592 @param data: The byte data to send to the peripheral 593 """ 594 twibuf = bytearray(data) 595 buf = bytearray( 596 [barobo.BaroboCtx.CMD_TWI_SEND, 597 len(twibuf)+5, 598 addr, 599 len(twibuf)]) 600 buf += twibuf 601 buf += bytearray([0x00]) 602 self._transactMessage(buf)

603

604 - def twiSendRecv(self, addr, senddata, recvsize):

605 """ 606 Send and receive data from a TWI device attached to the Linkbot. See 607 twiSend() and twiRecv() for more details. 608 609 @param addr: The TWI address to send data to. 610 @type senddata: iterable 611 @param senddata: The byte data to send to the peripheral 612 @type recvsize: number 613 @param recvsize: Number of bytes expected from TWI device 614 @rtype: bytearray 615 """ 616 twibuf = bytearray(senddata) 617 buf = bytearray( 618 [barobo.BaroboCtx.CMD_TWI_SENDRECV, 0, addr, len(twibuf)]) 619 buf += twibuf 620 buf += bytearray([recvsize, 0x00]) 621 buf[1] = len(buf) 622 response = self._transactMessage(buf) 623 return bytearray(response[2:-1])

Source Code for Module barobo.linkbot