def move_hips(b_1, epout, epin, buffer, orden, cola_read, cola_orden, vel,
ang):
write = conf.readData("cadera", "write")
read = conf.readData("cadera", "read")
media = cola_read.get()
[b_1, buffer, media] = libdef.openMov(b_1, media, epout, epin, buffer,
write, read)
ang = libdef.conversorAngEnc(1, 1, ang)
ite = libdef.numIte(ang, vel)
step_in = media[0]
signo = libdef.get_signo(21, buffer)
dato_in = [step_in, signo]
signal_out = ''
for i in range(ite):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, i, ite, orden, vel, media,
buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
value_err = libdef.getError(buffer, VEC_ERROR[0])
if value_err >= MAX_ERROR:
print("Limite articulacion")
cola_orden.put(1)
logging.warning(libdef.error_msg(1))
break
media = libdef.get_media(buffer, media)
#Control de error
cont = 0
while abs(dato_in[0] - media[0]) > 20 and value_err < MAX_ERROR:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(orden, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
if cont == 100:
print("Error en el while")
cola_orden.put(2)
logging.warning(libdef.error_msg(2))
break
cont += 1
[b_1, buffer, media] = libdef.closeMov(b_1, media, orden, signal_out,
epout, epin, buffer, write, read)
cola_read.put(media)
return b_1
def msg_start(epout,epin,buffer):
media = []
vec_pos = conf.readData("general","VEC_POS")
for i in range(len(vec_pos)):
vect_int = []
for j in range(2):
vect_int.append(buffer[vec_pos[i] + j])
dato_int = libdef.transform(vect_int)
media.append(dato_int)
b_1 = 0
b_1 = send_pkt1(b_1,epout,epin,buffer)
b_1 = send_wait(b_1,epout,epin,buffer)
b_1 = send_pkt2(b_1,epout,epin,buffer)
b_1 = send_wait(b_1,epout,epin,buffer)
b_1 = send_pkt3(b_1,epout,epin,buffer,media)
return [b_1, media]
def syncro(cola_sync, cola_read, epout, epin, buffer):
while 1:
b_1 = cola_sync.get()
if b_1 == conf.readData("general","EXIT"):
break
if b_1 == 256:
time.sleep(READ)
else:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer,cola_read.get())
cadena += libdef.get_encoder(buffer,media)
libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ)
media = libdef.get_media(buffer,media)
cola_read.put(media)
cola_sync.put(b_1)
import time
import libcomm
import libhex
import libdef
import conf
import log
import logging
###############################################################################
# Script que realiza la conexion inicial entre la aplicación y la controladora
# y mantiene la sincronización entre ambos durante el uso de la aplicacion.
###############################################################################
# Tiempo de espera para realizar una peticion de lectura tras la escritura
WRITE = conf.readData("general","WRITE")
# Tiempo de espera para empezar a generar el siguiente mensaje a enviar tras
# una peticion de lectura.
READ = conf.readData("general","READ")
# Define hilo principal de sincronizacion entre la controladora y el programa.
# Envía mensajes de estado de reposo a la controladora.
# Se mantiene una comunicacion en tiempo real con el estado de los encoder
# de cada motor. Se actualiza el valor del byte de secuencia y del vector
# media
#
# cola_sync -> Cola con el byte de secuencia
# cola_read -> Cola con el vector media de la posición de encoders
# epout -> Objeto de endpoint de salida de la controladora
# epin -> Objeto de enpoint de entrada de la controladora
def execute(cola_sync, cola_orden, cola_read, epout, epin, buffer):
#Inicializamos el home en false, esto cambiara una vez se realice el home
home = False
# Se corresponde con el angulo inicial respecto a la vertical
posRef = conf.readData("general", "posRef")
while 1:
select = cola_orden.get()
orden = select[0]
if orden != 19:
vel = select[1]
ite = select[2]
cola_sync.put(MAX_COUNT)
while orden != None:
b_1 = cola_sync.get()
if b_1 == MAX_COUNT:
cola_sync.put(MAX_COUNT)
else:
if orden != 19 and orden != 16 and orden != 17:
home = False
posRef = conf.readData("general", "posRef")
if orden == 4 or orden == 5:
b_1 = move_hips(b_1, epout, epin, buffer, orden, cola_read,
cola_orden, vel, ite)
elif orden == 6 or orden == 7:
b_1 = move_shoulder(b_1, epout, epin, buffer, orden,
cola_read, cola_orden, vel, ite)
elif orden == 8 or orden == 9:
b_1 = move_elbow(b_1, epout, epin, buffer, orden,
cola_read, cola_orden, vel, ite)
elif orden == 10 or orden == 11 or orden == 12 or orden == 13:
b_1 = move_wrist(b_1, epout, epin, buffer, orden,
cola_read, cola_orden, vel, ite)
elif orden == 14 or orden == 15:
b_1 = clamp(b_1, epout, epin, buffer, orden, cola_read)
elif orden == 16:
b_1 = motors_off(b_1, epout, epin, buffer, cola_read)
elif orden == 17:
b_1 = motors_on(b_1, epout, epin, buffer, cola_read)
elif orden == 18:
[b_1, status] = setHome.homing(b_1, epout, epin, buffer,
cola_read, cola_orden)
if status == 0:
home = True
status = 0
elif orden == 19:
if home == True:
[b_1,
posRef] = moveXYZ.controlXYZ(select[1], select[2],
posRef, b_1, epout, epin,
buffer, cola_read,
cola_orden)
else:
cola_orden.put(5)
print("Home no hecho")
logging.warning(libdef.error_msg(5))
elif orden == EXIT:
b_1 = motors_off(b_1, epout, epin, buffer, cola_read)
scorbotoff(b_1, epout, epin, buffer, cola_read)
cola_sync.put(EXIT)
break
orden = None
cola_sync.put(b_1)
cola_orden.put(DONE)
time.sleep(READ)
if orden == EXIT:
break
def clamp(b_1, epout, epin, buffer, orden, cola_read):
write = conf.readData("pinza", "write")
read = conf.readData("pinza", "read")
vel = conf.readData("pinza", "vel")
media = cola_read.get()
for i in range(1, 4):
cadena = libhex.clamp(i)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
cadena += libdef.get_encoder(buffer, media)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
step_in = media[5]
signo = libdef.get_signo(46, buffer)
dato_in = [step_in, signo]
signal_out = ''
ite = conf.readData("pinza", "ite_clamp")
for i in range(ite):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, i, ite, orden, vel, media,
buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
for _ in range(15):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(orden, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont = 0
while abs(dato_in[0] - media[5]) == 0:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(orden, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
if cont == 100:
print("ERROR: La articulación no responde")
cola_orden.put(
1) #Introduce codigo de error en la ejecucion de la orden
logging.warning(libdef.error_msg(1))
break
cont += 1
media = libdef.get_media(buffer, media)
cadena = libhex.clamp(4)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(orden, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cadena = libhex.clamp(5)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(orden, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cola_read.put(media)
return b_1
def move_wrist(b_1, epout, epin, buffer, orden, cola_read, cola_orden, vel,
ang):
write = conf.readData("wrist", "write")
read = conf.readData("wrist", "read")
media = cola_read.get()
[b_1, buffer, media] = libdef.openMov(b_1, media, epout, epin, buffer,
write, read)
if orden == 10 or orden == 11:
ang = libdef.conversorAngEnc(4, 1, ang)
ite = libdef.numIte(ang, vel)
else:
ang = libdef.conversorAngEnc(5, 1, ang)
ite = libdef.numIte(ang, vel)
step_in_1 = media[3]
step_in_2 = media[4]
signo_1 = libdef.get_signo(36, buffer)
signo_2 = libdef.get_signo(41, buffer)
dato_in_1 = [step_in_1, signo_1]
dato_in_2 = [step_in_2, signo_2]
signal_out = ''
for i in range(ite):
siguiente_1 = libdef.transform(
[buffer[VEC_POS[3]], buffer[VEC_POS[3] + 1]])
siguiente_2 = libdef.transform(
[buffer[VEC_POS[4]], buffer[VEC_POS[4] + 1]])
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
if orden == 10:
dato_in_1 = libdef.resta(dato_in_1, i + 1, vel, ite)
dato_in_2 = libdef.suma(dato_in_2, i + 1, vel, ite)
elif orden == 11:
dato_in_1 = libdef.suma(dato_in_1, i + 1, vel, ite)
dato_in_2 = libdef.resta(dato_in_2, i + 1, vel, ite)
elif orden == 12:
dato_in_1 = libdef.suma(dato_in_1, i + 1, vel, ite)
dato_in_2 = libdef.suma(dato_in_2, i + 1, vel, ite)
elif orden == 13:
dato_in_1 = libdef.resta(dato_in_1, i + 1, vel, ite)
dato_in_2 = libdef.resta(dato_in_2, i + 1, vel, ite)
signal_out = libdef.detrans(dato_in_1[0])
signal_out += dato_in_1[1]
signal_out += libdef.detrans(dato_in_2[0])
signal_out += dato_in_2[1]
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(orden, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
value_err1 = libdef.getError(buffer, VEC_ERROR[3])
value_err2 = libdef.getError(buffer, VEC_ERROR[4])
if value_err1 >= MAX_ERROR or value_err2 >= MAX_ERROR:
print("Limite articulacion")
cola_orden.put(
1) #Introduce codigo de error en la ejecucion de la orden
logging.warning(libdef.error_msg(1))
break
media = libdef.get_media(buffer, media)
cont = 0
while (
abs(dato_in_1[0] - media[3]) > 20
or abs(dato_in_2[0] - media[4]) > 20
) and value_err1 < MAX_ERROR and value_err2 < MAX_ERROR: #cambia media segun articulacion
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(orden, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
if cont == 100:
print("ERROR: La articulación no responde")
cola_orden.put(
2) #Introduce codigo de error en la ejecucion de la orden
logging.warning(libdef.error_msg(2))
break
cont += 1
[b_1, buffer, media] = libdef.closeMov(b_1, media, orden, signal_out,
epout, epin, buffer, write, read)
cola_read.put(media)
return b_1
import libhex
import libdef
import time
import conf
import setHome
import moveXYZ
import log
import logging
#################################################################################
# Script encargado de la creación de los mensajes de movimientos. Se separa
# por articulaciones ya que cada una tiene una estructura de mensajes diferentes.
#################################################################################
# Identificacion de que se va a cerrar el programa
EXIT = conf.readData("general", "EXIT")
# Posiciones de los datos de cada motor dentro del buffer
# [cadera, hombro, codo, m1_muñeca, m2_muñeca, pinza]
VEC_POS = conf.readData("general", "VEC_POS")
# Posiciones de los bytes de error de cada motor dentro del buffer
# [cadera, hombro, codo, m1_muñeca, m2_muñeca, pinza]
VEC_ERROR = conf.readData("general", "VEC_ERROR")
# Valor maximo del byte de secuencia
MAX_COUNT = conf.readData("general", "MAX_COUNT")
# Identificacion de que no hubo errores durante la accion
DONE = conf.readData("general", "DONE")
# Tiempo de espera para realizar una peticion de lectura tras la escritura
WRITE = conf.readData("general", "WRITE")
# Tiempo de espera para empezar a generar el siguiente mensaje a enviar tras
# una peticion de lectura.
READ = conf.readData("general", "READ")
def homing(b_1, epout, epin, buffer, cola_read, cola_orden):
status = 0
block = False
sw = libdef.get_switch(SW_SHOULDER, buffer[5])
media = cola_read.get()
signal_out = ''
logging.info(libdef.info_text(5))
#Comprueba hombro en su sitio
if sw == True:
sw = True
else:
#muevo el hombro
write = conf.readData("hombro", "write")
vel = conf.readData("hombro", "h_vel")
read = conf.readData("hombro", "read")
cadena = libhex.mov_comm(2)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
cadena += libdef.get_encoder(buffer, media)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
step_in = media[1]
signo = libdef.get_signo(26, buffer)
dato_in = [step_in, signo]
cont_vel = 0
while (sw == False):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, cont_vel - 1, 100, 6, vel,
media, buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
sw = libdef.get_switch(SW_SHOULDER, buffer[5])
value_err = libdef.getError(buffer, VEC_ERROR[1])
if value_err >= MAX_ERROR:
block = True
print("Limite articulacion")
cola_orden.put(1)
logging.warning(libdef.error_msg(1))
break
if cont_vel < 12:
cont_vel += 1
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#Realiza la frenada controlada
cont_vel = 88
for i in range(12):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, cont_vel - 1, 100, 6, vel,
media, buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont = 0
while abs(dato_in[0] - media[1]) > 20:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(6, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont += 1
if cont == 100:
print("ERROR: La articulación no responde")
cola_orden.put(2)
logging.warning(libdef.error_msg(2))
block = True
break
[b_1, buffer,
media] = libdef.closeMov(b_1, media, 6, signal_out, epout, epin,
buffer, write, read)
logging.info(libdef.info_text(18))
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#transicion entre articulaciones
for i in range(20):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += msg
libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ)
media = libdef.get_media(buffer, media)
#Compruebo codo
sw = libdef.get_switch(SW_ELBOW, buffer[5])
if sw == True:
sw = True
else:
#muevo el codo
write = conf.readData("codo", "write")
vel = conf.readData("codo", "h_vel")
read = conf.readData("codo", "read")
cadena = libhex.mov_comm(2)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
cadena += libdef.get_encoder(buffer, media)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
step_in = media[2]
signo = libdef.get_signo(31, buffer)
dato_in = [step_in, signo]
cont_vel = 0
while (sw == False):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, cont_vel - 1, 100, 8, vel,
media, buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
sw = libdef.get_switch(SW_ELBOW, buffer[5])
value_err = libdef.getError(buffer, VEC_ERROR[2])
if value_err >= MAX_ERROR:
print("Limite articulacion")
cola_orden.put(1)
logging.warning(libdef.error_msg(1))
block = True
break
if cont_vel < 12:
cont_vel += 1
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#Realiza la frenada controlada
cont_vel = 88
for i in range(12):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, cont_vel - 1, 100, 8, vel,
media, buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont = 0
while abs(dato_in[0] - media[2]) > 20 and block != True:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(8, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont += 1
if cont == 100:
block = True
cola_orden.put(2)
logging.warning(libdef.error_msg(2))
print("ERROR: La articulación no responde")
break
[b_1, buffer,
media] = libdef.closeMov(b_1, media, 8, signal_out, epout, epin,
buffer, write, read)
logging.info(libdef.info_text(19))
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#transicion entre articulaciones
for i in range(20):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += msg
libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ)
media = libdef.get_media(buffer, media)
#Combruebo pitch
sw = libdef.get_switch(SW_PITCH, buffer[5])
if sw == True:
sw = True
else:
#muevo pitch
write = conf.readData("wrist", "write")
vel = conf.readData("wrist", "h_vel")
read = conf.readData("wrist", "read")
cadena = libhex.mov_comm(2)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
cadena += libdef.get_encoder(buffer, media)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
step_in_1 = media[3]
step_in_2 = media[4]
signo_1 = libdef.get_signo(36, buffer)
signo_2 = libdef.get_signo(41, buffer)
dato_in_1 = [step_in_1, signo_1]
dato_in_2 = [step_in_2, signo_2]
cont_vel = 0
while (sw == False):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
dato_in_1 = libdef.suma(dato_in_1, cont_vel, vel, 100)
dato_in_2 = libdef.resta(dato_in_2, cont_vel, vel, 100)
signal_out = libdef.detrans(dato_in_1[0])
signal_out += dato_in_1[1]
signal_out += libdef.detrans(dato_in_2[0])
signal_out += dato_in_2[1]
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
sw = libdef.get_switch(SW_PITCH, buffer[5])
value_err1 = libdef.getError(buffer, VEC_ERROR[3])
value_err2 = libdef.getError(buffer, VEC_ERROR[4])
if value_err1 >= MAX_ERROR or value_err2 >= MAX_ERROR:
print("Limite articulacion")
block = True
cola_orden.put(1)
logging.warning(libdef.error_msg(1))
break
if cont_vel < 12:
cont_vel += 1
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#Ajuste de offset para alinear con la vertical
cont_vel = 87
for i in range(60):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
dato_in_1 = libdef.suma(dato_in_1, cont_vel, vel, 100)
dato_in_2 = libdef.resta(dato_in_2, cont_vel, vel, 100)
signal_out = libdef.detrans(dato_in_1[0])
signal_out += dato_in_1[1]
signal_out += libdef.detrans(dato_in_2[0])
signal_out += dato_in_2[1]
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont_vel += 1
#Realiza la frenada controlada
cont_vel = 88
for i in range(12):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
dato_in_1 = libdef.suma(dato_in_1, cont_vel, vel, 100)
dato_in_2 = libdef.resta(dato_in_2, cont_vel, vel, 100)
signal_out = libdef.detrans(dato_in_1[0])
signal_out += dato_in_1[1]
signal_out += libdef.detrans(dato_in_2[0])
signal_out += dato_in_2[1]
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont = 0
while abs(dato_in_1[0] - media[3]) > 20 or abs(dato_in_2[0] -
media[4]) > 20:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont += 1
if cont == 100:
block = True
cola_orden.put(2)
logging.warning(libdef.error_msg(2))
break
[b_1, buffer,
media] = libdef.closeMov(b_1, media, 10, signal_out, epout, epin,
buffer, write, read)
logging.info(libdef.info_text(20))
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#transicion entre articulaciones
for _ in range(20):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += msg
libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ)
media = libdef.get_media(buffer, media)
#Combruebo roll
sw = libdef.get_switch(SW_ROLL, buffer[5])
if sw == True:
sw = True
else:
#muevo roll
write = conf.readData("wrist", "write")
vel = conf.readData("wrist", "h_vel")
read = conf.readData("wrist", "read")
cadena = libhex.mov_comm(2)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
cadena += libdef.get_encoder(buffer, media)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
step_in_1 = media[3]
step_in_2 = media[4]
signo_1 = libdef.get_signo(36, buffer)
signo_2 = libdef.get_signo(41, buffer)
dato_in_1 = [step_in_1, signo_1]
dato_in_2 = [step_in_2, signo_2]
cont_vel = 0
while (sw == False):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
dato_in_1 = libdef.resta(dato_in_1, cont_vel, vel, 100)
dato_in_2 = libdef.resta(dato_in_2, cont_vel, vel, 100)
signal_out = libdef.detrans(dato_in_1[0])
signal_out += dato_in_1[1]
signal_out += libdef.detrans(dato_in_2[0])
signal_out += dato_in_2[1]
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
sw = libdef.get_switch(SW_ROLL, buffer[5])
value_err1 = libdef.getError(buffer, VEC_ERROR[3])
value_err2 = libdef.getError(buffer, VEC_ERROR[4])
if value_err1 >= MAX_ERROR or value_err2 >= MAX_ERROR:
print("Limite articulacion")
block = True
cola_orden.put(1)
logging.warning(libdef.error_msg(1))
break
if cont_vel < 12:
cont_vel += 1
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#Ajuste de offset para alinear con la horizontal
cont_vel = 87
for i in range(60):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
dato_in_1 = libdef.resta(dato_in_1, cont_vel, vel, 100)
dato_in_2 = libdef.resta(dato_in_2, cont_vel, vel, 100)
signal_out = libdef.detrans(dato_in_1[0])
signal_out += dato_in_1[1]
signal_out += libdef.detrans(dato_in_2[0])
signal_out += dato_in_2[1]
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont_vel += 1
#Realiza la frenada controlada
for i in range(12):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
dato_in_1 = libdef.resta(dato_in_1, cont_vel, vel, 100)
dato_in_2 = libdef.resta(dato_in_2, cont_vel, vel, 100)
signal_out = libdef.detrans(dato_in_1[0])
signal_out += dato_in_1[1]
signal_out += libdef.detrans(dato_in_2[0])
signal_out += dato_in_2[1]
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont = 0
while abs(dato_in_1[0] - media[3]) > 20 or abs(dato_in_2[0] -
media[4]) > 20:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(10, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont += 1
if cont == 100:
block = True
cola_orden.put(2)
logging.warning(libdef.error_msg(2))
print("ERROR: La articulación no responde")
break
[b_1, buffer,
media] = libdef.closeMov(b_1, media, 10, signal_out, epout, epin,
buffer, write, read)
logging.info(libdef.info_text(21))
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#transicion entre articulaciones
for i in range(20):
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += msg
libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ)
media = libdef.get_media(buffer, media)
#Combruebo cadera
sw = libdef.get_switch(SW_HIP, buffer[5])
if sw == True:
sw = True
else:
#muevo cadera
write = conf.readData("cadera", "write")
vel = conf.readData("cadera", "h_vel")
read = conf.readData("cadera", "read")
cadena = libhex.mov_comm(2)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
media = libdef.get_media(buffer, media)
cadena += libdef.get_encoder(buffer, media)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
step_in = media[0]
signo = libdef.get_signo(21, buffer)
dato_in = [step_in, signo]
cont_vel = 0
while (sw == False):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, cont_vel - 1, 100, 4, vel,
media, buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
sw = libdef.get_switch(SW_HIP, buffer[5])
value_err = libdef.getError(buffer, VEC_ERROR[0])
if value_err >= MAX_ERROR:
print("Limite articulacion")
block = True
cola_orden.put(1)
logging.warning(libdef.error_msg(1))
break
if cont_vel < 12:
cont_vel += 1
if block == True:
cola_read.put(media)
status = 1
return [b_1, status]
#Realiza la frenada controlada
cont_vel = 88
for i in range(12):
[b_1, cadena, signal_out,
dato_in] = libdef.builder(b_1, dato_in, cont_vel - 1, 100, 4, vel,
media, buffer)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont = 0
while abs(dato_in[0] - media[0]) > 20:
cadena = libhex.mov_comm(1)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 24)
msg = libdef.get_encoder(buffer, media)
cadena += libdef.getStruct(4, signal_out, msg)
libdef.set_msg(cadena, epout, epin, buffer, write, read)
media = libdef.get_media(buffer, media)
cont += 1
if cont == 100:
block = True
cola_orden.put(2)
logging.warning(libdef.error_msg(2))
break
[b_1, buffer,
media] = libdef.closeMov(b_1, media, 4, signal_out, epout, epin,
buffer, write, read)
logging.info(libdef.info_text(22))
cola_read.put(media)
return [b_1, status]
import libdef
import conf
import libhex
import time
import log
import logging
################################################################################
# Script para realizar el HOME del robot. La posicion fisica inicial de las
# articulaciones debe ser el correcto. La version del HOME no contempla la
# realizacion del HOME desde cualquier posicion.
################################################################################
# Posiciones de los datos de cada motor dentro del buffer
# [cadera, hombro, codo, m1_muñeca, m2_muñeca, pinza]
VEC_POS = conf.readData("general", "VEC_POS")
# Posiciones de los bytes de error de cada motor dentro del buffer
# [cadera, hombro, codo, m1_muñeca, m2_muñeca, pinza]
VEC_ERROR = conf.readData("general", "VEC_ERROR")
# Valor maximo del byte de secuencia
MAX_COUNT = conf.readData("general", "MAX_COUNT")
# Error maximo aceptable en los bytes de error
MAX_ERROR = conf.readData("general", "MAX_ERROR")
# Valor asociado al microinterruptor activo de la cadera
SW_HIP = conf.readData("cadera", "switch")
# Valor asociado al microinterruptor activo del hombro
SW_SHOULDER = conf.readData("hombro", "switch")
# Valor asociado al microinterruptor activo del codo
SW_ELBOW = conf.readData("codo", "switch")
# Valor asociado al microinterruptor activo del movimiento de pitch
SW_PITCH = conf.readData("wrist", "switch_pitch")
