def get_status_packet(instruction_packet,status_packet) : common_string = '' for i in range(3) : common_string += instruction_packet[i] if(common_string not in status_packet) : return False 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 number_of_parameters = 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 if(check_checksum(return_status_packet)) : return return_status_packet return False
def print_packet(packet) : # for character in packet : # print(hex(char_to_int(character)),end=' ') # print() for character in packet : print (hex(char_to_int(character))),
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 check_for_error(status_packet) : error_byte = 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_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(char_to_int(status_packet[i])) not_checksum_ = not_checksum(checksum) if(chr(not_checksum_) != status_packet[-1]) : return False return True
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')
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
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]
print_packet(parameters)
checksum = status_packet[i+5+number_of_parameters]
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
