def set_plataform(angles,translation): #([yaw,pitch,roll],[x,y,z])
plate_points = bf.plate_points(centroid_dist,scrapt,angles,translation)
try:
theta1,theta2 = bf.get_servo_angle(plate_points,servo_links,base_points)
#print("get_servo_angle")
servos_value = []
for i in theta1:
servos_value.append(int(i[0]))
#print("{}Angles{}\nYaw = {} | Pitch = {} |Roll = {}".format("-"*15,"-"*15,str(angles[0]).rjust(3,' '),str(angles[1]).rjust(3,' '),str(angles[2]).rjust(3,' ')))
#print("{}Translation{}\nDx = {} | Dy ={} | Dz = {}".format("-"*12,"-"*13,str(translation[0]).rjust(3,' '),str(translation[1]).rjust(3,' '),str(translation[2]).rjust(3,' ')))
#
#---#-Servos angles----#
#print("\n{}The servos value are{}\n".format("-"*11,"-"*12),end="")
#print("|",end="")
#for i in range(6):
# print(" ser{} |".format(i),end="")
#print("\n|",end="")
#for i in range(6):
# print(" {} |".format(str(servos_value[i ]).rjust(4,' ')),end="")
#
#print("\n"+"-"*43+"\n")
#----------Set angles servos----------#
#end_servo = bf.set_servo_values(servos_value,min_signal_degree,max_signal_degree,min_servo_signal,max_servo_signal,"online",servos)
end_servo = bf.set_servo_values(servos_value,min_signal_degree,max_signal_degree,min_servo_signal,max_servo_signal,"online")
except ValueError:
print("\n\x1b[1;31m"+"Error: It isn't posible set the current position (MathDomain Error)\n")
print("\x1b[0;37m",end="")
def set_plataform(angles,translation): #([yaw,pitch,roll],[x,y,z])
plate_points = bf.plate_points(centroid_dist,scrapt,angles,translation)
try:
theta1,theta2 = bf.get_servo_angle(plate_points,servo_links,base_points)
#print("get_servo_angle")
servos_value = []
for i in theta1:
servos_value.append(int(i[0]))
end_servo = bf.set_servo_values(servos_value,min_signal_degree,max_signal_degree,min_servo_signal,max_servo_signal,"online",servos)
except ValueError:
print("\n\x1b[1;31m"+"Error: It isn't posible set the current position (MathDomain Error)\n")
print("\x1b[0;37m",end="")
print("Se correra {} veces".format(val))
else:
print(
"Error: You should put almost 1 rerun ant the time in milliseconds"
)
if len(angles_rerun) == 0:
print("There isn't any record")
else:
for rerun_val in range(val):
for current in range(len(angles_rerun)):
plate_points = bf.plate_points(
centroid_dist, scrapt, angles_rerun[current],
translation_rerun[current])
theta1, theta2 = bf.get_servo_angle(
plate_points, servo_links, base_points)
bf.clear_screen()
print("Rerun {} - Record {}".format(
rerun_val + 1, current + 1))
servos_value = []
for i in theta1:
servos_value.append(int(i[0]))
print("{}Angles{}\nYaw = {} | Pitch = {} | Roll = {}".
format(
"-" * 15, "-" * 15,
str(angles_rerun[current][0]).rjust(3, ' '),
str(angles_rerun[current][1]).rjust(3, ' '),
str(angles_rerun[current][2]).rjust(3, ' ')))
print(