6轴游戏手柄测试python程序

为了测试6轴无线游戏手柄，我写了一个python程序，利用网络上都有的3d茶壶虚拟手柄的姿态，同时输出按钮信息。手柄通过hid协议上传4元数和按键信息，代码解析这些信息并操作茶壶去动作，

手柄信息如下：

运行图像如下：

附上代码（注释掉的代码可以在坐标转换时候使用）：

#program to parse the data read from the game controller and simulate the pose of the controller using a teapot

#author: shuaiwen

from __future__ import print_function

from OpenGL.GL import *

from OpenGL.GLU import *

from OpenGL.GLUT import *

import struct

import numpy

import hid

import time

import struct

import math

import tkinter as tk

import threading

global hidHandle

global pitch,yaw,roll

global base_rot

global button_info

global window

def toFloat(b0,b1,b2,b3):

    x=[b0,b1,b2,b3]

    return struct.unpack('<f',struct.pack('4B',*x))[0]

 

def q2Euler(w,x,y,z):

        t0 = +2.0 * (w * x + y * z)

        t1 = +1.0 - 2.0 * (x * x + y * y)

        X = math.degrees(math.atan2(t0, t1))

        t2 = +2.0 * (w * y - z * x)

        t2 = +1.0 if t2 > +1.0 else t2

        t2 = -1.0 if t2 < -1.0 else t2

        Y = math.degrees(math.asin(t2))

        t3 = +2.0 * (w * z + x * y)

        t4 = +1.0 - 2.0 * (y * y + z * z)

        Z = math.degrees(math.atan2(t3, t4))

        return X, Y, Z

 

def q_to_mat4(q):

    w, x, y, z = q

    return numpy.array(

        [[1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w, 0],

        [2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w, 0],

        [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y, 0],

        [0, 0, 0, 1] ],'f')

 

def drawFunc():

    glClear(GL_COLOR_BUFFER_BIT)

    global hidHandle

    global button_info

    global pitch,yaw,roll

    d=hidHandle.read(64)

    if d:

        q0=toFloat(d[0],d[1],d[2],d[3])

        q1=toFloat(d[4],d[5],d[6],d[7])

        q2=toFloat(d[8],d[9],d[10],d[11])

        q3=toFloat(d[12],d[13],d[14],d[15])

        key1=d[16]

        key2=d[18]

        key3=d[21]

        leftX=d[22]

        leftY=d[23]

        rightX=d[24]

        rightY=d[25]

        if key1&0x10:

            button_info.config(text='start key pressed')

            #print("start key pressed")

        if key1&0x20:

            button_info.config(text='back key pressed')

            #print("back key pressed")

        if key2==0xff:

            button_info.config(text='key 1 pressed')

            #print("key 1 pressed")

        if key3 ==0xff:

            button_info.config(text='key 2 pressed')

            #print("key 2 pressed")

        #print("lx:%d,ly=%d,rx=%d,ry=%d"%(leftX,leftY, rightX ,rightY))

        #qt=toFloat(0,0,0x90,0x40) #test data,equal to 4.5

        #theta=q2Euler(q0,q1*(-1.0),q2,q3)

        #print(theta)

       # delta0=theta[0]-pitch

       # pitch=theta[0]

       # delta1=theta[1]-yaw

       # yaw=theta[1]

       # delta2=theta[2]-roll

       # roll=theta[2]

       # if abs(delta0)>=1:

       #     glRotatef(delta0, 1, 0, 0)

       # if abs(delta1)>=1:

       #     glRotatef(delta1, 0, 1, 0)

       # if abs(delta2)>=1:

       #     glRotatef(delta2, 0, 0, 1)

        #tot_rot=q_to_mat4([q0,q1*(-1.0),q2,q3])

        #tot_rot=q_to_mat4([q0,q2,q3,q1*(-1.0)])

        #tot_rot=q_to_mat4([q0,q3,q2,q1*(-1.0)])

        tot_rot=q_to_mat4([q0,q1*(-1.0),q3,q2])   #according jinheyao Engineer, multiplier -1 shuold be act on the data.

        glLoadMatrixf(numpy.dot(tot_rot,base_rot))

    glutWireTeapot(0.5)

    glFlush()

 

def kbFunc(key, x, y):

    global window

    if key ==b'q' or key==b'Q':

        window.quit()

        sys.exit()

       

def initPos():

    global pitch,yaw,roll,base_rot

    pitch=9

    yaw=0

    roll=0

    base_rot=numpy.eye(4,dtype=int)

    base_rot[0]=[0,0,1,0]

    base_rot[2]=[-1,0,0,0]

   

def initHid():

    global hidHandle

    try:

        hidHandle=hid.device()

        hidHandle.open(0x2588, 0x2601)

        print("Manufacturer: %s" % hidHandle.get_manufacturer_string())

        print("Product: %s" % hidHandle.get_product_string())

        #print("Serial No: %s" % hidHandle.get_serial_number_string())

        hidHandle.set_nonblocking(1)

    except IOError as ex:

        print(ex)

 

def keyWin():

    global button_info

    global window

    window = tk.Tk()

    window.title('my window')

    window.geometry('300x100')

    button_info=tk.Label(window,bg='yellow',width=20,text='empty')

    button_info.pack()

    window.mainloop()

   

if __name__=="__main__":

    initHid()

    initPos()

    glutInit()

    glutInitDisplayMode(GLUT_SINGLE | GLUT_RGBA)

    glutInitWindowSize(400, 400)

    glutCreateWindow("Controller test")

    glutDisplayFunc(drawFunc)

    glutIdleFunc(drawFunc)

    glutKeyboardFunc(kbFunc)

    t =threading.Thread(target=keyWin,args=())

    t.start()

    glutMainLoop()