En una ocasión se me ocurrió hacer uso de joystick para manejar un servo y que contará con una interfaz gráfica. Como he estado utilizando mayormente el lenguaje Python, busqué un ejemplo y encontré uno del cual base ese ejemplo, sin embargo no he podido encontrar el ejemplo original. Parece que era Basic Motion Graphics with Python pero su pagina web ya no esta disponible.
En este ejemplo se moverá un cubo 3D de colores, con el acelerómetro. Si el acelerómetro se inclina hacia los costados, el cubo se desplazará hacia la derecha o izquierda, si se inclina al frente o atrás, este rotará hacia el adelante o hacia atrás, si se mueve hacia arriba y hacia bajo, el cubo se moverá hacia arriba y hacia abajo.
En la parte final, se encuentra comentado el código para controlar el cubo con un joystick USB.
Los movimientos registrados por el acelerómetro son enviados a la PC por medio de un microcontrolador. El microcontrolador es un PIC12F510 y el firmware del microcontrolador es el mismo que el de la entrada Prueba del acelerómetro MM7260 con PIC y PyQwt.
Los módulos necesarios son:
-PIL
Código:
#!/usr/bin/env python
"""
joystick.get_hat(0) : Obtiene el estado del hat
(la palanquita) en formato (x,y) de -1 a 1
"""
#######################
# EXTERNAL MODULES
#######################
import pygame
from pygame.locals import *
from OpenGL.GL import *
from OpenGL.GLU import *
import Image
import random
import sys
import time
import serial
#JOYSTICK
import pygame
from pygame import locals
#######################
# CONSTANTS
#######################
width = 720
height = 576
animName = 'Rotacion del cubo a traves de acelerometro'
#######################
# FUNCTIONS
#######################
def drawFrame( ejex = 0, ejey = 0, ejez = -6.0, rotx = 1, roty = 1):
"""
This draws a frame by calling all the other draw functions.
"""
# draw shapes
drawCube(ejex, ejey, ejez, rotx, roty)
# update screen
pygame.display.flip()
def drawCube( ejex, ejey, ejez , rotx , roty ):
"""
Draws a 3D cube within the animation.
"""
gradX = 360 * rotx #rota en el eje x de acuerdo al factor rotx del joystick
gradY = 360 * roty#rota en el eje y de acuerdo al factor roty del joystick
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity()
glTranslatef(ejex, ejey, ejez)
glRotatef(gradX,0,1.0, 0)
glRotatef(gradY,1.0, 0, 0)
glBegin(GL_QUADS)
glColor3f(0.0,1.0,0.0)
glVertex3f( 1.0, 1.0,-1.0)
glVertex3f(-1.0, 1.0,-1.0)
glVertex3f(-1.0, 1.0, 1.0)
glVertex3f( 1.0, 1.0, 1.0)
glColor3f(1.0,0.5,0.0)
glVertex3f( 1.0,-1.0, 1.0)
glVertex3f(-1.0,-1.0, 1.0)
glVertex3f(-1.0,-1.0,-1.0)
glVertex3f( 1.0,-1.0,-1.0)
glColor3f(1.0,0.0,0.0)
glVertex3f( 1.0, 1.0, 1.0)
glVertex3f(-1.0, 1.0, 1.0)
glVertex3f(-1.0,-1.0, 1.0)
glVertex3f( 1.0,-1.0, 1.0)
glColor3f(1.0,1.0,0.0)
glVertex3f( 1.0,-1.0,-1.0)
glVertex3f(-1.0,-1.0,-1.0)
glVertex3f(-1.0, 1.0,-1.0)
glVertex3f( 1.0, 1.0,-1.0)
glColor3f(0.0,0.0,1.0)
glVertex3f(-1.0, 1.0, 1.0)
glVertex3f(-1.0, 1.0,-1.0)
glVertex3f(-1.0,-1.0,-1.0)
glVertex3f(-1.0,-1.0, 1.0)
glColor3f(1.0,0.0,1.0)
glVertex3f( 1.0, 1.0,-1.0)
glVertex3f( 1.0, 1.0, 1.0)
glVertex3f( 1.0,-1.0, 1.0)
glVertex3f( 1.0,-1.0,-1.0)
glEnd()
def resizeGL((scrWidth, scrHeight)):
if scrHeight==0:
scrHeight=1
glViewport(0, 0, scrWidth, scrHeight)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45, 1.0*scrWidth/scrHeight, 0.1, 100.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
def initGL():
glShadeModel(GL_SMOOTH)
glClearColor(0.0, 0.0, 0.0, 0.0)
glClearDepth(1.0)
glEnable(GL_DEPTH_TEST)
glDepthFunc(GL_LEQUAL)
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)
def decodeData():
#self.timer.stop()
#time.sleep(3)
#self.timer.start(10)
data = ptoSerial.readline()
movX = 0
movY = 0
rotY = 0
if (len(data) <7):
print "Error"
return 0, 0, False
x = ord(data[1])
y = ord(data[3])
z = ord(data[5])
'''
#el minimo es 71 y el maximo es 195 .
195-71 = 123. 123/200 = (200/123) =
#en reposo esta en 143 pero debe de estar en 133,
el problema es que no esta perfectamente,
no estan los pines a 90 grados
'''
if x > 160:
movX = 1
elif x < 130:
movX = -1
else :
movX = 0
'''
par y aproximadamente el minimo es de 65
y el maximo es de 191. 191-65 = 126.
va de -1.00 a 1.00. (200/126) = (1.587 -200)/100
y por menos uno
para que la punta cuando suba apunte al cielo
'''
rotY = ((1.587 * y)-200) /100
if x < 150:
if z > 195 :
movY = +1
elif z < 180 :
movY = -1
else:
movY = 0
return movX, -rotY, movY
#######################
# MAIN
#######################
if __name__ == '__main__':
frames = 1
ejez = -6.0
ejex = 0
ejey = 0
dirx = 0
diry = 0
rotx = 1
roty = 1
zoom = -6
#ptoSerial = serial.Serial('/dev/ttyUSB0', 9600, timeout = 1)
ptoSerial = serial.Serial('COM3', 9600, timeout = 1)
# open pygame window
video_flags = OPENGL|DOUBLEBUF
pygame.init()
pygame.display.set_mode((width, height),video_flags)
pygame.display.set_caption(animName)
resizeGL((width, height))
initGL()
drawFrame()
#while True:
#drawFrame( ejex, ejey, ejez, rotx, roty )
while True:
time.sleep(.01)
x, roty, z = decodeData()
if x ==1:
ejex = ejex +0.1
elif x ==-1:
ejex = ejex -0.1
if z == 1:
ejey = ejey + 0.4
elif z == -1:
ejey = ejey - 0.4
else:
ejey = 0
drawFrame( ejex, ejey, ejez, rotx, roty )
for event in pygame.event.get():
if event.type == pygame.QUIT \
or event.type == KEYDOWN and event.key == K_ESCAPE:
ptoSerial.close()
sys.exit(0)
# while 1:
# time.sleep(0.1)
# e =pygame.event.poll()
# #acerca o aleja el cubo
# if e:
#
# #mueve en el eje x o y
# haty = j.get_hat(0)[1]
# hatx = j.get_hat(0)[0]
# if haty == -1:
# diry = -1
# print "abajo"
# elif haty == 1:
# diry = 1
# print "arriba"
# elif haty == 0:
# diry= 0
#
# if hatx == -1:
# dirx = -1
# print "izquierda"
# elif hatx == 1:
# dirx= 1
# elif hatx == 0:
# dirx = 0
# print "derecha"
# print j.get_hat(0)[0]
#
# if dirx == -1:
# ejex = ejex - 0.1
# elif dirx ==1:
# ejex = ejex + 0.1
#
# if diry == -1:
# ejey = ejey - 0.1
# elif diry ==1:
# ejey = ejey + 0.1
#
# #zoom
#
# if e.type == pygame.locals.JOYBUTTONDOWN:
# if e.dict["button"] ==6:
# print "alejar"
# zoom = -1
# elif e.dict["button"]==7:
# print "acercar"
# zoom = 1
#
# if e.type == pygame.locals.JOYBUTTONUP:
# if e.dict["button"] ==6 or e.dict["button"] ==7 :
# zoom = 0
#
# if zoom == -1:
# ejez =ejez - 0.1
# elif zoom == 1:
# ejez =ejez + 0.1
#
# #rota el cubo
# if e.type == pygame.locals.JOYAXISMOTION: # 7
# x , y = j.get_axis(0), j.get_axis(1)
# rotx = x
# roty = y
#
# #salir
# if e.type == pygame.locals.JOYBUTTONDOWN:
# if e.dict["button"] ==3:
# print "Salir"
# sys.exit(1)
Primero dibujamos el cubo en la posición con coordenadas XYZ, que equivalen a las variables ejex, ejey, ejez. Las variables rotx y roty, definen la dirección, el eje, y el giro del cubo. Las variables rotx y roty son factores que multiplicados por 360 (giro completo) ajustan los movimientos del cubo. Pueden tomar valores negativos, para que el movimiento sea contrario a las manecillas del reloj. La decodificación de los datos se hace de manera práctica, no es exacta. Se toman los valores normales del acelerómetro en reposo y se deja un margen de error. Si esto no se hiciera, las pequeñas fluctuaciones de voltaje moverían el cubo. Para saber el sentido de giro y de desplazamiento del cubo, se toma el valor normal del eje y se compara con el valor medido, si este es inferior, el giro o desplazamiento es negativo (valor con signo -), si es superior, el giro es positivo ( valor con signo +). Por simplicidad, el movimiento del eje z solo se indica con un salto del cubo, no refleja la aceleración medida. Todos los valores deben de estar entre –1.00 y 1.00 para que los movimientos sean fracciones de la unidad, si el movimiento es completo, entonces el valor máximo es 1 o –1, dependiendo del sentido del movimiento. Para lograr esta correspondencia, se hace una regla de tres con los valores máximos y mínimos, para después dividir este valor entre 100.
El código de la parte final que ha sido comentado, hace lo mismo que el acelerómetro, pero con un joystick. Los hats equivalen al D-Pad, y se componen de una lista que guarda en un elemento, los valores de las flechas arriba y abajo y en el otro los de izquierda y derecha. Estos valores son 0 (sin movimiento), 1, –1. Los axis equivalen a los ejes de un joystick, pero cada par de movimientos equivale a un eje. Izquierda y derecha, un eje, arriba y abajo otro eje. Esto creo que es por que existen controles como los de los aviones, que solo se mueven en un solo eje, arriba y abajo. Estos ejes regresan un valor que va de 1 a –1. El eje a la mitad equivaldría a 0.5, pues es un eje con valores analógicos. El joypad que utilice para este ejemplo es el sidewinder dual strike de Microsoft.
Esta imagen se ve como queda el cubo:
Con esto se puede hacer una animación por computadora en tiempo real, con objetos reales en movimiento.
0 comentarios:
Publicar un comentario