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 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 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 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