Mover y disparar los lanzacohetes

Este es el código que utilizaremos para controlar nuestras torretas y disparar. El programa funcionará con tantas torretas como queramos sin cambiar nada.

En los comentarios aparece una explicación detallada sobre cada fragmento de código. He pensado que sería mejor así en vez de ir copiando cada parte fuera del contexto y explicar cómo funciona.

Y poned un poco de música. Hoy toca Stravinsky.

#===========================
#====LIBRERIAS DE OPENCV====
#===========================
import cv2
import numpy as np
#===========================
 
#===========================
#====LIBRERIAS ARMAMENTO====
#===========================
import usb.core
import sys
import time
import os
#===========================
 
#===========================
#VARIABLES Y ARRAYS GLOBALES
#===========================
#-Position almacena la posicion actual de la torreta
#-Ammo es el contador de municion. Al llegar a 0 escribira un error
#-Contador sirve para calcular el tiempo que el objetivo permanece quieto
#===========================
position = [0,0]
ammo = 4
contador = 0
#===========================
 
#===========================
#====CALIBRAR ARMAMENTO=====
#===========================
#-Busca todas las torretas del mismo tipo y las guarda dentro de un array
#-Desactiva el kernel para cada torreta dentro del array (si no lo ha hecho ya)
#-Configura todas las torretas
#===========================
#Busca todas las torretas con esta id y las guarda en un array
turrets = usb.core.find(find_all = True, idVendor = 0x2123, idProduct = 0x1010)
 
#Itera dentro del array y configura las torretas
for i in turrets:
    if i.is_kernel_driver_active(0) == True:
        i.detach_kernel_driver(0)
    i.set_configuration()
#===========================
 
#===================================
#==FUNCIONES BASICAS DE MOVIMIENTO==
#===================================
#-Funciones basicas para mover las armas arriba, abajo, disparar, etc
#-Envian una secuencia en hexadecimal via USB que las armas interpretan para moverse
#-Las secuencias en hexadecimal estan extraidas de la (poca) documentacion oficial
#===================================
 
def moveUp(): #Mueve la torreta hacia arriba
    for dev in turrets:
        dev.ctrl_transfer(0x21,0x09,0,0,[0x02,0x02,0x00,0x00,0x00,0x00,0x00,0x00])
 
def moveDown(): #Torreta hacia abajo
    for dev in turrets:
        dev.ctrl_transfer(0x21,0x09,0,0,[0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00])
 
def moveLeft(): #Torreta hacia la izquierda
    for dev in turrets:
        dev.ctrl_transfer(0x21,0x09,0,0,[0x02,0x04,0x00,0x00,0x00,0x00,0x00,0x00])
 
def moveRight(): #Torreta hacia la derecha
    for dev in turrets:
        dev.ctrl_transfer(0x21,0x09,0,0,[0x02,0x08,0x00,0x00,0x00,0x00,0x00,0x00])
 
def stop(): #Detiene la torreta
    for dev in turrets:
        dev.ctrl_transfer(0x21,0x09,0,0,[0x02,0x20,0x00,0x00,0x00,0x00,0x00,0x00])
 
def dispara(): #Abre fuego y lanza un proyectil
    for dev in turrets:
        dev.ctrl_transfer(0x21,0x09,0,0,[0x02,0x10,0x00,0x00,0x00,0x00,0x00,0x00])
 
#=================================
#=MOVER LA TORRETA A UNA POSICION=
#=================================
#-Mueve la torreta a una cierta posicion siguiendo este proceso:
#    -Calcular la distancia a desplazarse
#    -Dar la orden de movimiento
#    -Esperar un tiempo correspondiente a la distancia
#    -Dar la orden de stop
#    -Actualizar el array de posicion
#Para saber el tiempo de desplazamiento calculamos la diferencia entre
#la posicion actual y la deseada. Esto es el numero de pixeles.
#Para convertir los pixeles a grados, utilizamos una regla de tres:
#
#    Ancho de pantalla/2 px   ===Equivale a===>    45 grados
#
#    1 px             ===Equivale a===>    x grados
#
#Un grado equivale, aproximadamente, a un movimiento de 0,166667 segundos.
#
#Los argumentos son:
#x = posicion del objetivo en el eje x
#y = posicion del objetivo en el eje y
#x_max = ancho de pantalla/2
#y_max = altura de pantalla/2
#=================================
#Aqui hay dragones
def setPosition(x, y, x_max, y_max):
 
    #Convertimos las dimensiones de la pantalla a float
    x_max = float(x_max)
    y_max = float(y_max)
 
    #El valor de la Y viene invertido, así que debemos multiplicarlo por -1
    y = y*-1
 
    #Calcular el tiempo
    delay_X = abs((x-position[0])*(45/x_max)*0.01666667)
    delay_Y = abs((y-position[1])*(22.5/y_max)*0.016666667)
 
    #Este if limita la distancia de rotacion
    if x <= 60 and x >= -60 and y <= (22.5) and y >= (-22.5):
 
        #Si el objetivo esta a la derecha, gira a la derecha
        if x > position[0]:
         
            #Mover, esperar y parar
            moveRight()
            time.sleep(delay_X)
            stop()
            time.sleep(0.02)
 
            #Actualizamos la posicion x
            position[0] = x
 
        #Si no, gira a la izquierda
        elif x < position[0]:
            #Mover, esperar y parar
            moveLeft()
            time.sleep(delay_X)
            stop()
            time.sleep(0.02)
 
            #Actualizamos la posicion x
            position[0] = x
 
        if y > position[1]:
 
            #Mover, esperar y parar
            moveUp()
            time.sleep(delay_Y)
            stop()
            time.sleep(0.02)
 
            #Actualizamos la posicion y
            position[1] = y
 
        elif y < position[1]:
            #Mover, esperar y parar
            moveDown()
            time.sleep(delay_Y)
            stop()
            time.sleep(0.02)
 
            #Actualizamos la posicion y
            position[1] = y
 
        #Descomentar la siguiente linea para ver la posicion de la torreta
        #print "Posicion torreta: ", position
