def send_pkt1(b_1,epout,epin,buffer): while b_1 < 4: cadena = libhex.mov_comm(6) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) cadena = libdef.fill_msg(cadena, 8) msg = libhex.get_msg1(b_1) cadena += msg libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) return b_1
def send_wait(b_1,epout,epin,buffer): while buffer[1] != 13: cadena = libhex.mov_comm(1) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) cadena = libhex.mov_comm(1) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) return b_1
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 motors_off(b_1, epout, epin, buffer, cola_read):
media = libdef.get_media(buffer, cola_read.get())
for i in range(1, 27):
cadena = libhex.motorsoff(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)
cola_read.put(media)
return b_1
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)
def send_pkt3(b_1,epout,epin,buffer, media): for i in range(40): cadena = libhex.mov_comm(1) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) media = libdef.get_media(buffer,media) libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) 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) cadena += libdef.get_encoder(buffer,media) libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) for i in range(1,8): cadena = libhex.mov_comm(6) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) cadena = libdef.fill_msg(cadena, 8) cadena += libhex.motorson(i) 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) return b_1
def scorbotoff(b_1, epout, epin, buffer, cola_read):
media = libdef.get_media(buffer, cola_read.get())
for i in range(1, 12):
cadena = libhex.mov_comm(6)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena += libhex.get_scorbotoff(i)
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)
b_1 = libsync.send_wait(b_1, epout, epin, buffer)
cadena = libhex.mov_comm(6)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 8)
cadena += libhex.get_msg1(4)
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)
cadena = libhex.mov_comm(6)
b_1 = libdef.countByte1(b_1)
cadena = cadena.format(libdef.f_byte(b_1))
cadena = libdef.fill_msg(cadena, 8)
cadena += libhex.get_msg1(2)
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)
cola_read.put(media)
def send_pkt2(b_1,epout,epin,buffer): for i in range(1,9): cadena = libhex.get_msg2(80) if(i < 3): b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1), '72') libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) if(i >= 3 and i < 6): b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1), '64') libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) if(i >= 6): b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1), '61') libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) cadena = libhex.get_msg2(81) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) cadena = libhex.get_msg2(82) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) b_7 = 0 b_6 = 1 for i in range(1,80): cadena = libhex.get_msg2(83) msg = libhex.get_msg2(i) b_1 = libdef.countByte1(b_1) b_7 = countByte7(b_7) b_6 = countByte6(b_7, b_6) cadena = cadena.format(libdef.f_byte(b_1), libdef.f_byte(b_6), libdef.f_byte(b_7)) cadena += msg libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) cadena = libhex.get_msg2(84) b_1 = libdef.countByte1(b_1) cadena = cadena.format(libdef.f_byte(b_1)) libdef.set_msg(cadena, epout, epin, buffer, WRITE, READ) return b_1
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
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]
