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(