def __eventThread(self):
while True:
evt = self.eventQueue.get(block=True, timeout=None)
if (evt[0] == barobo.BaroboCtx.EVENT_BUTTON) \
and self.callbackEnabled:
self.callbackfunc(evt[6], evt[7], self.callbackUserData)
elif (evt[0] == barobo.BaroboCtx.EVENT_JOINT_MOVED) \
and self.jointCallbackEnabled:
values = barobo._unpack('<Lfff', evt[2:18])
self.jointcallbackfunc(
values[0],
values[1]*180.0/math.pi,
values[2]*180.0/math.pi,
values[3]*180.0/math.pi,
evt[22])
elif (evt[0] == barobo.BaroboCtx.EVENT_ACCEL_CHANGED) \
and self.accelCallbackEnabled:
values = barobo._unpack('<L', evt[2:6]) + \
barobo._unpack('>3h', evt[6:12])
self.accelcallbackfunc(
values[0],
values[1]/16384.0,
values[2]/16384.0,
values[3]/16384.0)
elif evt[0] == barobo.BaroboCtx.EVENT_DEBUG_MSG:
s = barobo._unpack('s', evt[2:-1])
print ("Debug msg from {0}: {1}".format(self.serialID, s))
def getJointAnglesTime(self):
"""
Get the joint angles with a timestamp. The timestamp is the number of
seconds since the robot has powered on.
@rtype: [numbers]
@return: [seconds, angle1, angle2, angle3], all angles in degrees
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORANGLESTIMESTAMPABS, 0x03, 0x00])
response = self._transactMessage(buf)
millis = barobo._unpack('<L', response[2:6])[0]
data = barobo._unpack('<4f', response[6:6+16])
rc = [millis/1000.0]
rc += list(map(_util.rad2deg, data))
return rc
def _run(self):
# Try to read byte from physical layer
self.readbuf = bytearray([])
self.phys.flush()
self.phys.flushInput()
self.phys.flushOutput()
while True:
byte = self.phys.read()
if self.stopflag:
break
if byte is None:
continue
if DEBUG:
print ("Byte: {0}".format(list(map(hex, bytearray(byte)))))
self.readbuf += bytearray(byte)
if (len(self.readbuf) <= 2):
continue
if len(self.readbuf) == self.readbuf[1]:
# Received whole packet
if DEBUG:
print ("Recv: {0}".format(list(map(hex, self.readbuf))))
zigbeeAddr = barobo._unpack('!H', self.readbuf[2:4])[0]
if self.readbuf[0] != barobo.BaroboCtx.EVENT_REPORTADDRESS:
pkt = Packet(self.readbuf[5:-1], zigbeeAddr)
else:
pkt = Packet(self.readbuf, zigbeeAddr)
self.deliver(pkt)
self.readbuf = self.readbuf[self.readbuf[1]:]
def _run(self):
# Try to read byte from physical layer
self.readbuf = bytearray([])
self.phys.flush()
self.phys.flushInput()
self.phys.flushOutput()
while True:
byte = self.phys.read()
if self.stopflag:
break
if byte is None:
continue
if DEBUG:
print("Byte: {0}".format(list(map(hex, bytearray(byte)))))
self.readbuf += bytearray(byte)
if (len(self.readbuf) <= 2):
continue
if len(self.readbuf) == self.readbuf[1]:
# Received whole packet
if DEBUG:
print("Recv: {0}".format(list(map(hex, self.readbuf))))
zigbeeAddr = barobo._unpack('!H', self.readbuf[2:4])[0]
if self.readbuf[0] != barobo.BaroboCtx.EVENT_REPORTADDRESS:
pkt = Packet(self.readbuf[5:-1], zigbeeAddr)
else:
pkt = Packet(self.readbuf, zigbeeAddr)
self.deliver(pkt)
self.readbuf = self.readbuf[self.readbuf[1]:]
def getJointAnglesTime(self):
"""
Get the joint angles with a timestamp. The timestamp is the number of
seconds since the robot has powered on.
@rtype: [numbers]
@return: [seconds, angle1, angle2, angle3], all angles in degrees
"""
buf = bytearray(
[barobo.BaroboCtx.CMD_GETMOTORANGLESTIMESTAMPABS, 0x03, 0x00])
response = self._transactMessage(buf)
millis = barobo._unpack('<L', response[2:6])[0]
data = barobo._unpack('<4f', response[6:6 + 16])
rc = [millis / 1000.0]
rc += list(map(_util.rad2deg, data))
return rc
def getHWRev(self):
"""
Get the hardware revision of the Linkbot
@rtype: [major, minor, micro]
@return: The major, minor, and micro version numbers
"""
buf = bytearray([barobo.BaroboCtx.CMD_GET_HW_REV, 0x03, 0x00])
response = self._transactMessage(buf)
return barobo._unpack('<3B', response[2:5])
def getColorRGB(self):
"""
Get the current RGB values of the rgbled
@rtype: [number, number, number]
@return: The red, green, and blue values from 0 to 255
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETRGB, 0x03, 0x00])
response = self._transactMessage(buf)
return barobo._unpack('<3B', response[2:5])
def getHWRev(self):
"""
Get the hardware revision of the Linkbot
@rtype: [major, minor, micro]
@return: The major, minor, and micro version numbers
"""
buf = bytearray([barobo.BaroboCtx.CMD_GET_HW_REV, 0x03, 0x00])
response = self._transactMessage(buf)
return barobo._unpack('<3B', response[2:5])
def getColorRGB(self):
"""
Get the current RGB values of the rgbled
@rtype: [number, number, number]
@return: The red, green, and blue values from 0 to 255
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETRGB, 0x03, 0x00])
response = self._transactMessage(buf)
return barobo._unpack('<3B', response[2:5])
def getJointSpeed(self, joint):
"""
Get the speed setting of a joint. Returned value is in deg/s
@rtype: float
@return: The joint speed in deg/sec
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORSPEED, 4, joint-1, 0])
response = self._transactMessage(buf)
speed = barobo._unpack('<f', response[2:6])[0]
return _util.rad2deg(speed)
def getBatteryVoltage(self):
"""
Get the current battery voltage of the Linkbot.
@rtype: number
@return: Returns a value in Volts
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETBATTERYVOLTAGE, 0x03, 0x00])
response = self._transactMessage(buf)
voltage = barobo._unpack('<f', response[2:6])[0]
return voltage
def getJointSpeed(self, joint):
"""
Get the speed setting of a joint. Returned value is in deg/s
@rtype: float
@return: The joint speed in deg/sec
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORSPEED, 4, joint - 1, 0])
response = self._transactMessage(buf)
speed = barobo._unpack('<f', response[2:6])[0]
return _util.rad2deg(speed)
def getSerialID(self):
"""
Get the serial ID from the Linkbot
@return: A four character string
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETSERIALID, 3, 0])
response = self._transactMessage(buf)
botid = barobo._unpack('!4s', response[2:6])[0]
self.serialID = botid
return botid
def getBatteryVoltage(self):
"""
Get the current battery voltage of the Linkbot.
@rtype: number
@return: Returns a value in Volts
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETBATTERYVOLTAGE, 0x03, 0x00])
response = self._transactMessage(buf)
voltage = barobo._unpack('<f', response[2:6])[0]
return voltage
def getJointAngles(self):
"""
Get the current joint angles.
@rtype: [float, float, float]
@return: The joint angles in degrees
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORANGLESABS, 3, 0])
response = self._transactMessage(buf)
angles = barobo._unpack('<4f', response[2:18])
return list(map(_util.rad2deg, angles))
def getSerialID(self):
"""
Get the serial ID from the Linkbot
@return: A four character string
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETSERIALID, 3, 0])
response = self._transactMessage(buf)
botid = barobo._unpack('!4s', response[2:6])[0]
self.serialID = botid
return botid
def getJointAngles(self):
"""
Get the current joint angles.
@rtype: [float, float, float]
@return: The joint angles in degrees
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORANGLESABS, 3, 0])
response = self._transactMessage(buf)
angles = barobo._unpack('<4f', response[2:18])
return list(map(_util.rad2deg, angles))
def getJointAngle(self, joint):
"""
Get the current angle position of a joint.
@type joint: number
@param joint: Get the position of this joint. Can be 1, 2, or 3
@rtype: number
@return: Return the joint angle in degrees
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETMOTORANGLE, 0x04, joint-1, 0x00])
response = self._transactMessage(buf)
return _util.rad2deg(barobo._unpack('<f', response[2:6])[0])
def getAccelerometerData(self):
"""
Get the current accelerometer readings
@rtype: [number, number, number]
@return: A list of acceleration values in the x, y, and z directions.
Accelerometer values are in units of "G's", where 1 G is standard earth
gravitational acceleration (9.8m/s/s)
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETACCEL, 0x03, 0x00])
response = self._transactMessage(buf)
values = barobo._unpack('<3h', response[2:8])
return list(map(lambda x: x / 16384.0, values))
def getAccelerometerData(self):
"""
Get the current accelerometer readings
@rtype: [number, number, number]
@return: A list of acceleration values in the x, y, and z directions.
Accelerometer values are in units of "G's", where 1 G is standard earth
gravitational acceleration (9.8m/s/s)
"""
buf = bytearray([barobo.BaroboCtx.CMD_GETACCEL, 0x03, 0x00])
response = self._transactMessage(buf)
values = barobo._unpack('<3h', response[2:8])
return list(map(lambda x: x/16384.0, values))
def getBreakoutADC(self, adc):
"""
Get the ADC (Analog-to-digital) reading from a breakout-board's ADC
channel.
@type adc: number
@param adc: The ADC channel to get the value from [0, 7]
@rtype: number
@return: Value from 0-1023
"""
buf = bytearray([barobo.BaroboCtx.TWIMSG_HEADER, barobo.BaroboCtx.TWIMSG_ANALOGREADPIN, adc])
data = self.twiSendRecv(0x02, buf, 2)
return barobo._unpack('!h', data)[0]
def getJointAngle(self, joint):
"""
Get the current angle position of a joint.
@type joint: number
@param joint: Get the position of this joint. Can be 1, 2, or 3
@rtype: number
@return: Return the joint angle in degrees
"""
buf = bytearray(
[barobo.BaroboCtx.CMD_GETMOTORANGLE, 0x04, joint - 1, 0x00])
response = self._transactMessage(buf)
return _util.rad2deg(barobo._unpack('<f', response[2:6])[0])
def sfp_deliver(self, buf, size, userdata):
if (size <= 2):
return
if size == buf[1]:
# Received whole packet
if DEBUG:
print ("Recv: {0}".format(list(map(hex, buf[:size]))))
zigbeeAddr = barobo._unpack('!H', bytearray(buf[2:4]))[0]
if buf[0] != barobo.BaroboCtx.EVENT_REPORTADDRESS:
pkt = Packet(bytearray(buf[5:size]), zigbeeAddr)
else:
pkt = Packet(bytearray(buf[:size]), zigbeeAddr)
self.deliver(pkt)
def __eventThread(self):
while True:
evt = self.eventQueue.get(block=True, timeout=None)
if (evt[0] == barobo.BaroboCtx.EVENT_BUTTON) \
and self.callbackEnabled:
self.callbackfunc(evt[6], evt[7], self.callbackUserData)
elif (evt[0] == barobo.BaroboCtx.EVENT_JOINT_MOVED) \
and self.jointCallbackEnabled:
values = barobo._unpack('<Lfff', evt[2:18])
self.jointcallbackfunc(values[0], values[1] * 180.0 / math.pi,
values[2] * 180.0 / math.pi,
values[3] * 180.0 / math.pi, evt[22])
elif (evt[0] == barobo.BaroboCtx.EVENT_ACCEL_CHANGED) \
and self.accelCallbackEnabled:
values = barobo._unpack('<L', evt[2:6]) + \
barobo._unpack('>3h', evt[6:12])
self.accelcallbackfunc(values[0], values[1] / 16384.0,
values[2] / 16384.0,
values[3] / 16384.0)
elif evt[0] == barobo.BaroboCtx.EVENT_DEBUG_MSG:
s = barobo._unpack('s', evt[2:-1])
print("Debug msg from {0}: {1}".format(self.serialID, s))
def sfp_deliver(self, buf, size, userdata):
if (size <= 2):
return
if size == buf[1]:
# Received whole packet
if DEBUG:
print("Recv: {0}".format(list(map(hex, buf[:size]))))
zigbeeAddr = barobo._unpack('!H', bytearray(buf[2:4]))[0]
if buf[0] != barobo.BaroboCtx.EVENT_REPORTADDRESS:
pkt = Packet(bytearray(buf[5:size]), zigbeeAddr)
else:
pkt = Packet(bytearray(buf[:size]), zigbeeAddr)
self.deliver(pkt)
def getVersion(self):
"""
Get the firmware version of the Linkbot
@return: Something like (0, 0, 94) or (3, 0, 3), depending on the
oldness of the firmware
"""
try:
buf = bytearray([barobo.BaroboCtx.CMD_GETVERSIONS, 3, 0])
response = self._transactMessage(buf)
version = barobo._unpack('!3B', response[2:5])
except Exception as e:
buf = bytearray([barobo.BaroboCtx.CMD_GETVERSION, 3, 0])
response = self._transactMessage(buf)
version = (0, 0, response[2])
return version
def getVersion(self):
"""
Get the firmware version of the Linkbot
@return: Something like (0, 0, 94) or (3, 0, 3), depending on the
oldness of the firmware
"""
try:
buf = bytearray([barobo.BaroboCtx.CMD_GETVERSIONS, 3, 0])
response = self._transactMessage(buf)
version = barobo._unpack('!3B', response[2:5])
except Exception as e:
buf = bytearray([barobo.BaroboCtx.CMD_GETVERSION, 3, 0])
response = self._transactMessage(buf)
version = (0, 0, response[2])
return version
def getBreakoutADC(self, adc):
"""
Get the ADC (Analog-to-digital) reading from a breakout-board's ADC
channel.
@type adc: number
@param adc: The ADC channel to get the value from [0, 7]
@rtype: number
@return: Value from 0-1023
"""
buf = bytearray([
barobo.BaroboCtx.TWIMSG_HEADER,
barobo.BaroboCtx.TWIMSG_ANALOGREADPIN, adc
])
data = self.twiSendRecv(0x02, buf, 2)
return barobo._unpack('!h', data)[0]
def _getADCVolts(self, adc):
buf = bytearray([barobo.BaroboCtx.CMD_GETENCODERVOLTAGE, 4, adc, 0])
response = self._transactMessage(buf)
voltage = barobo._unpack('<f', response[2:6])[0]
return voltage
def _getADCVolts(self, adc):
buf = bytearray([barobo.BaroboCtx.CMD_GETENCODERVOLTAGE, 4, adc, 0])
response = self._transactMessage(buf)
voltage = barobo._unpack('<f', response[2:6])[0]
return voltage