#=================================
 
#===========================
#=====RESETEAR POSICION=====
#===========================
#Al iniciar el programa no sabemos la posicion de las torretas
#Las movemos al maximo a la derecha + abajo y despues al centro
#===========================
def reset():
    moveRight()
    time.sleep(6)
    stop()
    moveDown()
    time.sleep(3)
    stop()
    moveLeft()
    time.sleep(3)
    stop()
    moveUp()
    time.sleep(0.375)
    stop()
    position = [0,0]
    print "Ready to raise some hell!"
#===========================
 
#==========================
#INICIALIZAR WEBCAM Y ARMAS
#==========================
 
#Inicializamos la webcam y la haarcascade
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml')
captura = cv2.VideoCapture(0)
 
#Reseteamos la posicion de las torretas
reset()
 
#Leemos un frame y lo escalamos para ahorrar conseguir más fluidez. 0.35 deberia ser suficiente
_, image = captura.read()
img = cv2.resize(image, (0,0), fx=0.35, fy=0.35)
 
#Guardamos las dimensiones de la imagen. Hacemos esto fuera del bucle ya que con una vez es suficiente
#y nos ahorramos tiempo de calculo
altura = img.shape[0]
largo = img.shape[1]
 
#Las dividimos entre 2. Asi podremos establecer el centro de la imagen
#como el punto [0,0]
altura = altura/2
largo = largo/2
#===========================
 
#===========================
#======BUCLE PRINCIPAL======
#===========================
#-Capturamos una imagen y la convertimos a escala de grises
#-Buscamos una cara. Si la encontramos guardamos su posicion
#-Si sus coordenadas son proximas a la posicion de la torreta (el objetivo esta quieto), sumamos +1 al contador
#-Si no, movemos la torreta
#-Si el contador == 5, disparamos (si hay municion, si no salimos del programa)
#===========================
while(True):
 
    #Leemos un frame, lo escalamos y lo convertimos a blanco y negro
    _, image = captura.read()
    img = cv2.resize(image, (0,0), fx=0.35, fy=0.35)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
 
    #Buscamos una cara y guardamos sus coordenadas en un array:
    #[extremo_superior, extremo_inferior, largo, ancho]
    faces = face_cascade.detectMultiScale(gray, 1.3, 5)
 
    for(x,y,w,h) in faces:
        #Descomentar la siguiente linea para dibujar un rectangulo alrededor de la cara
        #cv2.rectangle(img,(x,y),(x+w,y+h),(125,255,0),2)
        centX = (x+(w/2))-largo
        centY = (y+(h/2))-altura
         
        #Escribe las coordenadas del objeto
        #print "Centre X: ", centX
        #print "Centre Y: ", centY
 
        #Si el objetivo se ha desplazado mas de 4 pixeles movemos las torretas
        #Reiniciamos el contador
        if abs(centX - position[0]) > 4:
            setPosition(centX, centY, largo, altura)
            contador = 0
 
        #Si no, sumamos +1 al contador
        #Si contador == 5 disparamos si queda municion o escribimos un mensaje de error y
        #salimos del programa
        else:
            contador += 1
            if ammo != 0 and contador == 5:
                dispara()
                print "Ammo = ", ammo
                ammo -= 1
                time.sleep(4)
                contador = 0
            elif ammo == 0 and contador == 5:
                print "Out of ammo. Time to sleep."
     
    #Podemos mostrar la captura por pantalla descomentando:
    #cv2.imshow('captura', img)
 
    #Si el usuario pulsa la tecla 'q' o nos quedamos sin municion salimos del programa
    if cv2.waitKey(1) & 0xFF == ord('q') or ammo == 0:
        break
 
captura.release()

 

 

One Comment

Leave a Reply