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="")
val = int(input_argv.pop(0))
delay = int(input_argv.pop(0))
delay = delay / 1000
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,
import borra_functions as bf
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from time import sleep
import importlib
###Create a new figure to graph 3D
#Draw base
base_points = bf.base_points(92)
#Draw plate
plate_points = bf.plate_points(72.91,6,[0,-6,0],[0,0,117])
#Draw servo
servo_links = [16.46,117.22]
theta1,theta2= bf.get_servo_angle(plate_points,servo_links,base_points)
#print("\n{}Finalizo correctamente{}".format("-"*35,"-"*35))
#for i in range(6):
# print("Punto {}:\n theta1 = {}\n theta2 = {}".format(i,theta1[i],theta2[i]))
#####Graficar
plt.ion()
fig = plt.figure()
ax = fig.add_subplot(111,projection='3d')
bf.draw_axis(110,110,220,ax,fig)
bf.draw_by_points(base_points,ax,fig,'orangered')
bf.draw_by_points(plate_points,ax,fig,'dodgerblue')
import borra_functions as bf
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from time import sleep
import importlib
###Create a new figure to graph 3D
#Draw base
base_points = bf.base_points(92)
#Draw plate
plate_points = bf.plate_points(72.91, 6, [0, 0, 0], [0, 0, 108])
#Draw servo
servo_links = [16.46, 117.22]
theta1, theta2 = bf.get_servo_angle(plate_points, servo_links, base_points)
#print("\n{}Finalizo correctamente{}".format("-"*35,"-"*35))
#for i in range(6):
# print("Punto {}:\n theta1 = {}\n theta2 = {}".format(i,theta1[i],theta2[i]))
#####Graficar
plt.ion()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
bf.draw_axis(110, 110, 220, ax, fig)
bf.draw_by_points(base_points, ax, fig, 'orangered')
bf.draw_by_points(plate_points, ax, fig, 'dodgerblue')
