def print_packet(packet): # for character in packet : # print(hex(char_to_int(character)),end=' ') # print() for character in packet: print(hex(string_handling.char_to_int(character))), print
def read_memory(self,dynamixel_id) : ''' returns a dictionary consisting of address:value pairs as elements, simulating the dynamixel memory ''' memory = {} text_memory = '' # with open(gui.WORKING_DIRECTORY + 'subordinate_directory' + SYSTEM_PATH_SEPERATOR + 'dummy_dynamixel' + SYSTEM_PATH_SEPERATOR + 'dymmy_dynamixel_memory.txt','r') as f : if dynamixel_id == 1 : memory_path = WORKING_DIRECTORY + 'subordinate_directory' + SYSTEM_PATH_SEPERATOR + 'dummy_dynamixel' + SYSTEM_PATH_SEPERATOR + 'dummy_dynamixel_memory_1.txt' elif dynamixel_id == 2 : memory_path = WORKING_DIRECTORY + 'subordinate_directory' + SYSTEM_PATH_SEPERATOR + 'dummy_dynamixel' + SYSTEM_PATH_SEPERATOR + 'dummy_dynamixel_memory_2.txt' else : raise 'dummy_dynamixel.read_memory(self,dynamixel_id) --> incorrect dynamixel ID' with open(memory_path,'r') as f : text_memory = f.read() list_memory = [] return_string = '' i=0 while i < len(text_memory) : if text_memory[i] == '#' : i += string_handling.skip_until_character(text_memory,chr(13),i) if string_handling.char_to_int(text_memory[i]) != 13 : return_string += text_memory[i] else : list_memory.append(return_string) return_string = '' i += 1 temp_list_memory = [] for line in list_memory : if line != '' : temp_list_memory.append(line) list_memory = temp_list_memory del temp_list_memory for line in list_memory : address = '' value = '' try : address = int(line.split(' ')[0].split(' ')[0]) except ValueError : if(line == '') : pass else : raise ValueError else : try : value = int(line.split(' ')[1].split(' ')[0]) except ValueError : # value = 0 value = '-' memory[address] = value return memory
def angle_from_status_packet(packet,offset) : def hex_to_angle(position_low,position_high,offset): angle = (char_to_int(position_high))*256 + char_to_int(position_low) angle = -angle angle += offset angle = angle%4096 return int(angle) #print(list(packet)) # debug number_of_parameters = char_to_int(packet[3]) - 2 parameters = [] for i in range(5,5+number_of_parameters) : parameters.append(packet[i]) #print("hex = ",parameters) # debug return hex_to_angle(parameters[0],parameters[1],offset)
def check_for_error(status_packet): """ Checks for error in status packet. Returns a list containing the bit numbers that contain errors in the error bit, as returned by the dynamixel. """ error_byte = string_handling.char_to_int(status_packet[4]) if error_byte == 0: return False else: error_byte_list = [] for i in range(8): error_byte_list.append((int(error_byte / (2 ** i))) & 0x01) return error_byte_list
def check_checksum(status_packet): def not_checksum(l): checksum = 0 for i in range(len(l)): checksum += l[i] not_checksum = (~checksum) & 0xFF return not_checksum checksum = [] for i in range(2, len(status_packet) - 1): checksum.append(string_handling.char_to_int(status_packet[i])) not_checksum_ = not_checksum(checksum) if chr(not_checksum_) != status_packet[-1]: return False return True
def hex_to_angle(position_low,position_high,offset): angle = (char_to_int(position_high))*256 + char_to_int(position_low) angle = -angle angle += offset angle = angle%4096 return int(angle)
def get_status_packet(instruction_packet, status_packet): """ retrives status packet from the string returned by dynamixel. eliminates all the noise, and extra bits from the status packet returned by the dynamixel """ # ------------------ comment when dynamixel 1 works ------------------------ if instruction_packet[2] == "\x01": return True # ------------------------------------------------------------------------- common_string = "" # is the string of initial characters that need to # be both in the instruction packet and the status packet for i in range(3): common_string += instruction_packet[i] if common_string not in status_packet: print("common string not there") # CHANGE -- PRINT TO GUI return False # check the index of the character where the status packet actually starts. # basically counter the effects of noise in the received status packet # by finding exactly where the status packet is in the string for i in range(len(status_packet)): if ( status_packet[i] == common_string[0] and status_packet[i + 1] == common_string[1] and status_packet[i + 2] == common_string[2] ): break # if status_packet[i,i+3] == common_string[:] : # break number_of_parameters = string_handling.char_to_int(status_packet[i + 3]) - 2 error_byte = status_packet[i + 4] parameters = "" for j in range(i + 5, i + 5 + number_of_parameters): parameters += status_packet[j] checksum = status_packet[i + 5 + number_of_parameters] return_status_packet = "" return_status_packet += common_string return_status_packet += chr(number_of_parameters + 2) return_status_packet += error_byte return_status_packet += parameters return_status_packet += checksum def check_checksum(status_packet): def not_checksum(l): checksum = 0 for i in range(len(l)): checksum += l[i] not_checksum = (~checksum) & 0xFF return not_checksum checksum = [] for i in range(2, len(status_packet) - 1): checksum.append(string_handling.char_to_int(status_packet[i])) not_checksum_ = not_checksum(checksum) if chr(not_checksum_) != status_packet[-1]: return False return True # finally, check if checksum is correct if check_checksum(return_status_packet): return return_status_packet return False