TXPACKET_MAX_LEN = 250 RXPACKET_MAX_LEN = 250 # for Protocol Packet PKT_HEADER0 = 0 PKT_HEADER1 = 1 PKT_ID = 2 PKT_LENGTH = 3 PKT_INSTRUCTION = 4 PKT_ERROR = 4 PKT_PARAMETER0 = 5 # Protocol Error bit ERRBIT_VOLTAGE = 1 #0x01 ERRBIT_SENSOR = 2 #0x02 ERRBIT_OVERHEAT = 4 #0x04 ERRBIT_CURRENT = 8 #0x08 ERRBIT_ANGLE = 16 #0x10 ERRBIT_OVERLOAD = 32 #0x20 BROADCAST_ID = 254 #0xFE # Instruction for Servo Protocol INST_PING = 1 #0x01 INST_READ = 2 #0x02 INST_WRITE = 3 #0x03 INST_REG_WRITE = 4 #0x04 INST_ACTION = 5 #0x05 INST_RESET = 6 #0x06 INST_SYNC_READ = 130 #0x82 INST_SYNC_WRITE = 131 #0x83 # Communication Result COMM_SUCCESS = 0 # tx or rx packet communication success COMM_PORT_BUSY = -1 # Port is busy (in use) COMM_TX_FAIL = -2 # Failed transmit instruction packet COMM_RX_FAIL = -3 # Failed get status packet COMM_TX_ERROR = -4 # Incorrect instruction packet COMM_RX_WAITING = -5 # Now receiving status packet COMM_RX_TIMEOUT = -6 # There is no status packet COMM_RX_CORRUPT = -7 # Incorrect status packet COMM_NOT_AVAILABLE = -9 #Protocol does not support this function class PacketHandler(object): def __init__(self, PortHandler, endianness): self.PortHandler = PortHandler self.endianness = endianness # 0: little-endian 1:big-endian def getEndian(self): return self.endianness def setEndian(self, e): self.endianness = e def toHost(self, a, b): if (a & (1 << b)): return -(a & ~(1 << b)) else: return a def toServo(self, a, b): if (a < 0): return (-a | (1 << b)) else: return a def makeWord16(self, a, b): if self.endianness == 0: return (a & 0xFF) | ((b & 0xFF) << 8) else: return (b & 0xFF) | ((a & 0xFF) << 8) def makeWord32(self, a, b): return (a & 0xFFFF) | (b & 0xFFFF) << 16 def getLowWord32(self, l): return l & 0xFFFF def get_Highword32(self, h): return (h >> 16) & 0xFFFF def getLowByte(self, w): if self.endianness == 0: return w & 0xFF else: return (w >> 8) & 0xFF def getHighByte(self, w): if self.endianness == 0: return (w >> 8) & 0xFF else: return w & 0xFF def getTxRxResult(self, result): if result == COMM_SUCCESS: return "[TxRxResult] Communication success!" elif result == COMM_PORT_BUSY: return "[TxRxResult] Port is in use!" elif result == COMM_TX_FAIL: return "[TxRxResult] Failed transmit instruction packet!" elif result == COMM_RX_FAIL: return "[TxRxResult] Failed get status packet from device!" elif result == COMM_TX_ERROR: return "[TxRxResult] Incorrect instruction packet!" elif result == COMM_RX_WAITING: return "[TxRxResult] Now receiving status packet!" elif result == COMM_RX_TIMEOUT: return "[TxRxResult] There is no status packet!" elif result == COMM_RX_CORRUPT: return "[TxRxResult] Incorrect status packet!" elif result == COMM_NOT_AVAILABLE: return "[TxRxResult] Protocol does not support this function!" else: return "" def getRxPacketError(self, error): if error & ERRBIT_VOLTAGE: return "[ServoStatus] Input voltage error!" if error & ERRBIT_SENSOR: return "[ServoStatus] Sensor error!" if error & ERRBIT_OVERHEAT: return "[ServoStatus] Overheat error!" if error & ERRBIT_CURRENT: return "[ServoStatus] Current error!" if error & ERRBIT_ANGLE: return "[ServoStatus] Angle error!" if error & ERRBIT_OVERLOAD: return "[ServoStatus] Overload error!" return "" def txPacket(self, txpacket): checksum = 0 total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH if self.PortHandler.is_using: return COMM_PORT_BUSY self.PortHandler.is_using = True # check max packet length if total_packet_length > TXPACKET_MAX_LEN: self.PortHandler.is_using = False return COMM_TX_ERROR # make packet header txpacket[PKT_HEADER0] = 0xFF txpacket[PKT_HEADER1] = 0xFF # add a checksum to the packet for index in range(2, total_packet_length - 1): # except header, checksum checksum += txpacket[index] txpacket[total_packet_length - 1] = ~checksum & 0xFF # tx packet self.PortHandler.clearPort() written_packet_length = self.PortHandler.writePort(txpacket) if total_packet_length != written_packet_length: self.PortHandler.is_using = False return COMM_TX_FAIL return COMM_SUCCESS def rxPacket(self): rxpacket = [] result = COMM_TX_FAIL checksum = 0 rx_length = 0 wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) while True: rxpacket.extend(self.PortHandler.readPort(wait_length - rx_length)) rx_length = len(rxpacket) if rx_length >= wait_length: # find packet header index = 0 for index in range(0, (rx_length - 1)): if (rxpacket[index] == 0xFF) and (rxpacket[index + 1] == 0xFF): break if index == 0: # found at the beginning of the packet if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or ( rxpacket[PKT_ERROR] > 0x7F): # unavailable ID or unavailable Length or unavailable Error # remove the first byte in the packet del rxpacket[0] rx_length -= 1 continue # re-calculate the exact length of the rx packet if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1): wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1 continue if rx_length < wait_length: # check timeout if self.PortHandler.isPacketTimeout(): if rx_length == 0: result = COMM_RX_TIMEOUT else: result = COMM_RX_CORRUPT break else: continue # calculate checksum for i in range(2, wait_length - 1): # except header, checksum checksum += rxpacket[i] checksum = ~checksum & 0xFF # verify checksum if rxpacket[wait_length - 1] == checksum: result = COMM_SUCCESS else: result = COMM_RX_CORRUPT break else: # remove unnecessary packets del rxpacket[0: index] rx_length -= index else: # check timeout if self.PortHandler.isPacketTimeout(): if rx_length == 0: result = COMM_RX_TIMEOUT else: result = COMM_RX_CORRUPT break self.PortHandler.is_using = False return rxpacket, result def txRxPacket(self, txpacket): rxpacket = None error = 0 # tx packet result = self.txPacket(txpacket) if result != COMM_SUCCESS: return rxpacket, result, error # (ID == Broadcast ID) == no need to wait for status packet or not available if txpacket[PKT_ID] == BROADCAST_ID: self.PortHandler.is_using = False return rxpacket, result, error # set packet timeout if txpacket[PKT_INSTRUCTION] == INST_READ: self.PortHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6) else: self.PortHandler.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM # rx packet while True: rxpacket, result = self.rxPacket() if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]: break if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]: error = rxpacket[PKT_ERROR] return rxpacket, result, error def ping(self, id): error = 0 txpacket = [0] * 6 if id > BROADCAST_ID: return COMM_NOT_AVAILABLE, error txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_PING rxpacket, result, error = self.txRxPacket(txpacket) return rxpacket, result, error def action(self, id): txpacket = [0] * 6 txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_ACTION _, result, _ = self.txRxPacket(txpacket) return result def readData(self, id, address, length): txpacket = [0] * 8 data = [] if id > BROADCAST_ID: return data, COMM_NOT_AVAILABLE, 0 txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = 4 txpacket[PKT_INSTRUCTION] = INST_READ txpacket[PKT_PARAMETER0 + 0] = address txpacket[PKT_PARAMETER0 + 1] = length rxpacket, result, error = self.txRxPacket(txpacket) if result == COMM_SUCCESS: error = rxpacket[PKT_ERROR] data.extend(rxpacket[PKT_PARAMETER0: PKT_PARAMETER0 + length]) return data, result, error def read1ByteData(self, id, address): data, result, error = self.readData(id, address, 1) data_read = data[0] if (result == COMM_SUCCESS) else 0 return data_read, result, error def read2ByteData(self, id, address): data, result, error = self.readData(id, address, 2) data_read = self.makeWord16(data[0], data[1]) if (result == COMM_SUCCESS) else 0 return data_read, result, error def read4ByteData(self, id, address): data, result, error = self.readData(id, address, 4) data_read = self.makeWord32(self.makeWord16(data[0], data[1]), self.makeWord16(data[2], data[3])) if (result == COMM_SUCCESS) else 0 return data_read, result, error def writeDataOnly(self, id, address, length, data): txpacket = [0] * (length + 7) txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = length + 3 txpacket[PKT_INSTRUCTION] = INST_WRITE txpacket[PKT_PARAMETER0] = address txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] result = self.txPacket(txpacket) self.PortHandler.is_using = False return result def writeReadData(self, id, address, length, data): txpacket = [0] * (length + 7) txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = length + 3 txpacket[PKT_INSTRUCTION] = INST_WRITE txpacket[PKT_PARAMETER0] = address txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] rxpacket, result, error = self.txRxPacket(txpacket) return result, error def write1ByteDataOnly(self, id, address, data): data_write = [data] return self.writeDataOnly(id, address, 1, data_write) def writeRead1ByteData(self, id, address, data): data_write = [data] return self.writeReadData(id, address, 1, data_write) def write2ByteDataOnly(self, id, address, data): data_write = [self.getLowByte(data), self.getHighByte(data)] return self.writeDataOnly(id, address, 2, data_write) def writeRead2ByteData(self, id, address, data): data_write = [self.getLowByte(data), self.getHighByte(data)] return self.writeReadData(id, address, 2, data_write) def write4ByteDataOnly(self, id, address, data): data_write = [self.getLowByte(self.getLowWord32(data)), self.getHighByte(self.getLowWord32(data)), self.getLowByte(self.get_Highword32(data)), self.getHighByte(self.get_Highword32(data))] return self.writeDataOnly(id, address, 4, data_write) def writeRead4ByteData(self, id, address, data): data_write = [self.getLowByte(self.getLowWord32(data)), self.getHighByte(self.getLowWord32(data)), self.getLowByte(self.get_Highword32(data)), self.getHighByte(self.get_Highword32(data))] return self.writeReadData(id, address, 4, data_write) def regWriteTxOnly(self, id, address, length, data): txpacket = [0] * (length + 7) txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = length + 3 txpacket[PKT_INSTRUCTION] = INST_REG_WRITE txpacket[PKT_PARAMETER0] = address txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] result = self.txPacket(txpacket) self.PortHandler.is_using = False return result def regWriteTxRx(self, id, address, length, data): txpacket = [0] * (length + 7) txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = length + 3 txpacket[PKT_INSTRUCTION] = INST_REG_WRITE txpacket[PKT_PARAMETER0] = address txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length] _, result, error = self.txRxPacket(txpacket) return result, error def syncReadTx(self, start_address, data_length, param, param_length): txpacket = [0] * (param_length + 8) # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN CHKSUM txpacket[PKT_ID] = BROADCAST_ID txpacket[PKT_LENGTH] = param_length + 4 # 7: INST START_ADDR DATA_LEN CHKSUM txpacket[PKT_INSTRUCTION] = INST_SYNC_READ txpacket[PKT_PARAMETER0 + 0] = start_address txpacket[PKT_PARAMETER0 + 1] = data_length txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] result = self.txPacket(txpacket) return result def syncReadRx(self, data_length, param_length): wait_length = (6 + data_length) * param_length self.PortHandler.setPacketTimeout(wait_length) rxpacket = [] rx_length = 0 while True: rxpacket.extend(self.PortHandler.readPort(wait_length - rx_length)) rx_length = len(rxpacket) if rx_length >= wait_length: result = COMM_SUCCESS break else: # check timeout if self.PortHandler.isPacketTimeout(): if rx_length == 0: result = COMM_RX_TIMEOUT else: result = COMM_RX_CORRUPT break self.PortHandler.is_using = False return result, rxpacket def syncWriteTxOnly(self, start_address, data_length, param, param_length): txpacket = [0] * (param_length + 8) # 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM txpacket[PKT_ID] = BROADCAST_ID txpacket[PKT_LENGTH] = param_length + 4 # 4: INST START_ADDR DATA_LEN ... CHKSUM txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE txpacket[PKT_PARAMETER0 + 0] = start_address txpacket[PKT_PARAMETER0 + 1] = data_length txpacket[PKT_PARAMETER0 + 2: PKT_PARAMETER0 + 2 + param_length] = param[0: param_length] _, result, _ = self.txRxPacket(txpacket) return result def reset(self, id): error = 0 txpacket = [0] * 6 if id > BROADCAST_ID: return COMM_NOT_AVAILABLE, error txpacket[PKT_ID] = id txpacket[PKT_LENGTH] = 2 txpacket[PKT_INSTRUCTION] = INST_RESET rxpacket, result, error = self.txRxPacket(txpacket) return result, error class GroupSyncRead: def __init__(self, handler, start_address, data_length): self.handler = handler self.start_address = start_address self.data_length = data_length self.last_result = False self.is_param_changed = False self.param = [] self.data_dict = {} self.clearParam() def makeParam(self): if not self.data_dict: # len(self.data_dict.keys()) == 0: return self.param = [] for scs_id in self.data_dict: self.param.append(scs_id) def addParam(self, id): if id in self.data_dict: # scs_id already exist return False self.data_dict[id] = [] # [0] * self.data_length self.is_param_changed = True return True def removeParam(self, id): if id not in self.data_dict: # NOT exist return del self.data_dict[id] self.is_param_changed = True def clearParam(self): self.data_dict.clear() def txPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE if self.is_param_changed is True or not self.param: self.makeParam() return self.handler.syncReadTx(self.start_address, self.data_length, self.param, len(self.data_dict.keys())) def rxPacket(self): self.last_result = True result = COMM_RX_FAIL if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE result, rxpacket = self.handler.syncReadRx(self.data_length, len(self.data_dict.keys())) if len(rxpacket) >= (self.data_length + 6): for id in self.data_dict: self.data_dict[id], result = self.readRx(rxpacket, id, self.data_length) if result != COMM_SUCCESS: self.last_result = False else: self.last_result = False return result def txRxPacket(self): result = self.txPacket() if result != COMM_SUCCESS: return result return self.rxPacket() def readRx(self, rxpacket, id, data_length): data = [] rx_length = len(rxpacket) rx_index = 0 while (rx_index + 6 + data_length) <= rx_length: headpacket = [0x00, 0x00, 0x00] while rx_index < rx_length: headpacket[2] = headpacket[1] headpacket[1] = headpacket[0] headpacket[0] = rxpacket[rx_index] rx_index += 1 if (headpacket[2] == 0xFF) and (headpacket[1] == 0xFF) and headpacket[0] == id: break if (rx_index+3+data_length) > rx_length: break if rxpacket[rx_index] != (data_length+2): rx_index += 1 continue rx_index += 1 Error = rxpacket[rx_index] rx_index += 1 calSum = id + (data_length+2) + Error data = [Error] data.extend(rxpacket[rx_index : rx_index+data_length]) for i in range(0, data_length): calSum += rxpacket[rx_index] rx_index += 1 calSum = ~calSum & 0xFF if calSum != rxpacket[rx_index]: return None, COMM_RX_CORRUPT return data, COMM_SUCCESS return None, COMM_RX_CORRUPT def isAvailable(self, id, address, data_length): #if self.last_result is False or scs_id not in self.data_dict: if id not in self.data_dict: return False, 0 if (address < self.start_address) or (self.start_address + self.data_length - data_length < address): return False, 0 if not self.data_dict[id]: return False, 0 if len(self.data_dict[id])<(data_length+1): return False, 0 return True, self.data_dict[id][0] def getData(self, id, address, data_length): if data_length == 1: return self.data_dict[id][address-self.start_address+1] elif data_length == 2: return self.handler.makeWord16(self.data_dict[id][address-self.start_address+1], self.data_dict[id][address-self.start_address+2]) elif data_length == 4: return self.handler.makeWord32(self.handler.makeWord16(self.data_dict[id][address-self.start_address+1], self.data_dict[id][address-self.start_address+2]), self.handler.makeWord16(self.data_dict[id][address-self.start_address+3], self.data_dict[id][address-self.start_address+4])) else: return 0 class GroupSyncWrite: def __init__(self, handler, start_address, data_length): self.handler = handler self.start_address = start_address self.data_length = data_length self.is_param_changed = False self.param = [] self.data_dict = {} self.clearParam() def makeParam(self): if not self.data_dict: return self.param = [] for id in self.data_dict: if not self.data_dict[id]: return self.param.append(id) self.param.extend(self.data_dict[id]) def addParam(self, id, data): if id in self.data_dict: # id already exist return False if len(data) > self.data_length: # input data is longer than set return False self.data_dict[id] = data self.is_param_changed = True return True def removeParam(self, id): if id not in self.data_dict: # NOT exist return del self.data_dict[id] self.is_param_changed = True def changeParam(self, id, data): if id not in self.data_dict: # NOT exist return False if len(data) > self.data_length: # input data is longer than set return False self.data_dict[id] = data self.is_param_changed = True return True def clearParam(self): self.data_dict.clear() def txPacket(self): if len(self.data_dict.keys()) == 0: return COMM_NOT_AVAILABLE if self.is_param_changed is True or not self.param: self.makeParam() # print(self.data_dict) return self.handler.syncWriteTxOnly(self.start_address, self.data_length, self.param, len(self.data_dict.keys()) * (1 + self.data_length))