Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
	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
Ejemplo n.º 3
0
        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)
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
    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
Ejemplo n.º 6
0
 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)
Ejemplo n.º 7
0
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